(*********************************************************************** Mathematica-Compatible Notebook This notebook can be used on any computer system with Mathematica 3.0, MathReader 3.0, or any compatible application. The data for the notebook starts with the line of stars above. To get the notebook into a Mathematica-compatible application, do one of the following: * Save the data starting with the line of stars above into a file with a name ending in .nb, then open the file inside the application; * Copy the data starting with the line of stars above to the clipboard, then use the Paste menu command inside the application. Data for notebooks contains only printable 7-bit ASCII and can be sent directly in email or through ftp in text mode. Newlines can be CR, LF or CRLF (Unix, Macintosh or MS-DOS style). NOTE: If you modify the data for this notebook not in a Mathematica- compatible application, you must delete the line below containing the word CacheID, otherwise Mathematica-compatible applications may try to use invalid cache data. For more information on notebooks and Mathematica-compatible applications, contact Wolfram Research: web: http://www.wolfram.com email: info@wolfram.com phone: +1-217-398-0700 (U.S.) Notebook reader applications are available free of charge from Wolfram Research. ***********************************************************************) (*CacheID: 232*) (*NotebookFileLineBreakTest NotebookFileLineBreakTest*) (*NotebookOptionsPosition[ 702021, 16605]*) (*NotebookOutlinePosition[ 703074, 16639]*) (* CellTagsIndexPosition[ 703030, 16635]*) (*WindowFrame->Normal*) Notebook[{ Cell[CellGroupData[{ Cell[TextData["TSi Dynamics Version 1.1"], "Title", Evaluatable->False, CellHorizontalScrolling->False, TextAlignment->Center, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Introduction"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "TSi Dynamics is a Mathematica package containing functions that support the \ assembly of mathematical models and simulation models for mechanical systems. \ In its current form, TSi Dynamics accommodates tree structures composed of \ combinations of rigid and flexible bodies and simple and compound kinematic \ joints, and which may also have supplemental algebraic and/or differential \ constraints. Thus, systems with closed loops and nonholonomic joints can be \ treated. \n\nTSi Dynamics creates fully nonlinear, explicit models. The model \ building process requires that the user provide defining data for individual \ joints and bodies, as well as the system interconnection structure. The \ joints are defined in terms of their primitive action parameters from which \ all the required kinematic relations are derived. Thus, a user can contrive \ unusual joint configurations and is not restricted to a predefined set of \ standard joints. The equations are formulated in Poincar\:201a's form of \ Lagrange's equations that admits the standard Lagrange equations as a special \ case. However, Poincar\:201a's form allows the exploitation of \ quasi-velocities which can significantly simplify the equations of motion.\n\n\ TSi Dynamics assembles the equations of motion and it contains functions that \ facilitate simulation. Simulation may done within Mathematica or externally \ using some other simulation software. TSi Dynamics includes functions which \ assist in performing simulations using the NDSolve function in Mathematica \ and other functions to construct a C-code subroutine that compiles as a \ \"MEX-file\" for use with MATLAB/SIMULINK."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Getting Started"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "This notebook illustrates the tools provided in TSi Dynamics including a few \ new functions contained in Version 1.1. TSi Dynamics is contained in the \ directory TSiDyn_1. That directory also contains several examples. TSiDyn_1 \ can be placed anywhwere you like, but a search path must be defined for it so \ that it will be loaded properly. For example, if you place it in the \ mathmatica packages directory with full path name \ \"c:\\math\\packages\\tsidyn_1\", then the following command does this."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["$Path=Join[$Path,{\"c:/math/packages/tsidyn_1/\"}]\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {\".\", \"C:\\\\MATH\\\\\", \"C:\\\\MATH\\\\PACKAGES\\\\\", \"C:\\\\MATH\ \\\\PACKAGES\\\\PRELOAD\\\\\", \"c:/math/packa\\ ges/tsidyn_1/\"} \ \>", "\<\ {., C:\\MATH\\, C:\\MATH\\PACKAGES\\, C:\\MATH\\PACKAGES\\PRELOAD\ \\, c:/math/packages/tsidyn_1/}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Open ]], Cell[TextData[ "If you don't want to do this every time you want to use the package, then \ the above command can be placed in the init.m notebook found in the Packages \ directory. That way the path will be set up every time Mathematica is \ initialized. "], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "\nOnce the search path is defined, TSi Dynamics can be loaded with the \ command"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["<Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ *** TSIDynamics successfully loaded *** \ \>", "\<\ *** TSIDynamics successfully loaded ***\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Open ]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["The Basics of TSi Dynamics"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["What's in the Package?"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "To find out what functions are available in TSi Dynamics simply use the \ command:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["?TSiDynamics"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Functions included in the TSiDynamics package are: Leuler, Xeuler. atilda,atil2a, GammaKin,GamCmpnd,HCompnd,Joints, Lrot,Rtran,XXCmpnd,XXeuc, ChainInertia, EndEffector,EndEffectorVelocity, KinematicReplacements, GeneralizedForce, DamperForce, GravPotential, SpringPotential, LeafPotential, FlxDissPot,FlxStrnPot, RgdBdyInrShift,TreeInertia, Cmat,CreateModel,CreateModelSim, PoinCoef,PoincareFunc,PoincareFuncSim, AlgConstrainedSys, DiffConstrainedSys, MakeODEs, CreateMEXFile,FindMostFrequentTerms, GenerateDissipationTerms,SinCosPowerReplaceToo, ReduceFLOPSToo,ReplaceVariableStr \ \>", "\<\ Functions included in the TSiDynamics package are: Leuler, Xeuler. atilda,atil2a, GammaKin,GamCmpnd,HCompnd,Joints, Lrot,Rtran,XXCmpnd,XXeuc, ChainInertia, EndEffector,EndEffectorVelocity, KinematicReplacements, GeneralizedForce, DamperForce, GravPotential, SpringPotential, LeafPotential, FlxDissPot,FlxStrnPot, RgdBdyInrShift,TreeInertia, Cmat,CreateModel,CreateModelSim, PoinCoef,PoincareFunc,PoincareFuncSim, AlgConstrainedSys, DiffConstrainedSys, MakeODEs, CreateMEXFile,FindMostFrequentTerms, GenerateDissipationTerms,SinCosPowerReplaceToo, ReduceFLOPSToo,ReplaceVariableStr \ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Information about any of these functions can be obtained in the usual way \ with ?Function. Most users will find that only a few of these functions are \ necessary for creating models and simulations. The most important are:\n\n \ Joints: generates all kinematic functions for list of joints\n \n \ CreateModelSim: creates the dynamic model parameters for tree structures from \ system data\n \n EndEffector & EndEffectorVelocity: creates the position, \ orientation and velocity of a body frame located at specified nodes as \ functions of configuration coordinates\n \n SpringPotential & \ LeafPotential: the former creates the potential energy function of a spring \ (defined by a local energy function) connected between specified nodes, the \ latter performs a similar function for energy storage elements that act \ between a system node and the space frame\n \n GeneralizedForce & \ DamperForce: the former is a general function for forming the generalized \ force vector from a local force acting at a specified node, and the latter is \ specific for a damper (defined by a local disspation function) connected \ between specified system nodes\n \n AlgConstrainedSys & \ DiffConstrainedSys: these account for algebraic and differential constraints \ added to a tree structure\n \n MakeODEs & CreateMEXFile: the former \ assembles the ordinary differential equations from the model parmeters \ generated by other functions into a form for numerical computation within \ Mathematica, the latter builds a C source function for computation in \ MATLAB/SIMULINK.\n \nAfter describing the basic data structures used by TSi \ Dynamics we will illustrate the use of these and some other functions."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Data Structures"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Tree Structures"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "TSi Dynamics builds models for mechanical systems which have a tree \ topology. Chain structures are a special case. Every system contains a base \ reference frame which is designated body '0'. Otherwise, bodies and joints \ can be numbered arbitrarily.\n\nA tree is composed of a set of defining \ chains. For instance consider a tree composed of the following sequences of \ bodies:\n\n 0,1,2,4 0,1,2,3,5 \ 0,1,2,3,6\n\nAll definingchains of any tree will start with body 0, so we \ need not list it. However, the body sequences alone do not adequately define \ a tree. For instance bodies 5 and 6 both connect to body 3, but they will do \ so through different joints. This information can be provided by defining \ each chain as an ordered list of pairs \[CapitalADoubleDot] each pair \ consisting of a body and its inboard joint: {inboard joint, body}. For \ example:\n\n {{1,1},{2,2},{5,4}} {{1,1},{2,2},{3,3},{4,5}} \ {{1,1},{2,2},{3,3},{6,6}}\n \n indicates that body 5 connects \ to body 3 at joint 4, and body 6 connects to body 3 at joint 6.\n \nA tree \ is defined by the data structure:\n\n Tree = {list of chains}\n chain = \ ordered list of pairs {inboard joint, body}\n = {{first inboard \ joint, first body},...,{last inboard joint, last body}}"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Joints"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Joints constrain the relative motion between two bodies, or more precisely, \ between two reference frames \[CapitalADoubleDot] one fixed in each body on \ either side of the joint. In TSi Dynamics, joints are of two types: simple \ and compound. Simple joints directly define relative motion between the two \ reference frames, whereas compound joints require intermediate reference \ frames to define the overall motion.\n\nSimple joints are characterized by \ the number of degrees of freedom, r, an r-vector of joint quasi-velocities, \ p, and a 6xr joint map matrix, H. Across the joint the relative velocity \ vector (Dw, DV) is Hp. Moreover, H is a constant (independent of the joint \ configuration) and the columns represent the joint action axes in the \ outboard frame (by convention). A compound joint is equivalent to a sequence \ of simple joints. Thus, it is neccessary to define a set of numbers that \ represent the degrees of freedom associated with each intermediate frame and \ a corresponding set of (constant) joint map matrices. When defining a joint \ in TSi Dynamics, it is necessary to also assign names for both the joint \ quasi-velocities and joint configuration variables.\n\nA k-frame compound \ joint with n degrees of freedom is defined by the data structure:\n\n \ {r, H,q,p} \n\nwhere \n \ r = k-vector whose elements define the number of degrees of freedom for each \ simple joint, with n = r1 + \[UAcute]\[UAcute] + rk. \n H = [H1 .. Hk], a \ matrix composed of the k joint map matrices of the simple joints.\n q = \ n-vector of joint coordinate names.\n p = n-vector of joint quasi-velocity \ names. \n\nLet us consider some elementary examples.\n\nA two degree of \ freedom revolute joint as a simple joint (spherical joint):"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "r1={2};H1={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};\n\ q1={a1x,a1z};p1={w1x,w1z};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "A two degree of freedom revolute joint as a compound joint (universal \ joint):"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "r2={1,1};H2={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};\n\ q2={a2x,a2z};p2={w2x,w2z};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "The function Joints computes all of the necessary quantities for modeling a \ list of joints. These include the kinematic matrix V(q), the Euclidean \ configuration matrix X(q), and the overall joint map matrix H(q). These are \ reported as lists."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2}};\n\n{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics Computing joint 2 kinematics \ \>", "\<\ Computing joint 1 kinematics Computing joint 2 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[V]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {{{1, 0}, {0, Cos[a1x]}}, {{1, 0}, {0, 1}}} \ \>", "\<\ {{{1, 0}, {0, Cos[a1x]}}, {{1, 0}, {0, 1}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[X]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{Cos[a1z], -(Cos[a1x] Sin[a1z]), Sin[a1x] Sin[a1z], 0}, {Sin[a1z], Cos[a1x] Cos[a1z], -(Cos[a1z] Sin[a1x]), 0}, {0, Sin[a1x], \ Cos[a1x], 0}, {0, 0, 0, 1}}, {{Cos[a2z], -(Cos[a2x] Sin[a2z]), Sin[a2x] Sin[a2z], 0}, {Sin[a2z], Cos[a2x] Cos[a2z], -(Cos[a2z] Sin[a2x]), 0}, {0, Sin[a2x], \ Cos[a2x], 0}, {0, 0, 0, 1}}} \ \>", "\<\ {{{Cos[a1z], -(Cos[a1x] Sin[a1z]), Sin[a1x] Sin[a1z], 0}, {Sin[a1z], Cos[a1x] Cos[a1z], -(Cos[a1z] Sin[a1x]), 0}, {0, Sin[a1x], \ Cos[a1x], 0}, {0, 0, 0, 1}}, {{Cos[a2z], -(Cos[a2x] Sin[a2z]), Sin[a2x] Sin[a2z], 0}, {Sin[a2z], Cos[a2x] Cos[a2z], -(Cos[a2z] Sin[a2x]), 0}, {0, Sin[a2x], \ Cos[a2x], 0}, {0, 0, 0, 1}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[H]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {{{1, 0}, {0, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}}, {{1, 0}, {0, Sin[a2x]}, {0, Cos[a2x]}, {0, 0}, {0, 0}, {0, 0}}} \ \>", "\<\ {{{1, 0}, {0, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}}, {{1, 0}, {0, Sin[a2x]}, {0, Cos[a2x]}, {0, 0}, {0, 0}, {0, 0}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Notice that joints 1 and 2 produce the same Euclidean configuration matrix. \ Thus, the configuration coordinates have the same physical meaning. Other \ observations are: (i) the overall joint map matrix for a compound joint \ depends on the joint configuration parameters, and (ii) the kinematic matrix \ is simpler for a compound joint representation than for a simple joint \ representation. A free or unconstrainedbody would be modeled using a six \ degree of freedom joint, such as"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "r3={6}; H3=IdentityMatrix[6];\nq3={ax,ay,az,x,y,z}; p3={wx,wy,wz,ux,uy,uz};\n\ \nJointLst={{r3,H3,q3,p3}};\n\n{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics \ \>", "\<\ Computing joint 1 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[V]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{1, Sin[ax] Tan[ay], Cos[ax] Tan[ay], 0, 0, 0}, {0, Cos[ax], -Sin[ax], \ 0, 0, 0}, {0, Sec[ay] Sin[ax], Cos[ax] Sec[ay], 0, 0, 0}, {0, 0, 0, Cos[ay] Cos[az], Cos[az] Sin[ax] Sin[ay] - Cos[ax] Sin[az], Cos[ax] Cos[az] Sin[ay] + Sin[ax] Sin[az]}, {0, 0, 0, Cos[ay] Sin[az], Cos[ax] Cos[az] + Sin[ax] Sin[ay] Sin[az], -(Cos[az] Sin[ax]) + Cos[ax] Sin[ay] Sin[az]}, {0, 0, 0, -Sin[ay], Cos[ay] Sin[ax], Cos[ax] Cos[ay]}}} \ \>", "\<\ {{{1, Sin[ax] Tan[ay], Cos[ax] Tan[ay], 0, 0, 0}, {0, Cos[ax], \ -Sin[ax], 0, 0, 0}, {0, Sec[ay] Sin[ax], Cos[ax] Sec[ay], 0, 0, 0}, {0, 0, 0, Cos[ay] Cos[az], Cos[az] Sin[ax] Sin[ay] - Cos[ax] Sin[az], Cos[ax] Cos[az] Sin[ay] + Sin[ax] Sin[az]}, {0, 0, 0, Cos[ay] Sin[az], Cos[ax] Cos[az] + Sin[ax] Sin[ay] Sin[az], -(Cos[az] Sin[ax]) + Cos[ax] Sin[ay] Sin[az]}, {0, 0, 0, -Sin[ay], Cos[ay] Sin[ax], Cos[ax] Cos[ay]}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[X]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{Cos[ay] Cos[az], Cos[az] Sin[ax] Sin[ay] - Cos[ax] Sin[az], Cos[ax] Cos[az] Sin[ay] + Sin[ax] Sin[az], x}, {Cos[ay] Sin[az], Cos[ax] Cos[az] + Sin[ax] Sin[ay] Sin[az], -(Cos[az] Sin[ax]) + Cos[ax] Sin[ay] Sin[az], y}, {-Sin[ay], Cos[ay] Sin[ax], Cos[ax] Cos[ay], z}, {0, 0, 0, 1}}} \ \>", "\<\ {{{Cos[ay] Cos[az], Cos[az] Sin[ax] Sin[ay] - Cos[ax] Sin[az], Cos[ax] Cos[az] Sin[ay] + Sin[ax] Sin[az], x}, {Cos[ay] Sin[az], Cos[ax] Cos[az] + Sin[ax] Sin[ay] Sin[az], -(Cos[az] Sin[ax]) + Cos[ax] Sin[ay] Sin[az], y}, {-Sin[ay], Cos[ay] Sin[ax], Cos[ax] Cos[ay], z}, {0, 0, 0, 1}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[H]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, \ 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}} \ \>", "\<\ {{{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, \ 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}}\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Rigid Bodies"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "A rigid body is defined by its mass, inertia matrix and the location of \ distinguished points or nodes where joints or sensors may be located. We \ assume the following:\n\n1) There is a distinguished point that corresponds \ to the inboard joint of the body. The body frame has its origin located \ there. \n2) The center of mass and all other points of interest (nodes) \ including outboard joint locations are defined in the body frame.\n3) The \ inertia matrix is defined in the body frame and it is the inertia matrix \ about the center of mass.\n\nThe data for a rigid body is organized in a list \ as follows> A rigid body with k outboard nodes is defined by the data \ structure:\n \n \ {com,{out1,..,outk},m,Inertia}\nwhere \n com is the center of mass \ location, \n outi = {node number, location} for the ith outboard node, \n \ m is the mass, and \n Inertia is the inertia tensor (about the center of \ mass). "], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Flexible Bodies"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "TSi Dynamics accommodates flexible bodies which satisfy the following \ assumptions:\n\n \[Bullet] Local deformations are small, so that linear \ stiffness (quadratic strain energy) and dissipation (quadratic dissipation \ function) relationships apply.\n \[Bullet] Body deformations can be \ characterized by a finite set of deformation coordinates, denoted by x. \n \ \[Bullet] Body frame center of mass location and joint locations and \ orientations can be defined as (affine) linear functions of the deformation \ coordinates.\n \nAny flexible body model in which a modal representation \ of flexure is valid satisfies these assumptions. Even with these assumptions, \ large global deformations are possible in which case the body inertia matrix \ as represented in the primary body frame may be a function of the deformation \ coordinates.\n\nThe primary body reference frame is a frame fixed in an \ infinitesimal body element with origin at the inboard joint location. We use \ the following notation for specifying the location of points in the primary \ frame under deformation. Let x be the n-vector of deformation coordinates. \ From the linearity assumption the position y of any point P in the body under \ deformation can be characterized by a matrix C and the relation\n\n \ y = C.{1, x}\n\nwhere C is a 3x(n+1) \ matrix. This is the type of information required to locate the center of \ gravity in the deformed body.\n\nMore information is typically required for \ general nodes at which sensors or joints are located. Here it is necessary to \ determine both the position of the node and the angular orientation of a \ local reference frame fixed in an infinitesimal element at the node. Under \ the small deformation assumption the orientation can be specified by any set \ of local parameters. We use Euler angles (3-2-1 convention). The orientation \ and position of the frame is defined by a 6-vector Y, the first three \ elements being the angles and the last three the position vector all relative \ to and specified in the primary body frame. As above, Y can be characterized \ by a matrix C\n\n Y = \ C.{1,x}\n\nwhere C is a 6x(n+1) matrix.\n\nA flexible body with k outboard \ nodes and n deformation coordinates is defined by the data structure:\n\n \ {Ccom,{out1,..,outk},m,{M(x),B,K},x,v}\n\nwhere Ccom is a \ 3x(n+1) matrix that defines the center of mass location, \n outi = {node \ number, Couti}, where Couti is a 6x(n+1) matrix that defines the orientation \ and location of the ith outboard node\n m is the mass \n M(x) is the \ inertia matrix, B is the dissipation matrix, K is the stiffness matrix\n x \ is an n-vector of deformation coordinate names\n v is an n-vector of \ deformation velocity names"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["A First Example: The Double Pendulum"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Building the Equations of Motion"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Building a model in TSi Dynamics generally proceeds in the following way:\n\n\ Define joint data.\n Define body data.\n Define interconnection \ structure.\n Define potential energy function and generalized forces.\n \ Assemble the model.\n\nThe following is a simple example.\n\nDefine Joint \ Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True, FontFamily->"Times New Roman", FontSize->12, FontWeight->"Plain", FontSlant->"Plain", FontTracking->"Plain", FontColor->GrayLevel[0], Background->GrayLevel[1], FontVariations->{"Underline"->False, "Outline"->False, "Shadow"->False}], Cell[TextData[ "r1={1};H1={{1},{0},{0},{0},{0},{0}};\nq1={a1x};p1={w1x};\n\n\ r2={1};H2={{1},{0},{0},{0},{0},{0}};\nq2={a2x};p2={w2x};\n\n\ JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Define Body Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "com1={0,0,-l1}; mass1=m1; out1={2,{0,0,-l1}};\n\ Inertia1=DiagonalMatrix[{0,0,0}];\n\ncom2={0,0,-l2}; mass2=m2; \ out2={3,{0,0,-l2}};\nInertia2=DiagonalMatrix[{0,0,0}];\n\n\ BodyLst={{com1,{out1},mass1,Inertia1},{com2,{out2},mass2,Inertia2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Define Interconnection Structure"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["TreeLst={{{1,1},{2,2}}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Define Potential Energy\n\nIn this case only gravity contributes to the \ potential energy. The only generalized forces are external torques acting at \ the two joints."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["g=gc; PE=0; Q={T1,T2};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cp,Fp,p,q}=CreateModelSim[JointLst,BodyLst,TreeLst,g,PE,Q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["We can look at some results."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Print[MatrixForm[V]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ 1 1 \ \>", "\<\ 1 1\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Recall that V returns as a list of kinematic matrices \[CapitalADoubleDot] \ one for each joint. Hence in this case we get two 1x1 matrices. This can be \ assembled into a single block diagonal matrix but it is more efficient to \ retain the list form."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Print[MatrixForm[M]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ 2 2 2 l1 m1 + l1 m2 + l2 m2 + 2 l1 l2 m2 Cos[a2x] l2 m2 (l2 + l1 Cos[a2x]) l2 m2 (l2 + l1 Cos[a2x]) 2 l2 m2 \ \>", "\<\ 2 2 2 l1 m1 + l1 m2 + l2 m2 + 2 l1 l2 m2 Cos[a2x] l2 m2 (l2 + l1 Cos[a2x]) l2 m2 (l2 + l1 Cos[a2x]) 2 l2 m2\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Print[MatrixForm[Fp]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ -T1 + gc (l1 m1 Sin[a1x] + l1 m2 Sin[a1x] + l2 m2 Sin[a1x + a2x]) -T2 + gc l2 m2 Sin[a1x + a2x] \ \>", "\<\ -T1 + gc (l1 m1 Sin[a1x] + l1 m2 Sin[a1x] + l2 m2 Sin[a1x + a2x]) -T2 + gc l2 m2 Sin[a1x + a2x]\ \>"], "Print", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "When doing time consuming computations you may want to save results \ periodically and then reload them for later computations. Since, as an \ analysis proceeds, different data sets may be saved under the same file name, \ it is a good idea to delete any older file before the new one is saved."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "DeleteFile[\"dbl_pend.dat\"];\nSave[\"dbl_pend.dat\",p,q,V,X,H,Cp,Fp,M];\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ DeleteFile::nffil: File not found during DeleteFile[dbl_pend.dat]. \ \>", "\<\ DeleteFile::nffil: File not found during \ DeleteFile[dbl_pend.dat].\ \>"], "Message", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData["Of course, the data is reloaded by the standard:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["<Infinity, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Simulation: Numerical Solution"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "We can easily construct a simulation within Mathematica. First assemble the \ system parameter matrices computed above into a system of ordinary \ differential equations using the TSi Dynamics function MakeODEs."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Equations=MakeODEs[p,q,V,M,Cp,Fp,t]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][a1x][t] == w1x[t], Derivative[1][a2x][t] == w2x[t], -T1 + gc*(l1*m1*Sin[a1x[t]] + l1*m2*Sin[a1x[t]] + l2*m2*Sin[a1x[t] + \ a2x[t]]) - l1*l2*m2*Sin[a2x[t]]*w2x[t]*(2*w1x[t] + w2x[t]) + (l1^2*m1 + l1^2*m2 + l2^2*m2 + \ 2*l1*l2*m2*Cos[a2x[t]])*Derivative[1][w1x][t] + l2*m2*(l2 + l1*Cos[a2x[t]])*Derivative[1][w2x][t] == 0, -T2 + gc*l2*m2*Sin[a1x[t] + a2x[t]] - \ (l1*l2*m2*Sin[a2x[t]]*w1x[t]*w2x[t])/2 + (l1*l2*m2*Sin[a2x[t]]*w1x[t]*(2*w1x[t] + w2x[t]))/2 + l2*m2*(l2 + l1*Cos[a2x[t]])*Derivative[1][w1x][t] + \ l2^2*m2*Derivative[1][w2x][t] == 0} \ \>", "\<\ {a1x'[t] == w1x[t], a2x'[t] == w2x[t], -T1 + gc (l1 m1 Sin[a1x[t]] + l1 m2 Sin[a1x[t]] + l2 m2 Sin[a1x[t] + \ a2x[t]]) - l1 l2 m2 Sin[a2x[t]] w2x[t] (2 w1x[t] + w2x[t]) + 2 2 2 (l1 m1 + l1 m2 + l2 m2 + 2 l1 l2 m2 Cos[a2x[t]]) w1x'[t] + l2 m2 (l2 + l1 Cos[a2x[t]]) w2x'[t] == 0, l1 l2 m2 Sin[a2x[t]] w1x[t] w2x[t] -T2 + gc l2 m2 Sin[a1x[t] + a2x[t]] - ---------------------------------- + 2 l1 l2 m2 Sin[a2x[t]] w1x[t] (2 w1x[t] + w2x[t]) ----------------------------------------------- + l2 m2 (l2 + l1 \ Cos[a2x[t]]) w1x'[t] + 2 2 l2 m2 w2x'[t] == 0}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Now, replace parameter symbols by numbers and set up initial conditions."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "DataReplacements={m1->1,l1->2,m2->1,l2->1,gc->1,T1->0,T2->0}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {m1 -> 1, l1 -> 2, m2 -> 1, l2 -> 1, gc -> 1, T1 -> 0, T2 -> 0} \ \>", "\<\ {m1 -> 1, l1 -> 2, m2 -> 1, l2 -> 1, gc -> 1, T1 -> 0, T2 -> 0}\ \ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Equations1=Equations/.DataReplacements"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][a1x][t] == w1x[t], Derivative[1][a2x][t] == w2x[t], 4*Sin[a1x[t]] + Sin[a1x[t] + a2x[t]] - 2*Sin[a2x[t]]*w2x[t]*(2*w1x[t] + \ w2x[t]) + (9 + 4*Cos[a2x[t]])*Derivative[1][w1x][t] + (1 + \ 2*Cos[a2x[t]])*Derivative[1][w2x][t] =\\ = 0, Sin[a1x[t] + a2x[t]] - Sin[a2x[t]]*w1x[t]*w2x[t] + Sin[a2x[t]]*w1x[t]*(2*w1x[t] + w2x[t]) + (1 + \ 2*Cos[a2x[t]])*Derivative[1][w1x][t] + Derivative[1][w2x][t] == 0} \ \>", "\<\ {a1x'[t] == w1x[t], a2x'[t] == w2x[t], 4 Sin[a1x[t]] + Sin[a1x[t] + a2x[t]] - 2 Sin[a2x[t]] w2x[t] (2 w1x[t] + \ w2x[t]) + (9 + 4 Cos[a2x[t]]) w1x'[t] + (1 + 2 Cos[a2x[t]]) w2x'[t] == 0, Sin[a1x[t] + a2x[t]] - Sin[a2x[t]] w1x[t] w2x[t] + Sin[a2x[t]] w1x[t] (2 w1x[t] + w2x[t]) + (1 + 2 Cos[a2x[t]]) w1x'[t] + \ w2x'[t] == 0}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["vars=Join[p,q]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {w1x, w2x, a1x, a2x} \ \>", "\<\ {w1x, w2x, a1x, a2x}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "InitialConditions=Table[ToExpression[StringJoin[ToString[vars[[i]]],\"[0]==0\ \"]],{i,Length[vars]}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {w1x[0] == 0, w2x[0] == 0, a1x[0] == 0, a2x[0] == 0} \ \>", "\<\ {w1x[0] == 0, w2x[0] == 0, a1x[0] == 0, a2x[0] == 0}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "InitialConditions={w1x[0] == 0, w2x[0] == 0, a1x[0] == .1, a2x[0] == .2}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {w1x[0] == 0, w2x[0] == 0, a1x[0] == 0.1, a2x[0] == 0.2} \ \>", "\<\ {w1x[0] == 0, w2x[0] == 0, a1x[0] == 0.1, a2x[0] == 0.2}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Equations2=Join[Equations1,InitialConditions]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][a1x][t] == w1x[t], Derivative[1][a2x][t] == w2x[t], 4*Sin[a1x[t]] + Sin[a1x[t] + a2x[t]] - 2*Sin[a2x[t]]*w2x[t]*(2*w1x[t] + \ w2x[t]) + (9 + 4*Cos[a2x[t]])*Derivative[1][w1x][t] + (1 + \ 2*Cos[a2x[t]])*Derivative[1][w2x][t] =\\ = 0, Sin[a1x[t] + a2x[t]] - Sin[a2x[t]]*w1x[t]*w2x[t] + Sin[a2x[t]]*w1x[t]*(2*w1x[t] + w2x[t]) + (1 + \ 2*Cos[a2x[t]])*Derivative[1][w1x][t] + Derivative[1][w2x][t] == 0, w1x[0] == 0, w2x[0] == 0, a1x[0] == 0.1, \ a2x[0] == 0.2} \ \>", "\<\ {a1x'[t] == w1x[t], a2x'[t] == w2x[t], 4 Sin[a1x[t]] + Sin[a1x[t] + a2x[t]] - 2 Sin[a2x[t]] w2x[t] (2 w1x[t] + \ w2x[t]) + (9 + 4 Cos[a2x[t]]) w1x'[t] + (1 + 2 Cos[a2x[t]]) w2x'[t] == 0, Sin[a1x[t] + a2x[t]] - Sin[a2x[t]] w1x[t] w2x[t] + Sin[a2x[t]] w1x[t] (2 w1x[t] + w2x[t]) + (1 + 2 Cos[a2x[t]]) w1x'[t] + \ w2x'[t] == 0, w1x[0] == 0, w2x[0] == 0, a1x[0] == 0.1, a2x[0] == 0.2}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Finally, integrate the equations using the Mathematica function NDSolve and \ plot the results."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["sols=NDSolve[Equations2,vars,{t,0,45}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{w1x -> InterpolatingFunction[\"<<>>\"], w2x -> \ InterpolatingFunction[\"<<>>\"], a1x -> InterpolatingFunction[\"<<>>\"], a2x -> \ InterpolatingFunction[\"<<>>\"]}} \ \>", "\<\ {{w1x -> InterpolatingFunction[{0., 45.}, <>], w2x -> \ InterpolatingFunction[{0., 45.}, <>], a1x -> InterpolatingFunction[{0., 45.}, <>], a2x -> \ InterpolatingFunction[{0., 45.}, <>]} }\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Plot[Evaluate[a1x[t]/.sols],{t,0,45}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[GraphicsData["PostScript", "\<\ %! %%Creator: Mathematica %%AspectRatio: .61803 MathPictureStart %% Graphics /Courier findfont 10 scalefont setfont % Scaling calculations 0.0238095 0.021164 0.308764 1.66883 [ [(10)] .23545 .30876 0 2 Msboxa [(20)] .44709 .30876 0 2 Msboxa [(30)] .65873 .30876 0 2 Msboxa [(40)] .87037 .30876 0 2 Msboxa [(-0.15)] .01131 .05844 1 0 Msboxa [(-0.1)] .01131 .14188 1 0 Msboxa [(-0.05)] .01131 .22532 1 0 Msboxa [(0.05)] .01131 .39221 1 0 Msboxa [(0.1)] .01131 .47565 1 0 Msboxa [(0.15)] .01131 .55909 1 0 Msboxa [ -0.001 -0.001 0 0 ] [ 1.001 .61903 0 0 ] ] MathScale % Start of Graphics 1 setlinecap 1 setlinejoin newpath [ ] 0 setdash 0 g p p .002 w .23545 .30876 m .23545 .31501 L s P [(10)] .23545 .30876 0 2 Mshowa p .002 w .44709 .30876 m .44709 .31501 L s P [(20)] .44709 .30876 0 2 Mshowa p .002 w .65873 .30876 m .65873 .31501 L s P [(30)] .65873 .30876 0 2 Mshowa p .002 w .87037 .30876 m .87037 .31501 L s P [(40)] .87037 .30876 0 2 Mshowa p .001 w .06614 .30876 m .06614 .31251 L s P p .001 w .10847 .30876 m .10847 .31251 L s P p .001 w .15079 .30876 m .15079 .31251 L s P p .001 w .19312 .30876 m .19312 .31251 L s P p .001 w .27778 .30876 m .27778 .31251 L s P p .001 w .32011 .30876 m .32011 .31251 L s P p .001 w .36243 .30876 m .36243 .31251 L s P p .001 w .40476 .30876 m .40476 .31251 L s P p .001 w .48942 .30876 m .48942 .31251 L s P p .001 w .53175 .30876 m .53175 .31251 L s P p .001 w .57407 .30876 m .57407 .31251 L s P p .001 w .6164 .30876 m .6164 .31251 L s P p .001 w .70106 .30876 m .70106 .31251 L s P p .001 w .74339 .30876 m .74339 .31251 L s P p .001 w .78571 .30876 m .78571 .31251 L s P p .001 w .82804 .30876 m .82804 .31251 L s P p .001 w .9127 .30876 m .9127 .31251 L s P p .001 w .95503 .30876 m .95503 .31251 L s P p .001 w .99735 .30876 m .99735 .31251 L s P p .002 w 0 .30876 m 1 .30876 L s P p .002 w .02381 .05844 m .03006 .05844 L s P [(-0.15)] .01131 .05844 1 0 Mshowa p .002 w .02381 .14188 m .03006 .14188 L s P [(-0.1)] .01131 .14188 1 0 Mshowa p .002 w .02381 .22532 m .03006 .22532 L s P [(-0.05)] .01131 .22532 1 0 Mshowa p .002 w .02381 .39221 m .03006 .39221 L s P [(0.05)] .01131 .39221 1 0 Mshowa p .002 w .02381 .47565 m .03006 .47565 L s P [(0.1)] .01131 .47565 1 0 Mshowa p .002 w .02381 .55909 m .03006 .55909 L s P [(0.15)] .01131 .55909 1 0 Mshowa p .001 w .02381 .07513 m .02756 .07513 L s P p .001 w .02381 .09182 m .02756 .09182 L s P p .001 w .02381 .1085 m .02756 .1085 L s P p .001 w .02381 .12519 m .02756 .12519 L s P p .001 w .02381 .15857 m .02756 .15857 L s P p .001 w .02381 .17526 m .02756 .17526 L s P p .001 w .02381 .19195 m .02756 .19195 L s P p .001 w .02381 .20863 m .02756 .20863 L s P p .001 w .02381 .24201 m .02756 .24201 L s P p .001 w .02381 .2587 m .02756 .2587 L s P p .001 w .02381 .27539 m .02756 .27539 L s P p .001 w .02381 .29208 m .02756 .29208 L s P p .001 w .02381 .32545 m .02756 .32545 L s P p .001 w .02381 .34214 m .02756 .34214 L s P p .001 w .02381 .35883 m .02756 .35883 L s P p .001 w .02381 .37552 m .02756 .37552 L s P p .001 w .02381 .40889 m .02756 .40889 L s P p .001 w .02381 .42558 m .02756 .42558 L s P p .001 w .02381 .44227 m .02756 .44227 L s P p .001 w .02381 .45896 m .02756 .45896 L s P p .001 w .02381 .49234 m .02756 .49234 L s P p .001 w .02381 .50902 m .02756 .50902 L s P p .001 w .02381 .52571 m .02756 .52571 L s P p .001 w .02381 .5424 m .02756 .5424 L s P p .001 w .02381 .04175 m .02756 .04175 L s P p .001 w .02381 .02506 m .02756 .02506 L s P p .001 w .02381 .00837 m .02756 .00837 L s P p .001 w .02381 .57578 m .02756 .57578 L s P p .001 w .02381 .59247 m .02756 .59247 L s P p .001 w .02381 .60915 m .02756 .60915 L s P p .002 w .02381 0 m .02381 .61803 L s P P 0 0 m 1 0 L 1 .61803 L 0 .61803 L closepath clip newpath p p p .004 w .02381 .47565 m .02505 .47577 L .02629 .47614 L .02753 .47675 L .02877 .4776 L .03125 .47993 L .03373 .48301 L .03869 .49057 L .04117 .49447 L .04365 .49797 L .04489 .49946 L .04613 .50069 L .04737 .50162 L .04861 .5022 L .04985 .50237 L .05109 .5021 L .05233 .50133 L .05357 .50002 L .05481 .49815 L .05605 .49566 L .05853 .48878 L .06101 .4792 L .06349 .46685 L .06845 .4339 L .07341 .39076 L .08333 .28309 L .09325 .1715 L .09821 .12455 L .10069 .10478 L .10317 .08786 L .10565 .074 L .10689 .06826 L .10813 .06332 L .10938 .05919 L .11062 .05587 L .11186 .05334 L .1131 .0516 L .11434 .05064 L .11558 .05043 L .11682 .05094 L .11806 .05215 L .1193 .05401 L .12054 .0565 L .12302 .06315 L .12798 .08177 L .13294 .10453 L .1379 .12812 L .14286 .14985 L .14782 .16794 L .1503 .17531 L Mistroke .15278 .18149 L .15526 .18653 L .15774 .19052 L .16022 .1936 L .1627 .19601 L .16766 .20003 L .17014 .20238 L .17262 .20548 L .1751 .20974 L .17758 .2155 L .18006 .22305 L .18254 .23262 L .1875 .25833 L .19246 .29285 L .20238 .38355 L .2123 .48492 L .21726 .53 L .21974 .54941 L .22222 .56618 L .2247 .58 L .22594 .58572 L .22718 .59062 L .22842 .59467 L .22966 .59787 L .2309 .6002 L .23214 .60166 L .23338 .60225 L .23462 .60198 L .23586 .60085 L .2371 .59889 L .23834 .59611 L .23958 .59253 L .24206 .58313 L .24702 .55643 L .25198 .52194 L .2619 .44459 L .26687 .40855 L .27183 .3775 L .27679 .35269 L .27927 .34273 L .28175 .33434 L .28671 .32159 L .29167 .3124 L .29663 .30385 L .29911 .29884 L .30159 .29286 L .30655 .27694 L .31151 .25471 L .31647 .22595 L .32143 .19147 L Mistroke .33135 .11353 L .33631 .07632 L .33879 .0598 L .34127 .0453 L .34375 .03324 L .34499 .02823 L .34623 .02397 L .34747 .02047 L .34871 .01778 L .34995 .01591 L .35119 .01488 L .35243 .01472 L .35367 .01541 L .35491 .01698 L .35615 .01942 L .35739 .02273 L .35863 .02689 L .36111 .03771 L .36359 .05171 L .36607 .06863 L .37103 .10986 L .38095 .20891 L .39087 .30369 L .39583 .34169 L .39831 .35755 L .40079 .37121 L .40327 .38266 L .40575 .39198 L .40823 .39933 L .41071 .40493 L .41319 .40909 L .41567 .41217 L .42063 .41668 L .42312 .41889 L .4256 .42155 L .42808 .42492 L .43056 .42924 L .43552 .44122 L .44048 .45789 L .4504 .50234 L .45536 .52638 L .46032 .54776 L .4628 .55635 L .46404 .55993 L .46528 .56296 L .46652 .56541 L .46776 .56723 L .469 .56837 L .47024 .5688 L .47148 .56851 L Mistroke .47272 .56745 L .47396 .56561 L .4752 .56297 L .47644 .55954 L .47768 .55529 L .48016 .54437 L .48264 .53027 L .48512 .51312 L .49008 .47059 L .5 .36295 L .50992 .25178 L .51488 .20473 L .51984 .16706 L .52232 .15222 L .5248 .14014 L .52728 .1308 L .52852 .12714 L .52976 .12412 L .531 .12173 L .53224 .11993 L .53348 .11868 L .53472 .11796 L .53596 .11772 L .5372 .11792 L .53844 .1185 L .53968 .11941 L .54216 .12204 L .54464 .1254 L .5496 .13274 L .55208 .13605 L .55456 .13877 L .5558 .13986 L .55704 .14073 L .55828 .14137 L .55952 .14178 L .56076 .14195 L .562 .14187 L .56324 .14154 L .56448 .14096 L .56572 .14015 L .56696 .13911 L .56944 .13639 L .5744 .12902 L .57937 .12073 L .58185 .11708 L .58309 .11555 L .58433 .11429 L .58557 .11334 L .58681 .11277 L .58805 .11262 L Mistroke .58929 .11293 L .59053 .11375 L .59177 .11513 L .59301 .11708 L .59425 .11966 L .59673 .12676 L .59921 .13658 L .60169 .14919 L .60417 .16459 L .60913 .20337 L .61905 .30604 L .62897 .41988 L .63393 .47061 L .63889 .51233 L .64137 .52901 L .64385 .54263 L .64509 .54825 L .64633 .55307 L .64757 .55708 L .64881 .5603 L .65005 .56272 L .65129 .56435 L .65253 .56523 L .65377 .56536 L .65501 .56477 L .65625 .5635 L .65749 .56158 L .65873 .55906 L .66121 .55238 L .66369 .54385 L .66865 .52298 L .67857 .47765 L .68353 .45834 L .68601 .45023 L .68849 .44326 L .69097 .43746 L .69345 .43278 L .69593 .42913 L .69841 .42633 L .70337 .42231 L .70585 .42042 L .70833 .41811 L .71081 .41497 L .71329 .41062 L .71577 .4047 L .71825 .39693 L .72321 .37508 L .72817 .34429 L .7381 .25875 L .74802 .15669 L Mistroke .75298 .10853 L .75794 .06766 L .76042 .05105 L .7629 .03741 L .76414 .03179 L .76538 .027 L .76662 .02305 L .76786 .01996 L .7691 .01774 L .77034 .01639 L .77158 .0159 L .77282 .01628 L .77406 .0175 L .7753 .01956 L .77654 .02242 L .77778 .02608 L .78026 .03561 L .78274 .04785 L .7877 .07888 L .79762 .15429 L .80258 .19143 L .80754 .22447 L .8125 .25176 L .81746 .27263 L .81994 .28073 L .82242 .28745 L .82738 .29778 L .83234 .30619 L .8373 .31583 L .83978 .32201 L .84226 .3295 L .84722 .34911 L .85218 .37536 L .85714 .40787 L .86706 .48514 L .87202 .52432 L .87698 .55899 L .87946 .57344 L .88194 .58541 L .88318 .59035 L .88442 .59454 L .88566 .59795 L .8869 .60056 L .88814 .60233 L .88938 .60325 L .89063 .60332 L .89187 .60251 L .89311 .60084 L .89435 .59829 L .89559 .59488 L Mistroke .89683 .59061 L .89931 .57959 L .90179 .56542 L .90675 .52873 L .91667 .43364 L .92659 .33602 L .93155 .29488 L .93651 .26167 L .93899 .24832 L .94147 .23715 L .94395 .22808 L .94643 .22093 L .94891 .21548 L .95139 .2114 L .95387 .20834 L .95635 .20588 L .96131 .20124 L .96379 .19834 L .96627 .19467 L .96875 .19001 L .97123 .18424 L .97619 .16912 L Mfstroke P P P % End of Graphics MathPictureEnd\ \>"], "Graphics", PageWidth->Infinity, Evaluatable->False, ImageSize->{300, 185}, ImageMargins->{{100, Inherited}, {Inherited, 0}}, ImageCache->GraphicsData["Bitmap", "\<\ CF5dJ6E]HGAYHf4PAg9QL6QYHgIIIVH000<3 0d92@X61PL30`000EED0EJX0EOl0EB8R8P00^d@0000cV@3<IVFH0<`L71dI6AXF5QLC4a000 ZUD0ZZX0Z_l0ZTA4A000gEH/1@0cc03Ic6H0IP/;2dY:BXV9RLS8b000oeD0ojX0ool0oeEE E@00kWL0000co`3IofH0V@l?3di>CXf=SLch0001Vc03oIS=V03?I03?oc6IVIS/k>gYjN[Vi^OShn03ooeGooj[oo`000?l0;@000?l0;@000?l0;@000?l0;@000?l0 ;@000?l0;@000?l0;@000?l0;@000?l0;@00024000?o0000o`0900008@000ol0003o00T0000Q00;o o`0:00008@000ol0003o00T0000Q0003o`0005D00om_00?o?P00024000?o0000E@000ol0o`1_0003 o`3o03h0000Q00;oEP001?l00?m^0003o`3o03h0000Q0003o`0005@000Go0000o`1]0005o`000?l0 ?@00024000?o0000E0001Ol0003o06d000Go0000o`0m00008@000ol0001D0003o`00008000?o0000 JP000ol000020003o`0003X0000Q0003o`0005@000?o00000P000ol0001Z0003o`00008000?o0000 >P0002400_mD0003o`0000<000?o0000JP000ol000020003o`0003X0000Q0003o`0005<000?o0000 0`000ol0001Y0003o`0000<000?o0000>P0000P00_l400;o0`03o`<00ol50003o`0001H000?o0000 >P000ol000030003o`0006T000?o00000`000ol0000j00001`001?l00?l300;o10000ol000050003 o`00008000?o00005@000ol0o`0k0003o`0000<000?o0000J@000ol000030003o`0003X000000`3o o`03o`050?l00?l02@000ol000050003o`0000800olD0004o`00oc/000?o00000`000ol0001Y0003 o`0000<000?o0000>P0000L000Co003o2@000ol0000200?o1@000ol0000D0004o`00oc/000?o0000 0`000ol0001X0003o`0000D000?o0000>@0000L000Co003o2@000ol000020003o`0000D000?o0000 50001Ol0003o03X000?o000010000ol0001W0003o`0000D000?o0000>@0000P00_l900;o1004o`@0 00?o00004`000ol000020003o`0003P000?o000010000ol0001W0003o`0000D000?o0000>@000240 00?o00004`000ol000020003o`0003L000?o00001@000ol0001W0003o`0000D000?o0000>@000240 0_lD0003o`0000<000?o0000=P000ol000050003o`0006L000?o00001P000ol0000h00008@000ol0 000C0003o`0000<000?o0000=P000ol000050003o`0006L000?o00001P000ol0000h00008@000ol0 000B0003o`0000@000?o0000=P000ol000050003o`0006L000?o00001P000ol0000h00008@000ol0 000B0003o`0000@000?o0000=@000ol000060003o`0006L000?o00001P000ol0000h00008@02oa<0 00?o00001@000ol0000d0003o`0000L000?o0000I@000ol000070003o`0003P0000Q0003o`000180 00?o00001@000ol0000d0003o`0000L000?o0000I@000ol000070003o`0003P0000Q0003o`000180 00?o00001@000ol0000d0003o`0000L000?o0000I@000ol000070003o`0003P0000Q0003o`000180 00?o00001P000ol0000c0003o`0000L000?o0000I@000ol000080003o`0003L0000Q0003o`000180 00?o00001P000ol0000b0003o`0000P000?o0000I@000ol000080003o`0003L0000Q00;o4`000ol0 00060003o`00038000?o000020000ol0001U0003o`0000P000?o0000=`00024000?o00004P000ol0 00060003o`00038000?o000020000ol0000h00;o:`000ol000080003o`0003L0000Q0003o`000140 00?o00001`000ol0000b0003o`0000P000?o0000=`001?l0oolZ0003o`0000P000?o0000=`000240 00?o00004@000ol000080003o`00034000?o000020000ol0000Y00?o2P001Ol0003o02X000?o0000 20000ol0000g00008@000ol0000A0003o`0000P000?o0000<@000ol000080003o`0002P00_l20003 o`0000L000Go0000o`0Z0003o`0000P000?o0000=`0002400_lB0003o`0000P000?o0000<@000ol0 00090003o`0002L000Go0000o`080003o`0000<000?o00009`000ol000080003o`0003L0000Q0003 o`00014000?o000020000ol0000a0003o`0000T000?o00009`000ol000020003o`0000D000?o0000 0`000ol0000W0003o`0000P000?o0000=`0000h00_l400;o0`03o`D000?o00004@000ol000080003 o`00030000?o00002P000ol0000W0003o`0000<000?o00000`000ol000040003o`0002H000?o0000 2@000ol0000g00003@001?l00?l300;o10000ol000040003o`00014000?o000020000ol0000`0003 o`0000X000?o00009P000ol000050005o`000?l01`000ol0000V0003o`0000X000?o0000=P0000L0 1Ol01@3o003o00T000?o00001003oa4000?o00002@000ol0000_0003o`0000X000?o00009P000ol0 000500Co20000ol0000V0003o`0000X000?o0000=P0000d000Co003o2@000ol000040003o`000140 00?o00002@000ol0000_0003o`0000X000?o00009P000ol0000B0003o`0002D000?o00002P000ol0 000f00003@001?l00?l90003o`0000@000?o000040000ol0000:0003o`0002l000?o00002P000ol0 000U0003o`0001<000?o00009@000ol0000:0003o`0003H0000>00;o2@02o`H000?o000040000ol0 000:0003o`0002l000?o00002P000ol0000U0003o`0001<000?o00009@000ol0000:0003o`0003H0 000Q0003o`00010000?o00002`000ol0000^0003o`0000X000?o00009@000ol0000C0003o`0002D0 00?o00002P000ol0000f00008@02oa4000?o00002`000ol0000^0003o`0000/000?o000090000ol0 000D0003o`0002@000?o00002P000ol0000f00008@000ol0000@0003o`0000/000?o0000;P000ol0 000;0003o`0002@000?o000050000ol0000T0003o`0000X000?o0000=P00024000?o000040000ol0 000<0003o`0002d000?o00002`000ol0000T0003o`0001@000?o000090000ol0000;0003o`0002d0 00?o00001@00024000?o000040000ol0000<0003o`0002d000?o00002`000ol0000T0003o`0001@0 00?o000090000ol0000;0003o`0002d000?o00001@0002400_lA0003o`0000`000?o0000;0000ol0 000<0003o`0002@000?o000050000ol0000T0003o`0000/000?o0000;0000ol0000600008@000ol0 000@0003o`0000`000?o0000;0000ol0000<0003o`0002<000?o00005@000ol0000S0003o`0000`0 00?o0000;0000ol0000600008@000ol0000@0003o`0000d000?o0000:`000ol0000<0003o`0002<0 00?o00005@000ol0000S0003o`0000`000?o0000;0000ol0000600008@000ol0000@0003o`0000h0 00?o0000:P000ol0000<0003o`0002<000?o00005P000ol0000R0003o`0000d000?o0000:P000ol0 000700008@000ol0000@0003o`0000h000?o0000:P000ol0000<0003o`0002<000?o00005P000ol0 000R0003o`0000d000?o0000:P000ol0000700008@02oa0000?o000040000ol0000Y0003o`0000d0 00?o00008P000ol0000F0003o`00028000?o00003@000ol0000Y0003o`0000P0000Q0003o`0000l0 00?o00004@000ol0000X0003o`0000d000?o00008@000ol0000G0003o`00028000?o00003@000ol0 000X0003o`0000T0000Q0003o`0000l000?o00004P000ol0000W0003o`0000d000?o00008@000ol0 000G0003o`00028000?o00003@000ol0000W00;o2`00024000?o00003`000ol0000C0003o`0002D0 00?o00003P000ol0000Q0003o`0001L000?o00008P000ol0000=0003o`0002H000?o00002`000240 0_l@0003o`0001<000?o00009@000ol0000>0003o`00024000?o00005`000ol0000R0003o`0000d0 00?o00009P000ol0000;00008@000ol0000?0003o`0001@000?o000090000ol0000>0003o`000240 00?o00005`000ol0000Q0003o`0000l000?o000090000ol0000<00008@000ol0000?0003o`0001D0 00?o00008`000ol0000>0003o`00024000?o00005`000ol0000Q0003o`0000l000?o00008`000ol0 000=00002002o`@00_l400;o0`03o`D000?o00003`000ol0000E0003o`0002<000?o00003P000ol0 000Q0003o`0001L000?o00008@000ol0000?0003o`0002<000?o00003@0000L000Co003o0`02o`<0 00Co003o1@000ol000020003o`0000l000?o00005@000ol0000R0003o`0000l000?o00008@000ol0 000G0003o`00024000?o00003`000ol0000S0003o`0000d000000`3oo`03o`050?l00?l020001?l0 0?l50003o`0000800ol?0003o`0001D000?o00008P000ol0000@0003o`00020000?o000060000ol0 000P0003o`0000l000?o00008P000ol0000>00001`001?l00?l80004o`00o`800ol50003o`0000h0 00?o00005`000ol0000Q0003o`00010000?o00007`000ol0000I0003o`00020000?o00003`000ol0 000R0003o`0000h000070004o`00o`P000Co003o0P000ol000050003o`0000h000?o00005`000ol0 000Q0003o`00010000?o00007`000ol0000I0003o`00020000?o00003`000ol0000Q0003o`0000l0 000800;o2P02o`<01?l40003o`0000h000?o00005`000ol0000Q0003o`00010000?o00007`000ol0 000I0003o`00020000?o00003`000ol0000Q0003o`0000l0000Q00;o3`000ol0000G0003o`000240 00?o000040000ol0000O0003o`0001T000?o000080000ol0000@0003o`00020000?o00003`000240 00?o00003P000ol0000G0003o`00020000?o00004@000ol0000O0003o`0001T000?o00007`000ol0 000A0003o`00020000?o00003`00024000?o00003P000ol0000H0003o`0001l000?o00004@000ol0 000O0003o`0001T000?o00007`000ol0000A0003o`00020000?o00003`00024000?o00003P000ol0 000H0003o`0001l000?o00004@000ol0000O0003o`0001T000?o00007`000ol0000A0003o`000200 00?o00003`00024000?o00003P000ol0000H0003o`0001l000?o00004@000ol0000O0003o`0001T0 00?o00007`000ol0000A0003o`00020000?o00003`0002400_l?0003o`0001P000?o00007`000ol0 000B0003o`0001h000?o00006P000ol0000N0003o`00018000?o00007P000ol0000@00008@000ol0 000>0003o`0001P000?o00001@03o`@00_lA0003o`00018000?o00002004o`<00_l=0003o`0001X0 00?o00000`03o`@00_lB0003o`00018000?o00002003o`<00_l>0003o`000100000Q0003o`0000d0 00?o00006@000ol000060003o`00008000Co003o40000ol0000B0003o`0000T000?o00000P001?l0 0?l<0003o`0001X000?o00001P001?l00?l20003o`0000l000?o00004P000ol000090005o`000?l0 0P000ol0000;0003o`000100000Q0003o`0000d000?o00006P000ol000050003o`00008000Co003o 3`000ol0000C0003o`0000X000Go0000o`020003o`0000X000?o00006P000ol000060004o`00o`80 00?o00003`000ol0000C0003o`0000H01?l20004o`00o`d000?o0000400002400_l>0003o`0001X0 00?o00001@000ol000020004o`00o`l000?o00004`000ol0000;0004o`00o`8000?o00002@000ol0 000K0003o`0000@00_l30004o`00oa4000?o00004`000ol000060003o`3o00<000Co003o3@000ol0 000@00008@000ol0000=0003o`0001X000?o00001@000ol000020004o`00o`l000?o00004`000ol0 00080004o`00o`8000Co003o2`000ol0000K0003o`0000H000Co003o0P000ol0000>0003o`0001@0 00?o00001P000ol0o`030004o`00o`d000?o00004000024000?o00003@000ol0000J0003o`0000@0 0_l500;o40000ol0000C0003o`0000T00_l400;o30000ol0000K0003o`0000<00ol400;o4@000ol0 000D0003o`0000L00_l400;o3@000ol0000A00008@000ol0000=0003o`0001X000?o000070000ol0 000D0003o`0001d000?o00006`000ol0000M0003o`0001@000?o000070000ol0000A00008@000ol0 000=0003o`0001/000?o00006`000ol0000D0003o`0001d000?o00006`000ol0000M0003o`0001D0 00?o00006`000ol0000A00008@02o`h000?o00006`000ol0000J0003o`0001H000?o000070000ol0 000L0003o`0001`000?o00005@000ol0000K0003o`000140000Q0003o`0000d000?o00006`000ol0 000J0003o`0001H000?o000070000ol0000L0003o`0001`000?o00005P000ol0000J0003o`000140 000Q0003o`0000d000?o00006`000ol0000J0003o`0001H000?o000070000ol0000L0003o`0001`0 00?o00005P000ol0000J0003o`000140000Q0003o`0000`000?o000070000ol0000I0003o`0001L0 00?o000070000ol0000L0003o`0001/000?o000060000ol0000I0003o`000140000J0?oo4_l10000 8@000ol000080005o`000?l01`000ol000080003o`0000T000?o000020000ol000090005o`000?l0 1P000ol000090003o`0000D000Co003o2`000ol000080005o`000?l01`000ol000080003o`0000T0 0_l90003o`0000T000?o00001@001?l00?l;0003o`0000P000?o0?l02@000ol000090005o`000?l0 1P000ol00009007o0@00024000?o000030000ol0000L0003o`0000P000?o00003@000ol0000H0003 o`0000`000?o000030000ol0000M0003o`0000L000?o00004@000ol0000J0003o`0000H000?o0000 3@000ol0000B00008@000ol0000<0003o`0001d000?o00005P000ol0000I0003o`0001/000?o0000 7@000ol0000K0003o`0001X000?o00005P000ol0000B00008@000ol0000<0003o`0001d000?o0000 5@000ol0000J0003o`0001/000?o00007@000ol0000K0003o`0001/000?o00005@000ol0000B0000 8@02o`d000?o00007@000ol0000E0003o`0001/000?o00006P000ol0000M0003o`0001/000?o0000 6`000ol0000E0003o`000180000Q0003o`0000`000?o00007@000ol0000D0003o`0001`000?o0000 6P000ol0000M0003o`0001/000?o00006`000ol0000D0003o`0001<0000Q0003o`0000`000?o0000 7@000ol0000D0003o`0001`000?o00006P000ol0000M0003o`0001X000?o000070000ol0000D0003 o`0001<0000Q0003o`0000`000?o00007@000ol0000D0003o`0001`000?o00006P000ol0000M0003 o`0001X000?o00007@000ol0000C0003o`0001<0000Q0003o`0000/000?o00007P000ol0000C0003 o`0001d000?o00006P000ol0000N0003o`0001T000?o00007@000ol0000C0003o`0001<0000Q00;o 30000ol0000N0003o`0001<000?o00007@000ol0000J0003o`0001h000?o00006@000ol0000M0003 o`0001<000?o00004`00024000?o00002`000ol0000N0003o`0001<000?o00007@000ol0000I0003 o`0001l000?o00006@000ol0000N0003o`00018000?o00004`00024000?o00002`000ol0000N0003 o`0001<000?o00007P000ol0000H0003o`0001l000?o00006@000ol0000N0003o`00018000?o0000 4`00024000?o00002`000ol0000N0003o`0001<000?o00007P000ol0000H0003o`0001l000?o0000 6@000ol0000N0003o`00018000?o00004`0002400_l<0003o`0001h000?o00004`000ol0000N0003 o`0001P000?o00007`000ol0000I0003o`0001h000?o00004P000ol0000C00008@000ol0000;0003 o`0001h000?o00004P000ol0000O0003o`0001P000?o00007`000ol0000H0003o`0001l000?o0000 4@000ol0000D00008@000ol0000;0003o`0001l000?o00004@000ol0000O0003o`0001P000?o0000 7`000ol0000H0003o`00020000?o000040000ol0000D00008@000ol0000;0003o`0001l000?o0000 4@000ol0000O0003o`0001P000?o00007`000ol0000H0003o`00020000?o000040000ol0000D0000 8@000ol0000;0003o`0001l000?o000040000ol0000P0003o`0001P000?o00007`000ol0000H0003 o`00020000?o000040000ol0000D00008@02o`/000?o000080000ol0000@0003o`00020000?o0000 60000ol0000O0003o`0001P000?o000080000ol0000@0003o`0001@0000Q0003o`0000X000?o0000 80000ol0000@0003o`00024000?o00005`000ol0000O0003o`0001P000?o000080000ol0000@0003 o`0001@0000800;o1002o`@00_l300?o1@000ol0000:0003o`00020000?o000040000ol0000Q0003 o`0001L000?o00007`000ol0000H0003o`00020000?o000040000ol0000D00001`001?l00?l300;o 0`001?l00?l50003o`00008000?o00002P000ol0000P0003o`00010000?o00008@000ol0000G0003 o`0001l000?o00005`000ol0000Q0003o`00010000?o0000500000L000Co003o20001?l00?l50003 o`0000800ol:0003o`00020000?o000040000ol0000R0003o`0001D000?o000080000ol0000G0003 o`00024000?o000040000ol0000D00001`001?l00?l80004o`00o`800ol50003o`0000X000?o0000 80000ol0000?0003o`0002<000?o00005@000ol0000Q0003o`0001H000?o00008P000ol0000?0003 o`0001@000070004o`00o`P000Co003o0P000ol000050003o`0000X000?o000080000ol0000?0003 o`0002<000?o00005@000ol0000Q0003o`0001H000?o00008P000ol0000?0003o`0001@0000800;o 2P02o`<01?l40003o`0000X000?o00008@000ol0000>0003o`0002<000?o00005@000ol0000Q0003 o`0001D000?o00008`000ol0000?0003o`0001@0000Q0003o`0000X000?o00008@000ol0000>0003 o`0002@000?o000050000ol0000Q0003o`0001D000?o00008`000ol0000?0003o`0001@0000Q00;o 2`000ol0000Q0003o`0000h000?o00009@000ol0000C0003o`00024000?o000050000ol0000T0003 o`0000h000?o00005@00024000?o00002@000ol0000R0003o`0000h000?o00009@02oa@000?o0000 8@000ol0000D0003o`0002@000?o00003P000ol0000E00008@000ol000090003o`00028000?o0000 3P000ol0000W0003o`00014000?o00008@000ol0000C0003o`0002D000?o00003P000ol0000E0000 8@000ol000090003o`00028000?o00003P000ol0000X0003o`00010000?o00008@000ol0000A00;o :@000ol0000=0003o`0001D0000Q00;o2P000ol0000R0003o`0000h000?o0000:@000ol0000?0003 o`00024000?o000040000ol0000Y0003o`0000d000?o00005@00024000?o00002@000ol0000R0003 o`0000d000?o0000:P000ol0000?0003o`00024000?o000040000ol0000Y0003o`0000d000?o0000 5@00024000?o00002@000ol0000R0003o`0000d000?o0000:P000ol0000?0003o`00024000?o0000 3`000ol0000Z0003o`0000d000?o00005@00024000?o00002@000ol0000S0003o`0000`000?o0000 :`000ol0000>0003o`00028000?o00003@000ol0000[0003o`0000d000?o00005@00024000?o0000 2@000ol0000S0003o`0000`000?o0000;0000ol0000=0003o`00028000?o00003@000ol0000[0003 o`0000d000?o00005@0002400_l:0003o`0002<000?o000030000ol0000/0003o`0000d000?o0000 8P000ol0000=0003o`0002/000?o00003@000ol0000E00008@000ol000090003o`0002<000?o0000 30000ol0000/0003o`0000`000?o00008`000ol0000=0003o`0002`000?o000030000ol0000E0000 8@000ol000080003o`0002@000?o000030000ol0000]0003o`0000/000?o00008`000ol0000<0003 o`0002d000?o00002`000ol0000F00008@000ol000080003o`0002@000?o000030000ol0000]0003 o`0000/000?o00008`000ol0000<0003o`0002d000?o00002`000ol0000F00008@02o`T000?o0000 90000ol0000;0003o`0002h000?o00002`000ol0000S0003o`0000/000?o0000;P000ol0000;0003 o`0001H0000Q0003o`0000P000?o000090000ol0000;0003o`0002h000?o00002`000ol0000T0003 o`0000X000?o0000;P000ol0000;0003o`0001H0000Q0003o`0000P000?o000090000ol0000;0003 o`0002h000?o00002`000ol0000T0003o`0000X000?o0000;P000ol0000;0003o`0001H0000>00;o 1002o`<00ol50003o`0000P000?o00009@000ol0000:0003o`0002l000?o00002P000ol0000T0003 o`0000T000?o0000;`000ol0000;0003o`0001H0000=0004o`00o`<00_l40003o`0000@000?o0000 20000ol0000U0003o`0000X000?o0000;`000ol0000:0003o`0002@000?o00002@000ol0000`0003 o`0000X000?o00005P0000d000Co003o2@000ol0000400?o1`000ol0000V0003o`0000X000?o0000 ;`000ol0000:0003o`0002@000?o00002@000ol0000`0003o`0000X000?o00005P0000d000Co003o 2@000ol000040003o`3o00L000?o00009P000ol0000:0003o`0002l000?o00002P000ol0000T0003 o`0000T000?o0000<0000ol0000:0003o`0001H0000=0004o`00o`T000?o000010001?l00?l60003 o`0002H000?o00002@000ol0000a0003o`0000P000?o00009@000ol000090003o`00030000?o0000 2@000ol0000G00003P02o`T00_l60004o`00o`D000?o00009`000ol000090003o`00034000?o0000 20000ol0000U0003o`0000P000?o0000<@000ol000090003o`0001L0000Q00;o0P000ol000020003 o`0002L000?o00002@000ol0000a0003o`0000P000?o00009@000ol000080003o`00034000?o0000 2@000ol0000G00008@000ol000020005o`000?l0:@000ol000090003o`00034000?o000020000ol0 000V0003o`0000L000?o0000<@000ol000090003o`0001L0000Q0003o`0000800_l00`3o000Y0003 o`0000T000?o0000@000ol000050003o`0001T000070004o`00o`<00_l40003 o`0000D000?o00000P000ol0000b0003o`0000D000?o0000=`001Ol0003o02d000Go0000o`0j0003 o`0000@000?o00006@0000L000Co003o2@000ol000050003o`0000800olb0003o`0000D000?o0000 =`001Ol0003o02d000Co003o>`000ol000040003o`0001T000070004o`00o`T000?o00000P03o`D0 00?o0000<`000ol000040003o`0003P000Co003o;P000ol0o`0k0003o`0000@000?o00006@0000L0 00Co003o2@000ol000020003o`0000D000?o0000<`000ol000040003o`0003P000Co0?oo;P03oc/0 00?o000010000ol0000I00002002o`T00_l400Co10000ol0000c0003o`0000<000?o0000>P02ofd0 00?o000010000ol0000I00008@000ol0000c0003o`0000<000?o0000Z@000ol000040003o`0001T0 000Q00;o=@000ol000020003o`000:T000?o00000`000ol0000J00008@000ol0000d0005o`000?l0 [0000ol000030003o`0001X0000Q0003o`0003@000Go0000o`2]0003o`00008000?o00006P000240 00?o0000=0001Ol0003o0:d000Go0000o`0M00008@000ol0000d0005o`000?l0[@001Ol0003o01d0 000Q00;o=P001?l00?n^0004o`00oad0000Q0003o`0003D000?o0?l0[`001?l00?lM00008@000ol0 000e00?o[`03oah0000Q0003o`000?l02@0002400_oo00X0000Q0003o`000?l02@00024000?o0000 o`090000o`0]0000o`0]0000o`0]0000o`0]0000o`0]0000o`0]0000o`0]0000o`0]0000\ \>"], ImageRangeCache->{{{0, 299}, {184, 0}} -> {-5.80608, -0.202677, 0.173842, 0.00220465}}], Cell[OutputFormData["\<\ Graphics[\"<<>>\"] \ \>", "\<\ -Graphics-\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Creating MEX-Files"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "It may be desireable to use the system model in an external program. TSi \ Dynamics provides functions to create C-Code which compiles as a MEX-File for \ use as an S-function in SIMULINK. An S-function may have inputs, outputs so \ that it can be interconnected with other subsystems within SIMULINK and it \ can have parameters which can be defined from within SIMULINK. In the \ following example, we define the joint torques as inputs and the (y,z) \ position of mass 2 as the outputs. Parameters include the system two masses, \ the two lengths and the gravitational constant."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["<Infinity, AspectRatioFixed->True], Cell[TextData[ "We can use the TSi Dynamics function EndEffector to define the functions."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "ChainLst={{1,1},{2,2}};\nTerminalNode=3;\n\ Xout=EndEffector[ChainLst,TerminalNode,BodyLst,X];\n\ yout={Xout[[2,4]],Xout[[3,4]]}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {l1*Sin[a1x] - l2*(-(Cos[a2x]*Sin[a1x]) - Cos[a1x]*Sin[a2x]), -(l1*Cos[a1x]) - l2*(Cos[a1x]*Cos[a2x] - Sin[a1x]*Sin[a2x])} \ \>", "\<\ {l1 Sin[a1x] - l2 (-(Cos[a2x] Sin[a1x]) - Cos[a1x] Sin[a2x]), -(l1 Cos[a1x]) - l2 (Cos[a1x] Cos[a2x] - Sin[a1x] Sin[a2x])}\ \>"], "Output",\ PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "Inputs = Q;\nOutputs = yout;\nMEXFilename = \"dbl_pend.c\";\n\ paramvec={m1,l1,m2,l2,gc};\nPassedParams = \ {\"X0\",\"m1\",\"l1\",\"m2\",\"l2\",\"gc\"};\nPassedParamsDimensions = \ {{2*Length[p],1},{1,1},{1,1},{1,1},{1,1},{1,1}};\n\ DeleteFile[\"dbl_pend.dat\"];\n\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"Inputs\" is similar to existing symbol \"Input\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"Inputs\" is similar to existing symbol \"Input\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Save[\"dbl_pend.dat\",p,q,Inputs,Outputs,PassedParams,\n \ PassedParamsDimensions,V,X,H,Cp,Fp,M,MEXFilename];\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Building More Complex Models"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Introductory Remarks"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "TSi Dynamics provides several routes to developing models. The most direct \ is to use the functions CreateModel or CreateModelSim. These two functions \ work essentially the same way but produce models in slightly different form. \ The latter is almost always preferable for creating simulations. One example \ of its use has already been given above for creating a model of the double \ pendulum. The following more elaborate examples illustrate some other \ functions in TSi Dynamics."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Partial Suspension System"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "We consider a simple split axle suspension in planar motion. The system \ consists of a chassis, two axles, two tires, two springs \[CapitalADoubleDot] \ each connecting nodes on one axle and the chassis, and two torsional dampers. \ The modeling process begins in the standard way with joint and body \ definitions. The coordinate system is chosen with z down and motion takes \ place in the y-z plane (y points to the right).\n\nJoint Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "(* Joint 1 - planar motion *)\nr1={3};\nq1={theta1,y,z};p1={w1,vy,vz};\n\ H1={{1,0,0},{0,0,0},{0,0,0},{0,0,0},{0,1,0},{0,0,1}};\n(* Joint 2 - \ suspension *)\nr2={1};\nq2={theta2};p2={w2};\nH2={{1},{0},{0},{0},{0},{0}};\n\ (* Joint 3 - suspension *)\nr3={1};\nH3={{1},{0},{0},{0},{0},{0}};\n\ q3={theta3};p3={w3};\n\nJointLst={{r1,H1,q1,p1},{r2,H2,q2,p2},{r3,H3,q3,p3}};\ \n{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics \ \>", "\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["Body Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Body 1 - Chassis *)\ncm1={0,0,0};\n\ out1={{2,{0,b/2,a/2}},{3,{0,-b/2,a/2}},{6,{0,b/2+a/2,-a/2}},{7,{0,-b/2-a/2,-a/\ 2}}};\nI1=DiagonalMatrix[{Ixx,Iyy,Izz}];\n\n(* Body 2 - Wheel & Axle *)\n\ cm2={0,d,0};\nout2={{4,{0,d,R}},{8,{0,Sqrt[2]*a,0}}};\n\ I2=DiagonalMatrix[{Wxx,Wyy,Wzz}];\n\n(* Body 3 - Wheel & Axle *)\n\ cm3={0,-d,0};\nout3={{5,{0,-d,R}},{9,{0,-Sqrt[2]*a,0}}};\n\ I3=DiagonalMatrix[{Wxx,Wyy,Wzz}];\n\n\ BodyLst={{cm1,out1,m1,I1},{cm2,out2,mw,I2},{cm3,out3,mw,I3}};\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "System Structure\n\nNotice that this system has a tree structure with two \ chains."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["TreeLst={{{1,1},{2,2}},{{1,1},{3,3}}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Potential Energy Constructions\n\nThe potential energy comes from two \ sources in addition to the gravitational potential: (i) the tires, and (ii) \ the springs. The tires are leaves of the tree and they react with the space \ frame. Computing the potential energy of the tires illustrates the use of the \ function LeafPotential.\n\nIt is assumed that each tire potential energy \ function is defined in terms of the space coordinates of the tire contact \ point (the tire outboard node). In this example, we suppose that only the \ vertical displacements are significant."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Define Tire PE in terms of Space Coordinates *)\n\n\ yy={{x2,y2,z2},{x3,y3,z3}};\nPot=(k/2)*(z2-z20)^2+(k/2)*(z3-z30)^2;"], "Input",\ PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Note that z20 and z30 may be specified as externally defined functions of \ time to simulate riding over rough terrain. LeafPotential replaces the \ absolute coordinates by the generalized coordinates."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Obtain Tire PE in terms of generalized Coordinates *)\n\n\ TirePE=LeafPotential[BodyLst,TreeLst,X,H,Pot,yy];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["TirePE"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])^2)/2 + (k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])^2)/2 \ \>", "\<\ a Cos[theta1] b \ Sin[theta1] (k Power[z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 d Sin[theta1 + theta2], 2]) / 2 + a Cos[theta1] b Sin[theta1] (k Power[z - z30 + ------------- + R Cos[theta1 + theta3] - ------------- - \ 2 2 d Sin[theta1 + theta3], 2]) / 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "The spring potential energy function SpringPotential is a specialized \ function for springs connecting two nodes in the system. It is assumed that \ the potential energy is a function of the length of the spring. Because only \ one variable is involved it is convenient to specify the potential energy as \ a function rather than as an expression in which the variables must be \ separately identified. SpringPotential constructs the potential energy \ function for the spring in terms of the generalized system coordinates."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "SpringPot[x_]:=(kdel/2)*(x-ls)^2\nq=Join[q1,q2,q3];\n\ SpringPE1=SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q];\n\ SpringPE2=SpringPotential[7,9,SpringPot,TreeLst,BodyLst,X,q];\n\n\ PE=TirePE+SpringPE1+SpringPE2;\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["SpringPE1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (kdel*(-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2)^2)/2 \ \>", "\<\ 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] 2 kdel (-ls + ----) 2 --------------------------------------------------------------------------- 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["SpringPE2"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (kdel*(-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta3] - \ 8*2^(1/2)*Sin[theta3]))^(1/2)/2)^2)/2 \ \>", "\<\ 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] 2 kdel (-ls + -------------------------------------------------------------) 2 --------------------------------------------------------------------------- 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Generalized Force\n\nThe only generalized forces acting are due to the \ dampers. We assume that the forces due to the torsional dampers are \ proportional to the relative angular velocities of the axles with respect to \ the chassis. As an alternative one could place a linear damper between two \ nodes and use the function DamperForce to compute the generalized force."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["Q={0,0,0,-c*w2,-c*w3};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Gravity\n\nSince, we have chosen the z-axis to point down, we change the \ sign of the gravitational acceleration"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["g=-gc;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Obtain Equations of Motion\n\nNotice that since the joint kinematic data V, \ X, and H has already been computed, we include this data in the argument list \ of CreateModelSim to avoid recomputing it."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cmat,F,pp,qq}=CreateModelSim[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H];\ "], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["M"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{Ixx + (a^2*mw)/2 + (b^2*mw)/2 + 2*d^2*mw + 2*Wxx + b*d*mw*Cos[theta2] \ + b*d*mw*Cos[theta3] + a*d*mw*Sin[theta2] - a*d*mw*Sin[theta3], -(mw*(a + d*Sin[theta2] - d*Sin[theta3])), d*mw*(Cos[theta2] - \ Cos[theta3]), d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2}, {-(mw*(a + d*Sin[theta2] - d*Sin[theta3])), 2*mw + m1, 0, \ -(d*mw*Sin[theta2]), d*mw*Sin[theta3]}, {d*mw*(Cos[theta2] - Cos[theta3]), 0, 2*mw + m1, \ d*mw*Cos[theta2], -(d*mw*Cos[theta3])}, {d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + \ (a*d*mw*Sin[theta2])/2, -(d*mw*Sin[theta2]), d*mw*Cos[theta2], d^2*mw + Wxx, 0}, {d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2, \ d*mw*Sin[theta3], -(d*mw*Cos[theta3]), 0, d^2*mw + Wxx}} \ \>", "\<\ 2 2 a mw b mw 2 {{Ixx + ----- + ----- + 2 d mw + 2 Wxx + b d mw Cos[theta2] + b d mw \ Cos[theta3] + 2 2 a d mw Sin[theta2] - a d mw Sin[theta3], -(mw (a + d Sin[theta2] - d \ Sin[theta3])), 2 b d mw Cos[theta2] a d \ mw Sin[theta2] d mw (Cos[theta2] - Cos[theta3]), d mw + Wxx + ------------------ + \ ------------------, 2 \ 2 2 b d mw Cos[theta3] a d mw Sin[theta3] d mw + Wxx + ------------------ - ------------------}, 2 2 {-(mw (a + d Sin[theta2] - d Sin[theta3])), 2 mw + m1, 0, -(d mw \ Sin[theta2]), d mw Sin[theta3]}, {d mw (Cos[theta2] - Cos[theta3]), 0, 2 mw + m1, d mw \ Cos[theta2], 2 b d mw Cos[theta2] a d mw \ Sin[theta2] -(d mw Cos[theta3])}, {d mw + Wxx + ------------------ + \ ------------------, 2 2 2 -(d mw Sin[theta2]), d mw Cos[theta2], d mw + Wxx, 0}, 2 b d mw Cos[theta3] a d mw Sin[theta3] {d mw + Wxx + ------------------ - ------------------, d mw Sin[theta3], 2 2 2 -(d mw Cos[theta3]), 0, d mw + Wxx}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Simplify[Cmat.pp]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {(d*mw*w2*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2 - (d*mw*w3*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2 + mw*w1*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - \ d*vz*Sin[theta3])\\ , -((2*mw + m1)*vz*w1) - d*mw*w2*(2*w1 + w2)*Cos[theta2] + d*mw*w3*(2*w1 + w3)*Cos[theta3] + d*mw*w1^2*(-Cos[theta2] + Cos[theta3]), (2*mw + m1)*vy*w1 - d*mw*w2*(2*w1 + w2)*Sin[theta2] + d*mw*w3*(2*w1 + \ w3)*Sin[theta3] - mw*w1^2*(a + d*Sin[theta2] - d*Sin[theta3]), (d*mw*w1*(2*vy*Cos[theta2] - a*w1*Cos[theta2] + 2*vz*Sin[theta2] + \ b*w1*Sin[theta2]))/2, (d*mw*w1*(-2*vy*Cos[theta3] + a*w1*Cos[theta3] - 2*vz*Sin[theta3] + \ b*w1*Sin[theta3]))/2} \ \>", "\<\ d mw w2 (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) {--------------------------------------------------- - 2 d mw w3 (2 w1 + w3) (a Cos[theta3] + b Sin[theta3]) --------------------------------------------------- + 2 mw w1 (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d \ vz Sin[theta3])\\ , -((2 mw + m1) vz w1) - d mw w2 (2 w1 + w2) Cos[theta2] + 2 d mw w3 (2 w1 + w3) Cos[theta3] + d mw w1 (-Cos[theta2] + Cos[theta3]), (2 mw + m1) vy w1 - d mw w2 (2 w1 + w2) Sin[theta2] + d mw w3 (2 w1 + w3) \ Sin[theta3] - 2 mw w1 (a + d Sin[theta2] - d Sin[theta3]), d mw w1 (2 vy Cos[theta2] - a w1 Cos[theta2] + 2 vz Sin[theta2] + b w1 \ Sin[theta2]) ----------------------------------------------------------------------------\ -------, 2 d mw w1 (-2 vy Cos[theta3] + a w1 Cos[theta3] - 2 vz Sin[theta3] + b w1 \ Sin[theta3]) ----------------------------------------------------------------------------\ --------} 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["F"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {gc*mw*(-(d*Cos[theta1 + theta2]) + d*Cos[theta1 + theta3] + \ a*Sin[theta1]) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*((b*Cos[theta1])/2 + d*Cos[theta1 + theta2] - (a*Sin[theta1])/2 - R*Sin[theta1 + theta2]) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(b*Cos[theta1])/2 - d*Cos[theta1 + theta3] - (a*Sin[theta1])/2 - R*Sin[theta1 + theta3]), Sin[theta1]*(-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + \ theta2] - d*k*Sin[theta1 + theta3]), Cos[theta1]* (-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + \ k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + theta2] - d*k*Sin[theta1 + \ theta3]), c*w2 - d*gc*mw*Cos[theta1 + theta2] + (2^(1/2)*a^2*kdel*(2*Cos[theta2] + Sin[theta2])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta2] + 8*2^(1/2)*Sin[theta2]))^(1/2) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*(d*Cos[theta1 + theta2] - R*Sin[theta1 + \ theta2]), c*w3 + d*gc*mw*Cos[theta1 + theta3] - (2^(1/2)*a^2*kdel*(2*Cos[theta3] - Sin[theta3])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta3] - \ 8*2^(1/2)*Sin[theta3]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta3] - 8*2^(1/2)*Sin[theta3]))^(1/2) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(d*Cos[theta1 + theta3]) - R*Sin[theta1 + \ theta3])} \ \>", "\<\ {gc mw (-(d Cos[theta1 + theta2]) + d Cos[theta1 + theta3] + a \ Sin[theta1]) + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 b Cos[theta1] a \ Sin[theta1] d Sin[theta1 + theta2]) (------------- + d Cos[theta1 + theta2] - \ ------------- - 2 \ 2 a Cos[theta1] R Sin[theta1 + theta2]) + k (z - z30 + ------------- + R Cos[theta1 + \ theta3] - 2 b Sin[theta1] ------------- - d Sin[theta1 + theta3]) 2 -(b Cos[theta1]) a Sin[theta1] (---------------- - d Cos[theta1 + theta3] - ------------- - R Sin[theta1 \ + theta3]), 2 2 Sin[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + k R Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + \ theta2] - d k Sin[theta1 + theta3]), Cos[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + k R \ Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + theta2] - d k Sin[theta1 + \ theta3]), c w2 - d gc mw Cos[theta1 + theta2] + 2 (Sqrt[2] a kdel (2 Cos[theta2] + Sin[theta2]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] (-ls + -------------------------------------------------------------)) \ / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 d Sin[theta1 + theta2]) (d Cos[theta1 + theta2] - R Sin[theta1 + \ theta2]), c w3 + d gc mw Cos[theta1 + theta3] - 2 (Sqrt[2] a kdel (2 Cos[theta3] - Sin[theta3]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] (-ls + -------------------------------------------------------------)) \ / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] + a Cos[theta1] b Sin[theta1] k (z - z30 + ------------- + R Cos[theta1 + theta3] - ------------- - 2 2 d Sin[theta1 + theta3]) (-(d Cos[theta1 + theta3]) - R Sin[theta1 + \ theta3])}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["A Flexible Two Link Manipulator"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Consider a two link, planar mechanism in which the lower end of link 1 is \ attached to the space frame by a revolute joint and the upper end of link 1 \ and the lower end of link 2 are joined by a second revolute joint. Assume \ that the links have the same mass per unit length, rho, modulus of \ elasticity, El, cross sectional area moments (constant along the length) \ {Ixx,Iyy,Izz}, and the lengths L1 and L2, respectively."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Modeling a Flexible Link"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "There are many approaches to modeling individual flexible elements. One view \ fixes a \"primary body reference frame\" in an infinitesimal element at the \ inboard joint. The location of any point in the deformed body are specified \ in this frame. As the body undergoes motion, it is necessary to track the \ location and orientation of the primary body frame as well as the location in \ that frame of any interesting body points. Thus, the number of degrees of \ freedom associated with a free deformable body will be the six degrees of \ freedom associated with the primary frame plus some number of degrees of \ freedom associated with the body deformations. Usually, it is desired to \ characterize body deformations by a finite number of degrees of freedom and \ this is often accomplished by using finite element or (truncated) modal \ expansion methods. \n\nAs an example take one of the links in the two \ link manipulator. We fix a reference frame in the link at its inboard joint. \ Suppose that the beam lies along the z-axis (points upward) with the inboard \ joint at the bottom. Moroever, assume that the link bends only in the y-z \ plane. Within this frame the link deforms as a centelevered beam and we can \ characterize the beam deformations in terms of its modes. A cantelevered \ (Euler) beam of length L has the characteristic equation\n\n\t\t\t\t\t\t\t\t\t\ \t\t\t\t\t\t\t\t\t\t\tCos[b*L] * Cosh[b*L] = -1\n\nwhich has a countable set \ of solutions br, r = 1,2,3...; the first three roots are approximately: "], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["bL={0.600*Pi,1.49*Pi,2.50*Pi};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["They define the natural frequencies by the relation"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["w[i_]:=(bL[[i]])^2*(El*Ixx/(rho*L^4))^(1/2);"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Correspondingly, the mode shapes are:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "W[i_,z_]:=((Cosh[bL[[i]]*z/L]-Cos[bL[[i]]*z/L])/(Cosh[bL[[i]]]+Cos[bL[[i]]])\ \n\n - \ (Sinh[bL[[i]]*z/L]-Sin[bL[[i]]*z/L])/(Sinh[bL[[i]]]+Sin[bL[[i]]])); "], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "From the above information we can construct, for example, a one-mode model \ for a link of length L. First, the inertia, damping and stiffness matrices \ are:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "M={{Ia,0,0,0,-m,0,0},\n {0,Ib,0,m,0,0,a},\n {0,0,Ic,0,0,0,0},\n \ {0,m,0, m,0,0,b},\n {-m,0,0,0,m,0,0},\n {0,0,0,0,0,m,0},\n \ {0,a,0,b,0,0,1}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "B=0*IdentityMatrix[7]; \n\ K=DiagonalMatrix[{0,0,0,0,0,0,(0.600*Pi)^2}]*(El*Ixx/(m*L^3));"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "where Ia, Ib, Ic are respectively Ixx m L^2/A, Iyy m L^2/A, Izz m L^2/A. Let \ y1 denote the modal coordinate, then the y position of the right end and \ anglular rotation about the x-axis (phiend) under deformation are given by:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["yend=W[1,L]*y1; phiend=D[W[1,z],z]/.z->L;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "and the center of mass (which corresponds to the midpoint) is"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["ycom=W[1,L/2]*y1;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Consequently, we have the matrices Cout and Ccom:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Cout={{0,D[W[1,z],z]/.z->L},\n {0,0},{0,0},\n \ {0,0},{0,W[1,L]},{L,0}};\nCcom={{0,0},{0,W[1,L/2]},{L,0}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell: Possible spelling error: new symbol name \"Cout\" is similar to existing symbols {Cot, Count, Xout}. \ \>", "\<\ General::spell: Possible spelling error: new symbol name \"Cout\" is similar to existing symbols {Cot, Count, Xout}.\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["The link body data is then"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Link1={N[Ccom],{{2,N[Cout]}},m,{M,B,K},{y11},{v11}}/.L->L1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {{{0, 0}, {0, 0.223912970343545}, {L1, 0}}, {{2, {{0, 0.904371004615765/L1}, {0, 0}, {0, 0}, {0, 0}, {0, \ 0.6583189111222529}, {L1, 0}}}}, m, {{{Ia, 0, 0, 0, -m, 0, 0}, {0, Ib, 0, m, 0, 0, a}, {0, 0, Ic, 0, 0, 0, 0}, {0, m, 0, m, 0, 0, b}, {-m, 0, 0, 0, m, 0, 0}, {0, 0, 0, 0, 0, m, 0}, {0, a, 0, b, 0, 0, 1}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, (0.36*El*Ixx*Pi^2)/(L1^3*m)}}}, \ {y11}, {v11}} \ \>", "\<\ {{{0, 0}, {0, 0.223913}, {L1, 0}}, {{2, 0.904371 {{0, --------}, {0, 0}, {0, 0}, {0, 0}, {0, 0.658319}, {L1, 0}}}}, m, L1 {{{Ia, 0, 0, 0, -m, 0, 0}, {0, Ib, 0, m, 0, 0, a}, {0, 0, Ic, 0, 0, 0, 0}, {0, m, 0, m, 0, 0, b}, {-m, 0, 0, 0, m, 0, 0}, {0, 0, 0, 0, 0, m, 0}, {0, a, 0, b, 0, 0, 1}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, 2 0.36 El Ixx Pi {0, 0, 0, 0, 0, 0, ---------------}}}, {y11}, {v11}} 3 L1 m\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "Link2={N[Ccom],{{3,N[Cout]}},m,{M,B,K},{y21},{v21}}/.L->L2"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {{{0, 0}, {0, 0.223912970343545}, {L2, 0}}, {{3, {{0, 0.904371004615765/L2}, {0, 0}, {0, 0}, {0, 0}, {0, \ 0.6583189111222529}, {L2, 0}}}}, m, {{{Ia, 0, 0, 0, -m, 0, 0}, {0, Ib, 0, m, 0, 0, a}, {0, 0, Ic, 0, 0, 0, 0}, {0, m, 0, m, 0, 0, b}, {-m, 0, 0, 0, m, 0, 0}, {0, 0, 0, 0, 0, m, 0}, {0, a, 0, b, 0, 0, 1}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, (0.36*El*Ixx*Pi^2)/(L2^3*m)}}}, \ {y21}, {v21}} \ \>", "\<\ {{{0, 0}, {0, 0.223913}, {L2, 0}}, {{3, 0.904371 {{0, --------}, {0, 0}, {0, 0}, {0, 0}, {0, 0.658319}, {L2, 0}}}}, m, L2 {{{Ia, 0, 0, 0, -m, 0, 0}, {0, Ib, 0, m, 0, 0, a}, {0, 0, Ic, 0, 0, 0, 0}, {0, m, 0, m, 0, 0, b}, {-m, 0, 0, 0, m, 0, 0}, {0, 0, 0, 0, 0, m, 0}, {0, a, 0, b, 0, 0, 1}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}}, {{0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0}, 2 0.36 El Ixx Pi {0, 0, 0, 0, 0, 0, ---------------}}}, {y21}, {v21}} 3 L2 m\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["BodyList={Link1,Link2};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"BodyList\" is similar to existing symbol \"BodyLst\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"BodyList\" is similar to existing symbol \"BodyLst\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "r1={1};\nH1=Transpose[{{1,0,0,0,0,0}}];\nq1={theta1};p1={w1};\n\nr2={1};\n\ H2=Transpose[{{1,0,0,0,0,0}}];\nq2={theta2};p2={w2};\n\n\ JointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"JointList\" is similar to existing symbol \"JointLst\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"JointList\" is similar to existing symbol \"JointLst\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["TreeList={{{1,1},{2,2}}};\nPE=0; Q={0,0,0,0};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"TreeList\" is similar to existing symbol \"TreeLst\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"TreeList\" is similar to existing symbol \"TreeLst\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cp,Fp,p,q}=CreateModelSim[JointList,BodyList,TreeList,g,PE,Q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["We can examine some of the results:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["M"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{2*Ia + 2*L1*m + L1^2*m + (0.817886913989728*Ib*y11^2)/L1^2 + \ 1.251270702730916*m*y11^2 + (1.635773827979455*m*y11^2)/L1, Ia + L1*m, (0.904371004615765*Ia)/L1 + 0.904371004615765*m + \ (0.7396732100669774*Ib*y11^2)/L1^3 + (0.7396732100669774*m*y11^2)/L1^2 - 0.6583189111222529*m*Cos[theta2] - 0.6583189111222529*L1*m*Cos[theta2] - \ (0.5384304226388572*m*y11^2*Cos[theta2])/L1^2 - (0.5384304226388572*m*y11^2*Cos[theta2])/L1 - \ 0.4333837887411888*m*y11*Sin[theta2], 0.904371004615765*b*y11 + (0.904371004615765*a*y11)/L1}, {Ia + L1*m, Ia, (0.904371004615765*Ia)/L1 - \ 0.6583189111222529*m*Cos[theta2], 0}, {(0.904371004615765*Ia)/L1 + 0.904371004615765*m + \ (0.7396732100669774*Ib*y11^2)/L1^3 + (0.7396732100669774*m*y11^2)/L1^2 - 0.6583189111222529*m*Cos[theta2] - 0.6583189111222529*L1*m*Cos[theta2] - \ (0.5384304226388572*m*y11^2*Cos[theta2])/L1^2 - (0.5384304226388572*m*y11^2*Cos[theta2])/L1 - \ 0.4333837887411888*m*y11*Sin[theta2], (0.904371004615765*Ia)/L1 - 0.6583189111222529*m*Cos[theta2], 1 + (0.817886913989728*Ia)/L1^2 + 0.4333837887411888*m + (0.6689390040756399*Ib*y11^2)/L1^4 + (0.1772294647733534*m*y11^2)/L1^2 - (1.190729070018376*m*Cos[theta2])/L1 - \ (0.973881724475188*m*y11^2*Cos[theta2])/L1^3 + (0.1772294647733534*m*y11^2*Cos[2*theta2])/L1^2, (0.817886913989728*a*y11)/L1^2 - \ (0.5953645350091882*b*y11*Cos[theta2])/L1}, {0.904371004615765*b*y11 + (0.904371004615765*a*y11)/L1, 0, (0.817886913989728*a*y11)/L1^2 - \ (0.5953645350091882*b*y11*Cos[theta2])/L1, 1}} \ \>", "\<\ 2 \ 2 2 0.817887 Ib y11 2 1.63577 m y11 {{2 Ia + 2 L1 m + L1 m + ---------------- + 1.25127 m y11 + --------------, \ Ia + L1 m, 2 L1 L1 2 2 0.904371 Ia 0.739673 Ib y11 0.739673 m y11 ----------- + 0.904371 m + ---------------- + --------------- - 0.658319 m \ Cos[theta2] - L1 3 2 L1 L1 2 2 0.53843 m y11 Cos[theta2] 0.53843 m y11 \ Cos[theta2] 0.658319 L1 m Cos[theta2] - -------------------------- - \ -------------------------- - 2 L1 L1 0.904371 a y11 0.433384 m y11 Sin[theta2], 0.904371 b y11 + --------------}, L1 0.904371 Ia {Ia + L1 m, Ia, ----------- - 0.658319 m Cos[theta2], 0}, L1 2 2 0.904371 Ia 0.739673 Ib y11 0.739673 m y11 {----------- + 0.904371 m + ---------------- + --------------- - 0.658319 m \ Cos[theta2] - L1 3 2 L1 L1 2 2 0.53843 m y11 Cos[theta2] 0.53843 m y11 \ Cos[theta2] 0.658319 L1 m Cos[theta2] - -------------------------- - \ -------------------------- - 2 L1 L1 0.904371 Ia 0.433384 m y11 Sin[theta2], ----------- - 0.658319 m Cos[theta2], L1 2 2 0.817887 Ia 0.668939 Ib y11 0.177229 m y11 1 + ----------- + 0.433384 m + ---------------- + --------------- - 2 4 2 L1 L1 L1 2 2 1.19073 m Cos[theta2] 0.973882 m y11 Cos[theta2] 0.177229 m y11 \ Cos[2 theta2] --------------------- - --------------------------- + \ -----------------------------, L1 3 2 L1 L1 0.817887 a y11 0.595365 b y11 Cos[theta2] -------------- - --------------------------}, 2 L1 L1 0.904371 a y11 0.817887 a y11 0.595365 b y11 \ Cos[theta2] {0.904371 b y11 + --------------, 0, -------------- - \ --------------------------, 1}} L1 2 L1 L1\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Simplify[Cp.p]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - \ d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - \ b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/4}}[ {{2*Ia + 2*L1*m + L1^2*m + (0.817886913989728*Ib*y11^2)/L1^2 + 1.251270702730916*m*y11^2 + (1.635773827979455*m*y11^2)/L1, Ia + L1*m, (0.904371004615765*Ia)/L1 + 0.904371004615765*m + \ (0.7396732100669774*Ib*y11^2)/L1^3 + (0.7396732100669774*m*y11^2)/L1^2 - 0.6583189111222529*m*Cos[theta2] - 0.6583189111222529*L1*m*Cos[theta2] - \ (0.5384304226388572*m*y11^2*Cos[theta2])/L1^2 - (0.5384304226388572*m*y11^2*Cos[theta2])/L1 - \ 0.4333837887411888*m*y11*Sin[theta2], 0.904371004615765*b*y11 + (0.904371004615765*a*y11)/L1}, {Ia + L1*m, Ia, (0.904371004615765*Ia)/L1 - \ 0.6583189111222529*m*Cos[theta2], 0}, {(0.904371004615765*Ia)/L1 + 0.904371004615765*m + \ (0.7396732100669774*Ib*y11^2)/L1^3 + (0.7396732100669774*m*y11^2)/L1^2 - 0.6583189111222529*m*Cos[theta2] - 0.6583189111222529*L1*m*Cos[theta2] - \ (0.5384304226388572*m*y11^2*Cos[theta2])/L1^2 - (0.5384304226388572*m*y11^2*Cos[theta2])/L1 - \ 0.4333837887411888*m*y11*Sin[theta2], (0.904371004615765*Ia)/L1 - 0.6583189111222529*m*Cos[theta2], 1 + (0.817886913989728*Ia)/L1^2 + 0.4333837887411888*m + (0.6689390040756399*Ib*y11^2)/L1^4 + (0.1772294647733534*m*y11^2)/L1^2 \ - (1.190729070018376*m*Cos[theta2])/L1 - \ (0.973881724475188*m*y11^2*Cos[theta2])/L1^3 + (0.1772294647733534*m*y11^2*Cos[2*theta2])/L1^2, (0.817886913989728*a*y11)/L1^2 - \ (0.5953645350091882*b*y11*Cos[theta2])/L1}, {0.904371004615765*b*y11 + (0.904371004615765*a*y11)/L1, 0, (0.817886913989728*a*y11)/L1^2 - \ (0.5953645350091882*b*y11*Cos[theta2])/L1, 1}}, {{{1}}, {{1}}, {{1, 0}, {0, 1}}}, {theta1, theta2, y11, y21}, {w1, w2, \ v11, v21}] . {w1, w2, v11, v21} \ \>", "\<\ {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz \ Sin[theta2] - d vz Sin[theta3]), d mw (2 w1 + w2) (a Cos[theta2] - b \ Sin[theta2]) -((2 mw + m1) vz), (2 mw + m1) vy, \ ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz \ Sin[theta2] + d mw (w1 + w2) \ Cos[theta2] 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) ------------------------------------------------------------------------\ ---------, 0}\\ 4 , {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) -------------------------------------------------------------------------\ -------}} 4 2 \ 2 2 0.817887 Ib y11 2 1.63577 m \ y11 [{{2 Ia + 2 L1 m + L1 m + ---------------- + 1.25127 m y11 + \ --------------, 2 L1 L1 2 2 0.904371 Ia 0.739673 Ib y11 0.739673 m y11 Ia + L1 m, ----------- + 0.904371 m + ---------------- + --------------- \ - L1 3 2 L1 L1 2 0.53843 m y11 \ Cos[theta2] 0.658319 m Cos[theta2] - 0.658319 L1 m Cos[theta2] - \ -------------------------- - 2 L1 2 0.53843 m y11 Cos[theta2] -------------------------- - 0.433384 m y11 Sin[theta2], L1 0.904371 a y11 0.904371 b y11 + --------------}, L1 0.904371 Ia {Ia + L1 m, Ia, ----------- - 0.658319 m Cos[theta2], 0}, L1 2 2 0.904371 Ia 0.739673 Ib y11 0.739673 m y11 {----------- + 0.904371 m + ---------------- + --------------- - L1 3 2 L1 L1 2 0.53843 m y11 \ Cos[theta2] 0.658319 m Cos[theta2] - 0.658319 L1 m Cos[theta2] - \ -------------------------- - 2 L1 2 0.53843 m y11 Cos[theta2] -------------------------- - 0.433384 m y11 Sin[theta2], L1 0.904371 Ia ----------- - 0.658319 m Cos[theta2], L1 2 2 0.817887 Ia 0.668939 Ib y11 0.177229 m y11 1 + ----------- + 0.433384 m + ---------------- + --------------- - 2 4 2 L1 L1 L1 2 2 1.19073 m Cos[theta2] 0.973882 m y11 Cos[theta2] 0.177229 m y11 \ Cos[2 theta2] --------------------- - --------------------------- + \ -----------------------------, L1 3 2 L1 L1 0.817887 a y11 0.595365 b y11 Cos[theta2] -------------- - --------------------------}, 2 L1 L1 0.904371 a y11 0.817887 a y11 0.595365 b y11 \ Cos[theta2] {0.904371 b y11 + --------------, 0, -------------- - \ --------------------------, 1}}, L1 2 L1 L1 {{{1}}, {{1}}, {{1, 0}, {0, 1}}}, {theta1, theta2, y11, y21}, {w1, w2, \ v11, v21}] . {w1, w2, v11, v21}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Fp"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {-(gc*(0.882231881465798*m*y11*Cos[theta1] + \ 0.223912970343545*m*y21*Cos[theta1 + theta2] - 2*L1*m*Sin[theta1] - L2*m*Sin[theta1 + theta2])), -(gc*(0.223912970343545*m*y21*Cos[theta1 + theta2] - L2*m*Sin[theta1 + \ theta2])), (0.36*El*Ixx*Pi^2*y11)/(L1^3*m) - 0.882231881465798*gc*m*Sin[theta1], (0.36*El*Ixx*Pi^2*y21)/(L2^3*m) - 0.223912970343545*gc*m*Sin[theta1 + \ theta2]} \ \>", "\<\ {-(gc (0.882232 m y11 Cos[theta1] + 0.223913 m y21 Cos[theta1 + \ theta2] - 2 L1 m Sin[theta1] - L2 m Sin[theta1 + theta2])), -(gc (0.223913 m y21 Cos[theta1 + theta2] - L2 m Sin[theta1 + theta2])), 2 0.36 El Ixx Pi y11 ------------------- - 0.882232 gc m Sin[theta1], 3 L1 m 2 0.36 El Ixx Pi y21 ------------------- - 0.223913 gc m Sin[theta1 + theta2]} 3 L2 m\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "The absolute position and orientation of a frame at the end of the second \ link can be obtained with the function EndEffector."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["EndPos=EndEffector[3,TreeList,BodyList,X];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Let's take a look at the position vector:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["EndPos[[{1,2,3},4]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {(0.5953645350091882*y11*y21*Cos[theta2])/L1 - \ (0.904371004615765*L2*y11*Sin[theta2])/L1, 0.6583189111222529*y11*Cos[theta1] - L1*Sin[theta1] + L2*(-(Cos[theta2]*Sin[theta1]) - Cos[theta1]*Sin[theta2]) + 0.6583189111222529*y21*(Cos[theta1]*Cos[theta2] - \ Sin[theta1]*Sin[theta2]), L1*Cos[theta1] + 0.6583189111222529*y11*Sin[theta1] + 0.6583189111222529*y21*(Cos[theta2]*Sin[theta1] + Cos[theta1]*Sin[theta2]) \ + L2*(Cos[theta1]*Cos[theta2] - Sin[theta1]*Sin[theta2])} \ \>", "\<\ 0.595365 y11 y21 Cos[theta2] 0.904371 L2 y11 Sin[theta2] {---------------------------- - ---------------------------, L1 L1 0.658319 y11 Cos[theta1] - L1 Sin[theta1] + L2 (-(Cos[theta2] Sin[theta1]) - Cos[theta1] Sin[theta2]) + 0.658319 y21 (Cos[theta1] Cos[theta2] - Sin[theta1] Sin[theta2]), L1 Cos[theta1] + 0.658319 y11 Sin[theta1] + 0.658319 y21 (Cos[theta2] Sin[theta1] + Cos[theta1] Sin[theta2]) + L2 (Cos[theta1] Cos[theta2] - Sin[theta1] Sin[theta2])}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Constraints"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Introductory Remarks"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Many systems of interest do not have a simple tree structure but can be \ viewed in terms of a tree with additional algebraic and/or differential \ constraints. A key advantage of the Lagrange formulation is that the explicit \ formulation of the relations that constrain the configuration of a system is \ usually avoided by appropriate choice of the generalized coordinates. \ However, with complex systems, a correct choice of generalized coordinates \ may be far from obvious. \n\nOne systematic method for dealing with \ mechanical configurations containing closed loops is to break (free) a \ minimal number of joints so as to obtain a tree structure which, of course, \ has a natural set of configuration coordinates. The loop-breaking joint \ constraints are then added as suplemental algebraic relations. The number of \ degrees of freedom of the system is typically reduced by the number of \ independent relations. If these relations are (globally) solvable for a like \ number of coordinates then these coordinates can be explicitly eliminated. \ When there are k such constraints, the number of coordinates is reduced by k \ and the number of quasi-velocities is also reduced by k. It may not be \ possible to eliminate all k excess coordinates, but it is always possible to \ eliminate k quasi-velocities.\n\nSome chain and loop structures give rise to \ differential constraints \[CapitalADoubleDot] such as when rolling of one \ body on another occurs. In general, when k independent differential \ constraints are present it is possible to eliminate k quasi-velocities. If \ the constraints are \"partially integrable\" then, in principle, it is also \ possible to eliminate some coordinates as well. When the constraints are \ \"completely integrable\" the number of configuration degrees of freedom are \ reduced by k and the system is said to be \"holonomic.\" Otherwise it is \ \"nonholonomic.\"\n\nEven when it is not possible or practical to eliminate \ all k configuration coordinates it is fairly straightforward to derived the \ reduced set of equations in which the k quasi-velocities are removed. These \ equations form a valid, closed system of differential equations that \ completely describe the system behavior. Presently, TSi Dynamics provides \ functions to generate this latter set of equations unless the user specifies \ a set of configuration coordinates that are to be removed."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["A Simple Closed Chain"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Consider a planar two bar linkage in which the lower end of bar 1 is \ constrained by a revolute joint on the y-axis at y=0 and the upper end of bar \ two slides on the z-axis. The upper end of bar 1 and the lower end of bar 2 \ are connected by a revolute joint."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "r1={1};\nH1=Transpose[{{1,0,0,0,0,0}}];\nq1={theta1};p1={w1};\n\nr2={1};\n\ H2=Transpose[{{1,0,0,0,0,0}}];\nq2={theta2};p2={w2};\n\n\ JointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Mass1=m1; CenterOfMass1={0,0,L1/2}; OutboardNode1={2,{0,0,L1}};\n\ InertiaMatrix1=DiagonalMatrix[{m1*L1^2/12,m1*L1^2/12,0}];\n\nMass2=m2; \ CenterOfMass2={0,0,L2/2}; OutboardNode2={3,{0,0,L2}};\n\ InertiaMatrix2=DiagonalMatrix[{m2*L2^2/12,m2*L2^2/12,0}];\n\n\ BodyList={{CenterOfMass1,{OutboardNode1},Mass1,InertiaMatrix1},\n \ {CenterOfMass2,{OutboardNode2},Mass2,InertiaMatrix2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"Mass1\" is similar to existing symbol \"mass1\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \"Mass1\ \" is similar to existing symbol \"mass1\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"Mass2\" is similar to existing symbol \"mass2\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \"Mass2\ \" is similar to existing symbol \"mass2\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["TreeList={{{1,1},{2,2}}};\nPE=0; Q={0,0};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cp,Fp,p,q}=CreateModelSim[JointList,BodyList,TreeList,g,PE,Q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "ChnBodyList={{CenterOfMass1,{0,0,L1},Mass1,InertiaMatrix1},\n \ {CenterOfMass2,{0,0,L2},Mass2,InertiaMatrix2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["EndPos=EndEffector[ChnBodyList,X];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["G=EndPos[[{2},4]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {-(L1*Sin[theta1]) + L2*(-(Cos[theta2]*Sin[theta1]) - \ Cos[theta1]*Sin[theta2])} \ \>", "\<\ {-(L1 Sin[theta1]) + L2 (-(Cos[theta2] Sin[theta1]) - Cos[theta1] \ Sin[theta2])}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "The the relation G=0 is not solvable for either theta1 or theta2 for all \ values of L1 and L2 \[CapitalADoubleDot] although, as we will describe below, \ it can be solved for particular values. Thus, we will not try to eliminate \ any configuration coordinate. The constrained dynamics are obtained with:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{Mm,Cm,Fm,Vm,phat,qhat}=AlgConstrainedSys[M,Cp,Fp,V,G,p,q]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"qhat\" is similar to existing symbol \"phat\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"qhat\" is similar to existing symbol \"phat\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ {{{(L1^2*L2^2*(2*m1 + 5*m2 - m2*Cos[2*theta1] - 3*m2*Cos[2*theta2] + 2*m1*Cos[2*(theta1 + theta2)] + 3*m2*Cos[2*(theta1 + theta2)]))/ (12*(L1*Cos[theta1] + L2*Cos[theta1 + theta2])^2)}}, {{{{-((L2*Cos[theta1 + theta2])/(L1*Cos[theta1] + L2*Cos[theta1 + \ theta2])), 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(a*Cos[theta2] - b*Sin[theta2])* (w2 - (2*L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/2, -(d*mw*(w3 - (2*L2*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + L2*Sin[theta1]*Sin[theta2]))*(a*Cos[theta3] + \ b*Sin[theta3]))/2}, {-((d*L2*mw*w2*(-Cos[theta2] + Cos[theta3])* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2])), 0, (L2*(2*mw + m1)*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]), -(d*mw*Cos[theta2]*(w2 - (2*L2*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) )), d*mw*Cos[theta3]*(w3 - (2*L2*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]))} , {(L2*mw*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2])* (a + d*Sin[theta2] - d*Sin[theta3]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]), -((L2*(2*mw + m1)*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2])), 0, -(d*mw*Sin[theta2]*(w2 - (2*L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) )), d*mw*(w3 - (2*L2*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]))* Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - a*w2*Cos[theta2] + \ 2*vz*Sin[theta2] + b*w2*Sin[theta2] + (2*a*L2*w2*Cos[theta2]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) - (2*b*L2*w2*Sin[theta2]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/4, (d*mw*Cos[theta2]* (w2 - (L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/2, (d*mw*Sin[theta2]* (w2 - (L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/2, (d*mw*(-2*vy*Cos[theta2] - 2*vz*Sin[theta2] - (a*L2*w2*Cos[theta2]*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) + (b*L2*w2*Sin[theta2]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + a*w3*Cos[theta3] - (2*a*L2*w2*Cos[theta3]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) - 2*vz*Sin[theta3] + b*w3*Sin[theta3] - (2*b*L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2])* Sin[theta3])/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/4, -(d*mw*Cos[theta3]* (w3 - (L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + L2*Sin[theta1]*Sin[theta2])))/2, -(d*mw*(w3 - (L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + L2*Sin[theta1]*Sin[theta2]))*Sin[theta3])/2, 0, (d*mw*(2*vy*Cos[theta3] + (a*L2*w2*Cos[theta3]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) + 2*vz*Sin[theta3] + (b*L2*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2])*Sin[theta3])/ (-(L1*Cos[theta1]) - L2*Cos[theta1]*Cos[theta2] + \ L2*Sin[theta1]*Sin[theta2]) ))/4}}[{{(L1^2*m1)/3 + L1^2*m2 + (L2^2*m2)/3 + \ L1*L2*m2*Cos[theta2], (L2*m2*(2*L2 + 3*L1*Cos[theta2]))/6}, {(L2*m2*(2*L2 + 3*L1*Cos[theta2]))/6, (L2^2*m2)/3}}, {{{1}}, {{1}}}, {theta1, theta2}, {-((L2*w2*Cos[theta1 + theta2])/ (L1*Cos[theta1] + L2*Cos[theta1 + theta2])), w2}] . {{-((L2*Cos[theta1 + theta2])/(L1*Cos[theta1] + L2*Cos[theta1 + \ theta2]))}, {1}} + (L1^2*L2^3*w2*Cos[theta1 + theta2]* (-(L2*m2*Cos[theta1]) - 3*L1*m2*Cos[theta1 - theta2] + 4*L1*m1*Cos[theta1 + theta2] + 9*L1*m2*Cos[theta1 + theta2] + 3*L2*m2*Cos[theta1 + 2*theta2])*Sin[theta2])/ (12*(L1*Cos[theta1] + L2*Cos[theta1 + theta2])^4) + (L1^2*L2^2*w2*Cos[theta1]*(L2*m2*Cos[theta1] + 3*L1*m2*Cos[theta1 - \ theta2] - 4*L1*m1*Cos[theta1 + theta2] - 9*L1*m2*Cos[theta1 + theta2] - 3*L2*m2*Cos[theta1 + 2*theta2])*Sin[theta1 + theta2])/ (12*(L1*Cos[theta1] + L2*Cos[theta1 + theta2])^3)}}, {-(gc*L1*L2*(-(m1*Sin[theta2]) - 3*m2*Sin[theta2] + m1*Sin[2*theta1 + \ theta2] + m2*Sin[2*theta1 + theta2]))/(4*(L1*Cos[theta1] + L2*Cos[theta1 + \ theta2]))}, {{-((L2*Cos[theta1 + theta2])/(L1*Cos[theta1] + L2*Cos[theta1 + theta2]))}, \ {1}}, {w2}, {theta1, theta2}} \ \>", "\<\ 2 2 {{{(L1 L2 (2 m1 + 5 m2 - m2 Cos[2 theta1] - 3 m2 Cos[2 theta2] + 2 m1 Cos[2 (theta1 + theta2)] + 3 m2 Cos[2 (theta1 + theta2)])) / 2 (12 (L1 Cos[theta1] + L2 Cos[theta1 + theta2]) )}}, L2 Cos[theta1 + theta2] {{{{-(----------------------------------------), 1}} . L1 Cos[theta1] + L2 Cos[theta1 + theta2] {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, (d mw (a Cos[theta2] - b Sin[theta2]) (w2 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] / 2, -(d mw (w3 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) --------------------------------------------------------------\ -------------) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] (a Cos[theta3] + b Sin[theta3])) / 2}, {-((d L2 mw w2 (-Cos[theta2] + Cos[theta3]) (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2])) / (-(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2])), L2 (2 mw + m1) w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) 0, \ ---------------------------------------------------------------------------, -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] -(d mw Cos[theta2] (w2 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] , d mw Cos[theta3] (w3 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -----------------------------------------------------------------\ ----------)}, -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] {(L2 mw w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) (a + d Sin[theta2] - d Sin[theta3])) / (-(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2]), L2 (2 mw + m1) w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -(-------------------------------------------------------------------\ --------), 0, -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] -(d mw Sin[theta2] (w2 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] , d mw (w3 - 2 L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -----------------------------------------------------------------\ ----------) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] Sin[theta3]}, {(d mw (2 vy Cos[theta2] - a w2 Cos[theta2] + 2 vz \ Sin[theta2] + b w2 Sin[theta2] + 2 a L2 w2 Cos[theta2] (-(Cos[theta1] Cos[theta2]) + \ Sin[theta1] Sin[theta2]) ---------------------------------------------------------------\ -------------\\ -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] - 2 b L2 w2 Sin[theta2] (-(Cos[theta1] Cos[theta2]) + \ Sin[theta1] Sin[theta2]) ---------------------------------------------------------------\ -------------) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] ) / 4, (d mw Cos[theta2] (w2 - L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] / 2, (d mw Sin[theta2] (w2 - L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ -----------))\\ -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] / 2, (d mw (-2 vy Cos[theta2] - 2 vz Sin[theta2] - a L2 w2 Cos[theta2] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ ----------- + -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] b L2 w2 Sin[theta2] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ -----------))\\ -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] / 4, 0}, {(d mw (-2 vy Cos[theta3] + a w3 Cos[theta3] - 2 a L2 w2 Cos[theta3] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) ----------------------------------------------------------------\ ------------ \\ -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] - 2 vz Sin[theta3] + b w3 Sin[theta3] - 2 b L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) Sin[theta3] ----------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 Sin[theta1] \ Sin[theta2] / 4, -(d mw Cos[theta3] (w3 - L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] / 2, -(d mw (w3 - L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) --------------------------------------------------------------\ -------------) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] Sin[theta3]) / 2, 0, (d mw (2 vy Cos[theta3] + a L2 w2 Cos[theta3] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------ \\ -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] + 2 vz Sin[theta3] + b L2 w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) \ Sin[theta3] ---------------------------------------------------------------\ ------------)) -(L1 Cos[theta1]) - L2 Cos[theta1] Cos[theta2] + L2 \ Sin[theta1] Sin[theta2] 2 2 L1 m1 2 L2 m2 / 4}}[{{------ + L1 m2 + ------ + L1 L2 m2 Cos[theta2], 3 3 \ 2 L2 m2 (2 L2 + 3 L1 Cos[theta2]) L2 m2 (2 L2 + 3 L1 Cos[theta2]) \ L2 m2 -------------------------------}, {-------------------------------, \ ------}}, 6 6 \ 3 L2 w2 Cos[theta1 + theta2] {{{1}}, {{1}}}, {theta1, theta2}, \ {-(----------------------------------------), w2}]\\ L1 Cos[theta1] + L2 Cos[theta1 + \ theta2] L2 Cos[theta1 + theta2] . {{-(----------------------------------------)}, {1}} + L1 Cos[theta1] + L2 Cos[theta1 + theta2] 2 3 (L1 L2 w2 Cos[theta1 + theta2] (-(L2 m2 Cos[theta1]) - 3 L1 m2 Cos[theta1 - theta2] + 4 L1 m1 Cos[theta1 + theta2] + 9 L1 m2 Cos[theta1 + theta2] + 3 L2 m2 Cos[theta1 + 2 theta2]) Sin[theta2]) / 4 (12 (L1 Cos[theta1] + L2 Cos[theta1 + theta2]) ) + 2 2 (L1 L2 w2 Cos[theta1] (L2 m2 Cos[theta1] + 3 L1 m2 Cos[theta1 - \ theta2] - 4 L1 m1 Cos[theta1 + theta2] - 9 L1 m2 Cos[theta1 + theta2] - 3 L2 m2 Cos[theta1 + 2 theta2]) Sin[theta1 + theta2]) / 3 (12 (L1 Cos[theta1] + L2 Cos[theta1 + theta2]) )}}, {-(gc L1 L2 (-(m1 Sin[theta2]) - 3 m2 Sin[theta2] + m1 Sin[2 theta1 + \ theta2] + m2 Sin[2 theta1 + theta2])) / (4 (L1 Cos[theta1] + L2 Cos[theta1 + \ theta2]))}, L2 Cos[theta1 + theta2] {{-(----------------------------------------)}, {1}}, {w2}, {theta1, \ theta2}} L1 Cos[theta1] + L2 Cos[theta1 + theta2]\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "These parameters define the constrained system dynamics. Now, let's consider \ the special case: L1=L2=L. This is the only situation where G=0 can be solved \ for either theta1 or theta2, so that either angle can be used to parameterize \ the system configuration. For simplicity, also set m1=m2=m. In this case the \ system matrices are:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{Mm,Cm,Fm,Vm}=Simplify[{Mm,Cm,Fm,Vm}/.{L1->L,L2->L,m1->m,m2->m}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{(L^2*m*(7 - Cos[2*theta1] - 3*Cos[2*theta2] + 5*Cos[2*(theta1 + \ theta2)])* Sec[theta1 + theta2/2]^2*Sec[theta2/2]^2)/48}}, {{{{-(Cos[theta1 + theta2]*Sec[theta1 + theta2/2]*Sec[theta2/2])/2, 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((m + 2*mw)*vz), (m + 2*mw)*vy, (d*mw*(a*Cos[theta2] - b*Sin[theta2])* (w2 - (2*L*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 2, -(d*mw*(w3 - (2*L*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]))* (a*Cos[theta3] + b*Sin[theta3]))/2}, {-((d*L*mw*w2*(-Cos[theta2] + Cos[theta3])* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])), 0, (L*(m + 2*mw)*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]), -(d*mw*Cos[theta2]*(w2 - (2*L*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))\\ , d*mw*Cos[theta3]*(w3 - (2*L*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]))}, {(L*mw*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2])* (a + d*Sin[theta2] - d*Sin[theta3]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]), -((L*(m + 2*mw)*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])), 0, -(d*mw*Sin[theta2]*(w2 - (2*L*w2* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))\\ , d*mw*(w3 - (2*L*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]))* Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - a*w2*Cos[theta2] + \ 2*vz*Sin[theta2] + b*w2*Sin[theta2] + (2*a*L*w2*Cos[theta2]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]) - (2*b*L*w2*Sin[theta2]*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 4, (d*mw*Cos[theta2]*(w2 - (L*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 2, (d*mw*Sin[theta2]*(w2 - (L*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 2, (d*mw*(-2*vy*Cos[theta2] - 2*vz*Sin[theta2] - (a*L*w2*Cos[theta2]*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]) + (b*L*w2*Sin[theta2]*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 4, 0}, {(d*mw*(-2*vy*Cos[theta3] + a*w3*Cos[theta3] - (2*a*L*w2*Cos[theta3]*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]) - 2*vz*Sin[theta3] + b*w3*Sin[theta3] - (2*b*L*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2])*Sin[theta3])/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 4, -(d*mw*Cos[theta3]*(w3 - (L*w2*(-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]))) /2, -(d*mw*(w3 - (L*w2*(-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]))* Sin[theta3])/2, 0, (d*mw* (2*vy*Cos[theta3] + (a*L*w2*Cos[theta3]* (-(Cos[theta1]*Cos[theta2]) + Sin[theta1]*Sin[theta2]))/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2]) + 2*vz*Sin[theta3] + (b*L*w2* (-(Cos[theta1]*Cos[theta2]) + \ Sin[theta1]*Sin[theta2])*Sin[theta3])/ (-(L*Cos[theta1]) - L*Cos[theta1]*Cos[theta2] + \ L*Sin[theta1]*Sin[theta2])))/ 4}}[{{(L^2*m*(5 + 3*Cos[theta2]))/3, (L^2*m*(2 + \ 3*Cos[theta2]))/6}, {(L^2*m*(2 + 3*Cos[theta2]))/6, (L^2*m)/3}}, {{{1}}, {{1}}}, {theta1, \ theta2}, {-(w2*Cos[theta1 + theta2]*Sec[theta1 + theta2/2]*Sec[theta2/2])/2, \ w2}] . {{-(Cos[theta1 + theta2]*Sec[theta1 + theta2/2]*Sec[theta2/2])/2}, {1}} \ + (L^2*m*w2*Cos[theta1]*(Cos[theta1] + 3*Cos[theta1 - theta2] - 13*Cos[theta1 + theta2] - 3*Cos[theta1 + 2*theta2])*Sec[theta1 + \ theta2/2]^3* Sec[theta2/2]^3*Sin[theta1 + theta2])/96 + (L^2*m*w2*Cos[theta1 + theta2]*(-Cos[theta1] - 3*Cos[theta1 - theta2] + 13*Cos[theta1 + theta2] + 3*Cos[theta1 + 2*theta2])*Sec[theta1 + \ theta2/2]^4* Sec[theta2/2]^2*Tan[theta2/2])/96}}, {-(gc*L*m*Sec[theta1 + theta2/2]*Sec[theta2/2]*(-2*Sin[theta2] + \ Sin[2*theta1 + theta2]))/ 4}, {{-(Cos[theta1 + theta2]*Sec[theta1 + theta2/2]*Sec[theta2/2])/2}, \ {1}}} \ \>", "\<\ 2 {{{(L m (7 - Cos[2 theta1] - 3 Cos[2 theta2] + 5 Cos[2 (theta1 + theta2)]) theta2 2 theta2 2 Sec[theta1 + ------] Sec[------] ) / 48}}, 2 2 theta2 theta2 -(Cos[theta1 + theta2] Sec[theta1 + ------] Sec[------]) 2 2 {{{{--------------------------------------------------------, 1}} . 2 {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((m + 2 mw) vz), (m + 2 mw) vy, (d mw (a Cos[theta2] - b Sin[theta2]) 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) (w2 - \ ------------------------------------------------------------------------)) -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L \ Sin[theta1] Sin[theta2] / 2, -(d mw (w3 - 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) --------------------------------------------------------------\ ----------) -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] (a Cos[theta3] + b Sin[theta3])) / 2}, {-((d L mw w2 (-Cos[theta2] + Cos[theta3]) (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2])) / (-(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2])), 0, L (m + 2 mw) w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------------\ ---, -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] -(d mw Cos[theta2] (w2 - 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ---------)), -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] d mw Cos[theta3] (w3 - 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -----------------------------------------------------------------\ -------)}, -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] {(L mw w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) (a + d Sin[theta2] - d Sin[theta3])) / (-(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2]), L (m + 2 mw) w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -(-------------------------------------------------------------------\ -----), 0, -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] -(d mw Sin[theta2] (w2 - 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ---------)), -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] d mw (w3 - 2 L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) -----------------------------------------------------------------\ -------) -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] Sin[theta3]}, {(d mw (2 vy Cos[theta2] - a w2 Cos[theta2] + 2 vz \ Sin[theta2] + b w2 Sin[theta2] + 2 a L w2 Cos[theta2] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ------------ \\ -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 2 b L w2 Sin[theta2] (-(Cos[theta1] Cos[theta2]) + \ Sin[theta1] Sin[theta2]) - \ --------------------------------------------------------------------------- -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L \ Sin[theta1] Sin[theta2] )) / 4, (d mw Cos[theta2] L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) (w2 - \ ------------------------------------------------------------------------)) -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L \ Sin[theta1] Sin[theta2] / 2, (d mw Sin[theta2] (w2 - L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ --------)) / 2 -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] , (d mw (-2 vy Cos[theta2] - 2 vz Sin[theta2] - a L w2 Cos[theta2] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ --------- + -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] b L w2 Sin[theta2] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ ---------)) / -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 4, 0}, {(d mw (-2 vy Cos[theta3] + a w3 Cos[theta3] - 2 a L w2 Cos[theta3] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ----------------------------------------------------------------\ ----------- - -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 2 vz Sin[theta3] + b w3 Sin[theta3] - 2 b L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) \ Sin[theta3] ----------------------------------------------------------------\ -----------))\\ -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] / 4, -(d mw Cos[theta3] (w3 - L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ---------)) / -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 2, -(d mw (w3 - L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) --------------------------------------------------------------\ ----------) -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] Sin[theta3]) / 2, 0, (d mw (2 vy Cos[theta3] + a L w2 Cos[theta3] (-(Cos[theta1] Cos[theta2]) + Sin[theta1] \ Sin[theta2]) ---------------------------------------------------------------\ ---------- + -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 2 vz Sin[theta3] + b L w2 (-(Cos[theta1] Cos[theta2]) + Sin[theta1] Sin[theta2]) \ Sin[theta3] ---------------------------------------------------------------\ ----------)) \\ -(L Cos[theta1]) - L Cos[theta1] Cos[theta2] + L Sin[theta1] \ Sin[theta2] 2 2 L m (5 + 3 Cos[theta2]) L m (2 + 3 Cos[theta2]) / 4}}[{{------------------------, ------------------------}, 3 6 2 2 L m (2 + 3 Cos[theta2]) L m {------------------------, ----}}, {{{1}}, {{1}}}, {theta1, theta2}, 6 3 theta2 theta2 -(w2 Cos[theta1 + theta2] Sec[theta1 + ------] Sec[------]) 2 2 {-----------------------------------------------------------, w2}] . 2 theta2 theta2 -(Cos[theta1 + theta2] Sec[theta1 + ------] Sec[------]) 2 2 {{--------------------------------------------------------}, {1}} + 2 2 (L m w2 Cos[theta1] (Cos[theta1] + 3 Cos[theta1 - theta2] - 13 \ Cos[theta1 + theta2] - theta2 3 theta2 3 3 Cos[theta1 + 2 theta2]) Sec[theta1 + ------] Sec[------] \ Sin[theta1 + theta2]) 2 2 2 / 96 + (L m w2 Cos[theta1 + theta2] (-Cos[theta1] - 3 Cos[theta1 - theta2] + 13 Cos[theta1 + theta2] + theta2 4 theta2 2 \ theta2 3 Cos[theta1 + 2 theta2]) Sec[theta1 + ------] Sec[------] \ Tan[------]) / 96}}, 2 2 \ 2 theta2 theta2 -(gc L m Sec[theta1 + ------] Sec[------] (-2 Sin[theta2] + Sin[2 theta1 + \ theta2])) 2 2 {---------------------------------------------------------------------------\ ---------}, 4 theta2 theta2 -(Cos[theta1 + theta2] Sec[theta1 + ------] Sec[------]) 2 2 {{--------------------------------------------------------}, {1}}} 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "It is convenient toassemble the corresponding differential equations."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Eqns=MakeODEs[phat,qhat,Vm,Mm,Cm,Fm,t]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][theta1][t] == -(Cos[theta1[t] + theta2[t]]*Sec[theta1[t] \ + theta2[t]/2]* Sec[theta2[t]/2]*w2[t])/2, Derivative[1][theta2][t] == w2[t], -(gc*L*m*Sec[theta1[t] + theta2[t]/2]*Sec[theta2[t]/2]* (-2*Sin[theta2[t]] + Sin[2*theta1[t] + theta2[t]]))/4 + w2[t]*({{-(Cos[theta1[t] + theta2[t]]*Sec[theta1[t] + \ theta2[t]/2]*Sec[theta2[t]/2])/2, 1}} . {{mw*(a*vz - d*vy*Cos[theta3] + d*vy*Cos[theta2[t]] - \ d*vz*Sin[theta3] + d*vz*Sin[theta2[t]]), -((m + 2*mw)*vz), (m + 2*mw)*vy, (d*mw*(a*Cos[theta2[t]] - b*Sin[theta2[t]])* (w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2, -(d*mw*(a*Cos[theta3] + b*Sin[theta3])* (w3 - (2*L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2}, {-((d*L*mw*(Cos[theta3] - Cos[theta2[t]])* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])), 0, (L*(m + 2*mw)*(-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]), -(d*mw*Cos[theta2[t]]*(w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]))), d*mw*Cos[theta3]*(w3 - (2*L* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]))}, {(L*mw*(a - d*Sin[theta3] + d*Sin[theta2[t]])* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]), -((L*(m + 2*mw)*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])), 0, -(d*mw*Sin[theta2[t]]*(w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]))), d*mw*Sin[theta3]*(w3 - (2*L* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]))}, {(d*mw*(2*vy*Cos[theta2[t]] + 2*vz*Sin[theta2[t]] - \ a*Cos[theta2[t]]*w2[t] + b*Sin[theta2[t]]*w2[t] + (2*a*L*Cos[theta2[t]]* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t]) /(-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]) - (2*b*L*Sin[theta2[t]]* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t]) /(-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/4, (d*mw*Cos[theta2[t]]*(w2[t] - (L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2, (d*mw*Sin[theta2[t]]*(w2[t] - (L*(-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2, (d*mw*(-2*vy*Cos[theta2[t]] - 2*vz*Sin[theta2[t]] - (a*L*Cos[theta2[t]]*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]) + (b*L*Sin[theta2[t]]*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + b*w3*Sin[theta3] - (2*a*L*Cos[theta3]* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t]) /(-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]) - (2*b*L*Sin[theta3]*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/4, -(d*mw*Cos[theta3]*(w3 - (L* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t] )/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2, -(d*mw*Sin[theta3]*(w3 - (L* (-(Cos[theta1[t]]*Cos[theta2[t]]) + \ Sin[theta1[t]]*Sin[theta2[t]])*w2[t] )/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/2, 0, (d*mw*(2*vy*Cos[theta3] + 2*vz*Sin[theta3] + (a*L*Cos[theta3]*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]]) + (b*L*Sin[theta3]*(-(Cos[theta1[t]]*Cos[theta2[t]]) + Sin[theta1[t]]*Sin[theta2[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[theta2[t]] + L*Sin[theta1[t]]*Sin[theta2[t]])))/4}}[{{(L^2*m*(5 + \ 3*Cos[theta2[t]]))/ 3, (L^2*m*(2 + 3*Cos[theta2[t]]))/6}, {(L^2*m*(2 + 3*Cos[theta2[t]]))/6, (L^2*m)/3}}, {{{1}}, {{1}}}, {theta1[t], theta2[t]}, {-(Cos[theta1[t] + theta2[t]]*Sec[theta1[t] \ + theta2[t]/2]* Sec[theta2[t]/2]*w2[t])/2, w2[t]}] . {{-(Cos[theta1[t] + theta2[t]]*Sec[theta1[t] + \ theta2[t]/2]*Sec[theta2[t]/2])/2}, {1}} + (L^2*m*Cos[theta1[t]]* (Cos[theta1[t]] + 3*Cos[theta1[t] - theta2[t]] - 13*Cos[theta1[t] + \ theta2[t]] - 3*Cos[theta1[t] + 2*theta2[t]])*Sec[theta1[t] + theta2[t]/2]^3* Sec[theta2[t]/2]^3*Sin[theta1[t] + theta2[t]]*w2[t])/96 + (L^2*m*Cos[theta1[t] + theta2[t]]* (-Cos[theta1[t]] - 3*Cos[theta1[t] - theta2[t]] + 13*Cos[theta1[t] \ + theta2[t]] + 3*Cos[theta1[t] + 2*theta2[t]])*Sec[theta1[t] + theta2[t]/2]^4* Sec[theta2[t]/2]^2*Tan[theta2[t]/2]*w2[t])/96) + (L^2*m*(7 - Cos[2*theta1[t]] - 3*Cos[2*theta2[t]] + 5*Cos[2*(theta1[t] + \ theta2[t])])* Sec[theta1[t] + \ theta2[t]/2]^2*Sec[theta2[t]/2]^2*Derivative[1][w2][t])/48 == 0} \ \>", "\<\ \ theta2[t] theta2[t] {theta1'[t] == -(Cos[theta1[t] + theta2[t]] Sec[theta1[t] + ---------] \ Sec[---------] 2 \ 2 w2[t]) / 2, theta2'[t] == w2[t], theta2[t] theta2[t] -(gc L m Sec[theta1[t] + ---------] Sec[---------] 2 2 (-2 Sin[theta2[t]] + Sin[2 theta1[t] + theta2[t]])) / 4 + theta2[t] \ theta2[t] w2[t] ({{-(Cos[theta1[t] + theta2[t]] Sec[theta1[t] + ---------] \ Sec[---------]) / 2, 2 \ 2 1}} . {{mw (a vz - d vy Cos[theta3] + d vy Cos[theta2[t]] - d vz \ Sin[theta3] + d vz Sin[theta2[t]]), -((m + 2 mw) vz), (m + 2 mw) vy, (d mw (a Cos[theta2[t]] - b Sin[theta2[t]]) (w2[t] - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2, -(d mw (a Cos[theta3] + b Sin[theta3]) (w3 - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2}, {-((d L mw (Cos[theta3] - Cos[theta2[t]]) (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]])), 0, (L (m + 2 mw) (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]), -(d mw Cos[theta2[t]] (w2[t] - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))), d mw Cos[theta3] (w3 - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) \\ / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))}, {(L mw (a - d Sin[theta3] + d Sin[theta2[t]]) (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]), -((L (m + 2 mw) (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]])), 0, -(d mw Sin[theta2[t]] (w2[t] - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))), d mw Sin[theta3] (w3 - (2 L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) \\ / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))}, {(d mw (2 vy Cos[theta2[t]] + 2 vz Sin[theta2[t]] - a \ Cos[theta2[t]] w2[t] + b Sin[theta2[t]] w2[t] + (2 a L Cos[theta2[t]] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + \ L Sin[theta1[t]] Sin[theta2[t]]) - (2 b L Sin[theta2[t]] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + \ L Sin[theta1[t]] Sin[theta2[t]]))) / 4, (d mw Cos[theta2[t]] (w2[t] - (L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2, (d mw Sin[theta2[t]] (w2[t] - (L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2, (d mw (-2 vy Cos[theta2[t]] - 2 vz Sin[theta2[t]] - (a L Cos[theta2[t]] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]) + (b L Sin[theta2[t]] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 4, 0}, {(d mw (-2 vy Cos[theta3] + a w3 Cos[theta3] - 2 vz Sin[theta3] + b w3 Sin[theta3] - (2 a L Cos[theta3] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + \ L Sin[theta1[t]] Sin[theta2[t]]) - (2 b L Sin[theta3] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 4, -(d mw Cos[theta3] (w3 - (L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t] ) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2, -(d mw Sin[theta3] (w3 - (L (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] \ Sin[theta2[t]]) w2[t] ) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]))) / 2, 0, (d mw (2 vy Cos[theta3] + 2 vz Sin[theta3] + (a L Cos[theta3] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + L Sin[theta1[t]] Sin[theta2[t]]) + (b L Sin[theta3] (-(Cos[theta1[t]] Cos[theta2[t]]) + Sin[theta1[t]] Sin[theta2[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[theta2[t]] + 2 L m (5 + 3 \ Cos[theta2[t]]) L Sin[theta1[t]] Sin[theta2[t]]))) / \ 4}}[{{---------------------------, 3 2 2 2 L m (2 + 3 Cos[theta2[t]]) L m (2 + 3 Cos[theta2[t]]) L m ---------------------------}, {---------------------------, \ ----}}, 6 6 3 {{{1}}, {{1}}}, {theta1[t], theta2[t]}, theta2[t] \ theta2[t] {-(Cos[theta1[t] + theta2[t]] Sec[theta1[t] + ---------] \ Sec[---------] w2[t]) / 2 2 theta2[\ t] 2, w2[t]}] . {{-(Cos[theta1[t] + theta2[t]] Sec[theta1[t] + \ ---------] 2 theta2[t] Sec[---------]) / 2}, {1}} + 2 2 (L m Cos[theta1[t]] (Cos[theta1[t]] + 3 Cos[theta1[t] - theta2[t]] - 13 Cos[theta1[t] + theta2[t]] - 3 Cos[theta1[t] + 2 theta2[t]]) theta2[t] 3 theta2[t] 3 Sec[theta1[t] + ---------] Sec[---------] Sin[theta1[t] + \ theta2[t]] w2[t]) / 96 2 2 2 + (L m Cos[theta1[t] + theta2[t]] (-Cos[theta1[t]] - 3 Cos[theta1[t] - theta2[t]] + 13 Cos[theta1[t] \ + theta2[t]] + theta2[t] 4 \ theta2[t] 2 3 Cos[theta1[t] + 2 theta2[t]]) Sec[theta1[t] + ---------] \ Sec[---------] 2 \ 2 theta2[t] Tan[---------] w2[t]) / 96) + 2 2 (L m (7 - Cos[2 theta1[t]] - 3 Cos[2 theta2[t]] + 5 Cos[2 (theta1[t] + \ theta2[t])]) theta2[t] 2 theta2[t] 2 Sec[theta1[t] + ---------] Sec[---------] w2'[t]) / 48 == 0} 2 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Notice that there are two kinematic equations and one dynamic equation. \ Since L1=L2=L it is easy to show that theta2=-2 theta1. Hence we can make \ this replacement to obtain:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["SpEqns=Simplify[Eqns/.theta2[t]->-2*theta1[t]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][theta1][t] == -w2[t]/2, Derivative[1][theta2][t] == \ w2[t], -(gc*L*m*Sin[theta1[t]]) + {{-1/2, 1}} . {{mw*(a*vz - d*vy*Cos[theta3] + d*vy*Cos[2*theta1[t]] - \ d*vz*Sin[theta3] - d*vz*Sin[2*theta1[t]]), -((m + 2*mw)*vz), (m + 2*mw)*vy, (d*mw*(a*Cos[2*theta1[t]] + b*Sin[2*theta1[t]])* (w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2, -(d*mw*(a*Cos[theta3] + b*Sin[theta3])* (w3 - (2*L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2}, {-((d*L*mw*(Cos[theta3] - Cos[2*theta1[t]])* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])), 0, (L*(m + 2*mw)*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]), -(d*mw*Cos[2*theta1[t]]*(w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]))), d*mw*Cos[theta3]*(w3 - (2*L* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t]) /(-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]))}, {(L*mw*(a - d*Sin[theta3] - d*Sin[2*theta1[t]])* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]), -((L*(m + 2*mw)*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])), 0, d*mw*Sin[2*theta1[t]]*(w2[t] - (2*L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])), d*mw*Sin[theta3]*(w3 - (2*L* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t]) /(-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]))}, {(d*mw*(2*vy*Cos[2*theta1[t]] - 2*vz*Sin[2*theta1[t]] - \ a*Cos[2*theta1[t]]*w2[t] - b*Sin[2*theta1[t]]*w2[t] + (2*a*L*Cos[2*theta1[t]]* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]) + (2*b*L*Sin[2*theta1[t]]* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/4, (d*mw*Cos[2*theta1[t]]*(w2[t] - (L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2, -(d*mw*Sin[2*theta1[t]]*(w2[t] - (L*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2, (d*mw*(-2*vy*Cos[2*theta1[t]] + 2*vz*Sin[2*theta1[t]] - (a*L*Cos[2*theta1[t]]*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]) - (b*L*Sin[2*theta1[t]]*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + b*w3*Sin[theta3] - (2*a*L*Cos[theta3]* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]) - (2*b*L*Sin[theta3]*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/4, -(d*mw*Cos[theta3]*(w3 - (L* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2, -(d*mw*Sin[theta3]*(w3 - (L* (-(Cos[theta1[t]]*Cos[2*theta1[t]]) - \ Sin[theta1[t]]*Sin[2*theta1[t]])* w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/2, 0, (d*mw*(2*vy*Cos[theta3] + 2*vz*Sin[theta3] + (a*L*Cos[theta3]*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]]) + (b*L*Sin[theta3]*(-(Cos[theta1[t]]*Cos[2*theta1[t]]) - Sin[theta1[t]]*Sin[2*theta1[t]])*w2[t])/ (-(L*Cos[theta1[t]]) - L*Cos[theta1[t]]*Cos[2*theta1[t]] - L*Sin[theta1[t]]*Sin[2*theta1[t]])))/4}}[{{(L^2*m* (5 + 3*Cos[2*theta1[t]]))/3, (L^2*m*(2 + 3*Cos[2*theta1[t]]))/6}, \ {(L^2*m*(2 + 3*Cos[2*theta1[t]]))/6, (L^2*m)/3}}, {{{1}}, {{1}}}, {theta1[t], -2*theta1[t]}, {-w2[t]/2, w2[t]}] . {{-1/2}, {1}}*w2[t] + (L^2*m*(5 - 3*Cos[2*theta1[t]])*Derivative[1][w2][t])/12 == 0} \ \>", "\<\ -w2[t] {theta1'[t] == ------, theta2'[t] == w2[t], 2 1 -(gc L m Sin[theta1[t]]) + {{-(-), 1}} . 2 {{mw (a vz - d vy Cos[theta3] + d vy Cos[2 theta1[t]] - d vz \ Sin[theta3] - d vz Sin[2 theta1[t]]), -((m + 2 mw) vz), (m + 2 mw) vy, (d mw (a Cos[2 theta1[t]] + b Sin[2 theta1[t]]) (w2[t] - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2, -(d mw (a Cos[theta3] + b Sin[theta3]) (w3 - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2}, {-((d L mw (Cos[theta3] - Cos[2 theta1[t]]) (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t])\\ / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]])), 0, (L (m + 2 mw) (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]), -(d mw Cos[2 theta1[t]] (w2[t] - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))), d mw Cos[theta3] (w3 - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))}, {(L mw (a - d Sin[theta3] - d Sin[2 theta1[t]]) (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]), -((L (m + 2 mw) (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]])), 0, d mw Sin[2 theta1[t]] (w2[t] - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]])), d mw Sin[theta3] (w3 - (2 L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))}, {(d mw (2 vy Cos[2 theta1[t]] - 2 vz Sin[2 theta1[t]] - a Cos[2 \ theta1[t]] w2[t] - b Sin[2 theta1[t]] w2[t] + (2 a L Cos[2 theta1[t]] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]) + (2 b L Sin[2 theta1[t]] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 4, (d mw Cos[2 theta1[t]] (w2[t] - (L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2, -(d mw Sin[2 theta1[t]] (w2[t] - (L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2, (d mw (-2 vy Cos[2 theta1[t]] + 2 vz Sin[2 theta1[t]] - (a L Cos[2 theta1[t]] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]) - (b L Sin[2 theta1[t]] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 4, 0}, {(d mw (-2 vy Cos[theta3] + a w3 Cos[theta3] - 2 vz Sin[theta3] + b w3 Sin[theta3] - (2 a L Cos[theta3] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]) - (2 b L Sin[theta3] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 4, -(d mw Cos[theta3] (w3 - (L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2, -(d mw Sin[theta3] (w3 - (L (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 \ theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 2, 0, (d mw (2 vy Cos[theta3] + 2 vz Sin[theta3] + (a L Cos[theta3] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]) + (b L Sin[theta3] (-(Cos[theta1[t]] Cos[2 theta1[t]]) - Sin[theta1[t]] Sin[2 theta1[t]]) w2[t]) / (-(L Cos[theta1[t]]) - L Cos[theta1[t]] Cos[2 theta1[t]] - L Sin[theta1[t]] Sin[2 theta1[t]]))) / 4}}[{{ 2 2 L m (5 + 3 Cos[2 theta1[t]]) L m (2 + 3 Cos[2 theta1[t]]) -----------------------------, -----------------------------}, 3 6 2 2 L m (2 + 3 Cos[2 theta1[t]]) L m {-----------------------------, ----}}, {{{1}}, {{1}}}, {theta1[t], \ -2 theta1[t]}, 6 3 2 -w2[t] 1 L m (5 - 3 Cos[2 theta1[t]]) \ w2'[t] {------, w2[t]}] . {{-(-)}, {1}} w2[t] + \ ------------------------------------ == 0} 2 2 12\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Simple trigonometric identities can be used to verify that these equations \ are equivalent to those given by Ginsberg for this example."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Another Closed Chain"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Consider a two bar linkage in the x-y plane. The left end of bar 1 is \ constrained to slide on the x-axis and the right end of bar two slides on the \ y-axis. The right end of bar 1 and the left end of bar 2 are connected by a \ revolute joint."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "r1={1,1};\nH1=Transpose[{{0,0,1,0,0,0},{0,0,0,1,0,0}}];\n\ q1={theta1,x};p1={w1,vx};\n\nr2={1};\nH2=Transpose[{{0,0,1,0,0,0}}];\n\ q2={theta2};p2={w2};\n\nJointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Mass1=m1; CenterOfMass1={L1/2,0,0}; OutboardNode1={2,{L1,0,0}};\n\ InertiaMatrix1=DiagonalMatrix[{0,m1*L1^2/2,m1*L1^2/2}];\n\nMass2=m2; \ CenterOfMass2={L2/2,0,0}; OutboardNode2={3,{L2,0,0}};\n\ InertiaMatrix2=DiagonalMatrix[{0,m2*L2^2/2,m2*L2^2/2}];\n\n\ BodyList={{CenterOfMass1,{OutboardNode1},Mass1,InertiaMatrix1},\n \ {CenterOfMass2,{OutboardNode2},Mass2,InertiaMatrix2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["TreeList={{{1,1},{2,2}}};\nPE=0; Q={0,0,0};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cp,Fp,p,q}=CreateModelSim[JointList,BodyList,TreeList,g,PE,Q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing joint 2 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["DeleteFile[\"Linkage.dat\"];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ DeleteFile::nffil: File not found during DeleteFile[Linkage.dat]. \ \>", "\<\ DeleteFile::nffil: File not found during DeleteFile[Linkage.dat].\ \ \>"], "Message", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData["Save[\"Linkage.dat\",p,q,V,X,H,Cp,Fp,M];\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["<Infinity, AspectRatioFixed->True], Cell[TextData[ "ChnBodyList={{CenterOfMass1,{L1,0,0},Mass1,InertiaMatrix1},\n \ {CenterOfMass2,{L2,0,0},Mass2,InertiaMatrix2}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["EndPos=EndEffector[ChnBodyList,X];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["G=EndPos[[{1},4]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {x + L1*Cos[theta1] + L2*(Cos[theta1]*Cos[theta2] - \ Sin[theta1]*Sin[theta2])} \ \>", "\<\ {x + L1 Cos[theta1] + L2 (Cos[theta1] Cos[theta2] - Sin[theta1] \ Sin[theta2])}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Since G=0 always has a solution for x, we will eliminate x from the \ constrained equations."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{Mm,Cm,Fm,Vm,phat,qhat}=AlgConstrainedSys[M,Cp,Fp,V,G,p,q,{2}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{{(3*L1^2*m1)/4 + (L2^2*m1)/2 + (L1^2*m2)/2 + (3*L2^2*m2)/4 + \ (L1^2*m2*Cos[2*theta1])/2 + (L1*L2*m1*Cos[theta2])/2 + (L1*L2*m2*Cos[theta2])/2 - (L2^2*m1*Cos[2*(theta1 + theta2)])/2 - (L1*L2*m1*Cos[2*theta1 + \ theta2])/2 + (L1*L2*m2*Cos[2*theta1 + theta2])/2, (L2*(2*L2*m1 + 3*L2*m2 + L1*m1*Cos[theta2] + L1*m2*Cos[theta2] - 2*L2*m1*Cos[2*(theta1 + theta2)] - L1*m1*Cos[2*theta1 + theta2] + L1*m2*Cos[2*theta1 + theta2]))/4}, {(L2*(2*L2*m1 + 3*L2*m2 + L1*m1*Cos[theta2] + L1*m2*Cos[theta2] - 2*L2*m1*Cos[2*(theta1 + theta2)] - L1*m1*Cos[2*theta1 + theta2] + L1*m2*Cos[2*theta1 + theta2]))/4, (L2^2*(2*m1 + 3*m2 - 2*m1*Cos[2*(theta1 + theta2)]))/4}}, {{{{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + \ theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, \ (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/ 4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + (3*L2^2*m2)/4 + \ L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, (L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + L2*w2*Sin[theta1 \ + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L1*w1*Cos[theta1] + L2*w1*Cos[theta1 + theta2] + L2*w2*Cos[theta1 + \ theta2])* ((L1*m1*Sin[theta1])/2 + L2*m1*Sin[theta1 + theta2] + (L2*m2*Sin[theta1 \ + theta2])/2)\\ , {{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, \ (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/ 4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + (3*L2^2*m2)/4 + \ L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, (L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + L2*w2*Sin[theta1 \ + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + L2*(w1 + w2)*Cos[theta1 + theta2]* ((L1*m1*Sin[theta1])/2 + L2*m1*Sin[theta1 + theta2] + (L2*m2*Sin[theta1 \ + theta2])/2)} , {{{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, \ (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/ 4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + (3*L2^2*m2)/4 + \ L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, (L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + L2*w2*Sin[theta1 \ + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L2*(2*m1 + m2)*(L1*w1*Cos[theta1] + L2*w1*Cos[theta1 + theta2] + L2*w2*Cos[theta1 + theta2])*Sin[theta1 + theta2])/2, {{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + \ theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, \ (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/ 4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + (3*L2^2*m2)/4 + \ L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, (L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + L2*w2*Sin[theta1 \ + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L2^2*(2*m1 + m2)*(w1 + w2)*Sin[2*(theta1 + theta2)])/4}}, {0, 0}, {{1, \ 0}, {0, 1}}, {w1, w2}, {theta1, theta2}} \ \>", "\<\ 2 2 2 2 2 3 L1 m1 L2 m1 L1 m2 3 L2 m2 L1 m2 Cos[2 theta1] L1 L2 m1 \ Cos[theta2] {{{-------- + ------ + ------ + -------- + -------------------- + \ -------------------- + 4 2 2 4 2 2 2 L1 L2 m2 Cos[theta2] L2 m1 Cos[2 (theta1 + theta2)] -------------------- - ------------------------------- - 2 2 L1 L2 m1 Cos[2 theta1 + theta2] L1 L2 m2 Cos[2 theta1 + theta2] ------------------------------- + -------------------------------, 2 2 (L2 (2 L2 m1 + 3 L2 m2 + L1 m1 Cos[theta2] + L1 m2 Cos[theta2] - 2 L2 m1 Cos[2 (theta1 + theta2)] - L1 m1 Cos[2 theta1 + theta2] + L1 m2 Cos[2 theta1 + theta2])) / 4}, {(L2 (2 L2 m1 + 3 L2 m2 + L1 m1 Cos[theta2] + L1 m2 Cos[theta2] - 2 L2 m1 Cos[2 (theta1 + theta2)] - L1 m1 Cos[2 theta1 + theta2] + L1 m2 Cos[2 theta1 + theta2])) / 4, 2 L2 (2 m1 + 3 m2 - 2 m1 Cos[2 (theta1 + theta2)]) -------------------------------------------------}}, 4 {{{{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + \ theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) ---------------------------------------------------------------------\ ------------, 4 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, \ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ---------------------------------------------------------------------\ -----------}}[ 4 2 2 3 L1 m1 2 3 L2 m2 {{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 2\ L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 L2 \ m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}}\\ + (L1 w1 Cos[theta1] + L2 w1 Cos[theta1 + theta2] + L2 w2 Cos[theta1 + \ theta2]) L1 m1 Sin[theta1] L2 m2 Sin[theta1 + \ theta2] (----------------- + L2 m1 Sin[theta1 + theta2] + \ --------------------------), 2 2 {{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + \ theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) ---------------------------------------------------------------------\ ------------, 4 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, \ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ---------------------------------------------------------------------\ -----------}}[ 4 2 2 3 L1 m1 2 3 L2 m2 {{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 2\ L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 L2 \ m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}}\\ + L2 (w1 + w2) Cos[theta1 + theta2] L1 m1 Sin[theta1] L2 m2 Sin[theta1 + \ theta2] (----------------- + L2 m1 Sin[theta1 + theta2] + \ --------------------------)}, 2 2 {{{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + \ theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) ---------------------------------------------------------------------\ ------------, 4 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, \ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ---------------------------------------------------------------------\ -----------}}[ 4 2 2 3 L1 m1 2 3 L2 m2 {{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 2\ L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 L2 \ m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}}\\ + (L2 (2 m1 + m2) (L1 w1 Cos[theta1] + L2 w1 Cos[theta1 + theta2] + L2 w2 Cos[theta1 + theta2]) Sin[theta1 + theta2]) / 2, {{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + \ theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) ---------------------------------------------------------------------\ ------------, 4 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, \ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ---------------------------------------------------------------------\ -----------}}[ 4 2 2 3 L1 m1 2 3 L2 m2 {{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 2\ L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 L2 \ m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}}\\ 2 L2 (2 m1 + m2) (w1 + w2) Sin[2 (theta1 + theta2)] + --------------------------------------------------}}, {0, 0}, {{1, \ 0}, {0, 1}}, 4 {w1, w2}, {theta1, theta2}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Simplify[Cm.phat]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {w2*({{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, \ L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - b*w1*Sin[theta3]))/4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + \ (3*L2^2*m2)/4 + L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - \ (L2*m2*Sin[theta1 + theta2])/2, (L2*m2*(3*L2 + \ 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + \ L2*w2*Sin[theta1 + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + L2*(w1 + w2)*Cos[theta1 + theta2]* ((L1*m1*Sin[theta1])/2 + L2*m1*Sin[theta1 + theta2] + \ (L2*m2*Sin[theta1 + theta2])/2) ) + w1*({{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - b*w1*Sin[theta3]))/4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + \ (3*L2^2*m2)/4 + L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - \ (L2*m2*Sin[theta1 + theta2])/2, (L2*m2*(3*L2 + \ 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + \ L2*w2*Sin[theta1 + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L1*w1*Cos[theta1] + L2*w1*Cos[theta1 + theta2] + L2*w2*Cos[theta1 + \ theta2])* ((L1*m1*Sin[theta1])/2 + L2*m1*Sin[theta1 + theta2] + \ (L2*m2*Sin[theta1 + theta2])/2) ), w1*({{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - b*w1*Sin[theta3]))/4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + \ (3*L2^2*m2)/4 + L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - \ (L2*m2*Sin[theta1 + theta2])/2, (L2*m2*(3*L2 + \ 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + \ L2*w2*Sin[theta1 + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L2*(2*m1 + m2)*(L1*w1*Cos[theta1] + L2*w1*Cos[theta1 + theta2] + L2*w2*Cos[theta1 + theta2])*Sin[theta1 + theta2])/2) + w2*({{1, L1*Sin[theta1] + L2*Sin[theta1 + theta2], 0}, {0, L2*Sin[theta1 + theta2], 1}} . {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - b*w1*Sin[theta2]))/4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - b*w1*Sin[theta3]))/4}}[{{(3*L1^2*m1)/4 + L1^2*m2 + \ (3*L2^2*m2)/4 + L1*L2*m2*Cos[theta2], -(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - \ (L2*m2*Sin[theta1 + theta2])/2, (L2*m2*(3*L2 + \ 2*L1*Cos[theta2]))/4}, {-(L1*m1*Sin[theta1])/2 - L1*m2*Sin[theta1] - (L2*m2*Sin[theta1 + \ theta2])/2, m1 + m2, -(L2*m2*Sin[theta1 + theta2])/2}, {(L2*m2*(3*L2 + 2*L1*Cos[theta2]))/4, -(L2*m2*Sin[theta1 + \ theta2])/2, (3*L2^2*m2)/4}}, {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1*Cos[theta1]) - L2*Cos[theta1 + theta2], theta2}, {w1, L1*w1*Sin[theta1] + L2*w1*Sin[theta1 + theta2] + \ L2*w2*Sin[theta1 + theta2], w2}] . {{1, 0}, {L1*Sin[theta1] + L2*Sin[theta1 + theta2], L2*Sin[theta1 + theta2]}, {0, 1}} + (L2^2*(2*m1 + m2)*(w1 + w2)*Sin[2*(theta1 + theta2)])/4)} \ \>", "\<\ {w2 ({{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 \ Sin[theta1 + theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / \ 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 Sin[theta2]) --------------------------------------------------------------------\ -------------\\ 4 , 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / \ 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) --------------------------------------------------------------------\ ------------}} 4 2 2 3 L1 m1 2 3 L2 m2 [{{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 \ 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 \ L2 m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 \ 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}} + L2 (w1 + w2) Cos[theta1 + theta2] L1 m1 Sin[theta1] L2 m2 Sin[theta1 + \ theta2] (----------------- + L2 m1 Sin[theta1 + theta2] + \ --------------------------)) + 2 2 w1 ({{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / \ 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 Sin[theta2]) --------------------------------------------------------------------\ -------------\\ 4 , 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / \ 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) --------------------------------------------------------------------\ ------------}} 4 2 2 3 L1 m1 2 3 L2 m2 [{{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 \ 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 \ L2 m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 \ 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}} + (L1 w1 Cos[theta1] + L2 w1 Cos[theta1 + theta2] + L2 w2 Cos[theta1 \ + theta2]) L1 m1 Sin[theta1] L2 m2 Sin[theta1 + \ theta2] (----------------- + L2 m1 Sin[theta1 + theta2] + \ --------------------------)), 2 2 w1 ({{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + \ theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / \ 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 Sin[theta2]) --------------------------------------------------------------------\ -------------\\ 4 , 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / \ 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) --------------------------------------------------------------------\ ------------}} 4 2 2 3 L1 m1 2 3 L2 m2 [{{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 \ 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 \ L2 m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 \ 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}} + (L2 (2 m1 + m2) (L1 w1 Cos[theta1] + L2 w1 Cos[theta1 + theta2] + L2 w2 Cos[theta1 + theta2]) Sin[theta1 + theta2]) / 2) + w2 ({{1, L1 Sin[theta1] + L2 Sin[theta1 + theta2], 0}, {0, L2 Sin[theta1 + theta2], 1}} . {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz Sin[theta3]), -((2 mw + m1) vz), (2 mw + m1) vy, d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz Sin[theta2] + 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / \ 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 Sin[theta2]) --------------------------------------------------------------------\ -------------\\ 4 , 0}, {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 \ Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / \ 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) --------------------------------------------------------------------\ ------------}} 4 2 2 3 L1 m1 2 3 L2 m2 [{{-------- + L1 m2 + -------- + L1 L2 m2 Cos[theta2], 4 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] -------------------- - L1 m2 Sin[theta1] - \ --------------------------, 2 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -------------------------------}, 4 -(L1 m1 Sin[theta1]) L2 m2 Sin[theta1 + \ theta2] {-------------------- - L1 m2 Sin[theta1] - \ --------------------------, m1 + m2, 2 2 -(L2 m2 Sin[theta1 + theta2]) -----------------------------}, 2 \ 2 L2 m2 (3 L2 + 2 L1 Cos[theta2]) -(L2 m2 Sin[theta1 + theta2]) 3 \ L2 m2 {-------------------------------, -----------------------------, \ --------}}, 4 2 \ 4 {{{1, 0}, {0, 1}}, {{1}}}, {theta1, -(L1 Cos[theta1]) - L2 Cos[theta1 \ + theta2], theta2}, {w1, L1 w1 Sin[theta1] + L2 w1 Sin[theta1 + theta2] + L2 w2 Sin[theta1 + theta2], w2}] . {{1, 0}, {L1 Sin[theta1] + L2 Sin[theta1 + theta2], L2 Sin[theta1 + \ theta2]}, {0, 1}} 2 L2 (2 m1 + m2) (w1 + w2) Sin[2 (theta1 + theta2)] + --------------------------------------------------)} 4\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "The Rolling Disk: a Classic Example of Differential Constraints"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "As an illustration we take an example from Neimark and Fufaev. Consider a \ disk that rolls without slipping on the x-y plane (z is up). Assume that the \ disk is of mass m and radius R. One approach is to ignore the the rolling \ constraints and formulate the equations of motion for the free disk in space, \ and then add the required constraints. A body frame is established with \ origin at the center of the disk. The six degree of freedom, simple joint is \ defined by: "], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["r={6};\nH=IdentityMatrix[6]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, 0, 0, \ 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}} \ \>", "\<\ {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0}, {0, \ 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "q={psi,theta,phi,x,y,z};\np={wx,wy,wz,vx,vy,vz};\nJointList={{r,H,q,p}};\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "In this setup the x,y,z coordinates locate the center of the disk in the \ space frame and psi, theta, phi are Euler angles in the z-x-y (or 3-2-1) \ convention. \n\nThe body data is:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Mass=m; CenterOfMass={0,0,0}; OutboardNode={2,{0,-R*Sin[psi],-R*Cos[psi]}};\n\ InertiaMatrix=DiagonalMatrix[{J,Iy,Iy}];\n\ BodyList={{CenterOfMass,{OutboardNode},Mass,InertiaMatrix}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["and the remaining data:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["TreeList={{{1,1}}};\nPE=0; Q={0,0,0,0,0,0};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["The unconstrained disk model is obtained with"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,M,Cp,Fp,p,q}=CreateModelSim[JointList,BodyList,TreeList,g,PE,Q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Joint Kinematics Computing joint 1 kinematics Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["DeleteFile[\"disk2.dat\"];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ DeleteFile::nffil: File not found during DeleteFile[disk2.dat]. \ \>", "\<\ DeleteFile::nffil: File not found during DeleteFile[disk2.dat].\ \ \>"], "Message", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData["Save[\"disk2.dat\",p,q,V,X,H,Cp,Fp,M];\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Now, we formulate the differential constraints. Rolling without slipping \ implies that the velocity of the disk contact point must be zero. An \ expression for the (angular and translational) velocity at the outboard node \ as a function of the configuration variables is easily obtained using the \ function EndEffectorVelocity."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["<Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{ChainList}=TreeList;\nVCont=EndEffectorVelocity[ChainList,BodyList,X,H,p]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"ChainList\" is similar to existing symbol \"ChainLst\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"ChainList\" is similar to existing symbol \"ChainLst\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ {wx, wy, wz, vx - R*wy*Cos[psi] + R*wz*Sin[psi], vy + R*wx*Cos[psi], vz \ - R*wx*Sin[psi]} \ \>", "\<\ {wx, wy, wz, vx - R wy Cos[psi] + R wz Sin[psi], vy + R wx \ Cos[psi], vz - R wx Sin[psi]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "The rolling constraint requires that the translation velocity of the contact \ point (the last three of the elements of the six-vector VCont) be zero. Thus, \ we obtain the constrained system:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "{Mm,Cm,Fm,Vm,T,phat}=DiffConstrainedSys[M,Cp,Fp,V,VCont[[Range[4,6]]],p,q,{4,\ 5,6}];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "The dynamics are reduced to three dimensions and the original six \ quasi-velocities are reduced to three, in fact, we have:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["phat"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {wx, wy, wz} \ \>", "\<\ {wx, wy, wz}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "The set of configuration coordinates is not reduced by the function \ DiffConstrainedSys. In general, a set of differential constraints may not \ admit any such reduction. Such would be the case if the constraints were \ completely nonholonomic. In the present case, however, the constraints are \ \"partially\" integrable and from basic geometry one can see that (cg) hight \ = Rcos(theta)-R. Using this relationship, the coordinate z can be eliminated \ from the equations."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["{Sols}=Solve[{X[[1]][[3,4]]==R*(Cos[theta]-1)},{z}]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"Sols\" is similar to existing symbol \"sols\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"Sols\" is similar to existing symbol \"sols\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData["\<\ {{z -> -(R*(1 - Cos[theta]))}} \ \>", "\<\ {{z -> -(R (1 - Cos[theta]))}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Because the translation parameters x,y,z are, in fact, the space \ coordinates, z is precisely the hight of the center of gravity. Inspection \ shows that only Mm and Vm depend on z so we define:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["Mmo=Simplify[Mm/.Sols];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Vmo=Simplify[Vm[[ {1,2,3,4,5} ]]/.Sols];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Now, we assemble the governing equations."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Eqns=MakeODEs[phat,q[[{1,2,3,4,5}]],Vmo,Mmo,Cm,Fm,t]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][psi][t] == wx[t] + Sin[psi[t]]*Tan[theta[t]]*wy[t] + Cos[psi[t]]*Tan[theta[t]]*wz[t], Derivative[1][theta][t] == Cos[psi[t]]*wy[t] - Sin[psi[t]]*wz[t], Derivative[1][phi][t] == Sec[theta[t]]*Sin[psi[t]]*wy[t] + Cos[psi[t]]*Sec[theta[t]]*wz[t], Derivative[1][x][t] == R*Sin[phi[t]]*wx[t] + R*Cos[phi[t]]*Cos[psi[t]]*Cos[theta[t]]*wy[t] - R*Cos[phi[t]]*Cos[theta[t]]*Sin[psi[t]]*wz[t], Derivative[1][y][t] == -(R*Cos[phi[t]]*wx[t]) + R*Cos[psi[t]]*Cos[theta[t]]*Sin[phi[t]]*wy[t] - R*Cos[theta[t]]*Sin[phi[t]]*Sin[psi[t]]*wz[t], {{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, \ 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] + \ b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}} [{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], \ Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], -(R*Sin[psi[t]])}, \ {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wx[t] + {{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], \ 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] + \ b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}} [{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], \ Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], -(R*Sin[psi[t]])}, \ {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wy[t] + {{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], \ 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] + \ b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}} [{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], \ Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], -(R*Sin[psi[t]])}, \ {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wz[t] + (J + m*R^2)*Derivative[1][wx][t] == 0, gc*m*R*Cos[psi[t]]*Sin[theta[t]] + wx[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} + m*R*Cos[psi[t]]*(-(R*Sin[psi[t]]*wy[t]) - R*Cos[psi[t]]*wz[t])) + wz[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} + m*R*Cos[psi[t]]^2*Tan[theta[t]]*(-(R*Sin[psi[t]]*wy[t]) - \ R*Cos[psi[t]]*wz[t])) + wy[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} + m*R*Cos[psi[t]]*Sin[psi[t]]*Tan[theta[t]]* (-(R*Sin[psi[t]]*wy[t]) - R*Cos[psi[t]]*wz[t])) + (Iy + m*R^2*Cos[psi[t]]^2)*Derivative[1][wy][t] - (m*R^2*Sin[2*psi[t]]*Derivative[1][wz][t])/2 == 0, -(gc*m*R*Sin[psi[t]]*Sin[theta[t]]) + wx[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} - m*R*Sin[psi[t]]*(-(R*Sin[psi[t]]*wy[t]) - R*Cos[psi[t]]*wz[t])) + wz[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} - m*R*Cos[psi[t]]*Sin[psi[t]]*Tan[theta[t]]* (-(R*Sin[psi[t]]*wy[t]) - R*Cos[psi[t]]*wz[t])) + wy[t]*({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + \ 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R*Cos[psi[t]]*wy[t] - R*Sin[psi[t]]*wz[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}} - m*R*Sin[psi[t]]^2*Tan[theta[t]]*(-(R*Sin[psi[t]]*wy[t]) - \ R*Cos[psi[t]]*wz[t])) - (m*R^2*Sin[2*psi[t]]*Derivative[1][wy][t])/2 + (Iy + m*R^2*Sin[psi[t]]^2)*Derivative[1][wz][t] == 0} \ \>", "\<\ {psi'[t] == wx[t] + Sin[psi[t]] Tan[theta[t]] wy[t] + Cos[psi[t]] \ Tan[theta[t]] wz[t], theta'[t] == Cos[psi[t]] wy[t] - Sin[psi[t]] wz[t], phi'[t] == Sec[theta[t]] Sin[psi[t]] wy[t] + Cos[psi[t]] Sec[theta[t]] \ wz[t], x'[t] == R Sin[phi[t]] wx[t] + R Cos[phi[t]] Cos[psi[t]] Cos[theta[t]] \ wy[t] - R Cos[phi[t]] Cos[theta[t]] Sin[psi[t]] wz[t], y'[t] == -(R Cos[phi[t]] wx[t]) + R Cos[psi[t]] Cos[theta[t]] Sin[phi[t]] \ wy[t] - R Cos[theta[t]] Sin[phi[t]] Sin[psi[t]] wz[t], {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, \ 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] + \ b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] Sin[psi[t]] \ wx[t])) / 4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] Sin[theta[t]], \ Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R Sin[psi[t]])}, \ {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wx[t] + {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], \ 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] + \ b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] Sin[psi[t]] \ wx[t])) / 4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] Sin[theta[t]], \ Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R Sin[psi[t]])}, \ {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wy[t] + {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], \ 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] + \ b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] Sin[psi[t]] \ wx[t])) / 4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] Sin[theta[t]], \ Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R Sin[psi[t]])}, \ 2 {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wz[t] + (J + m R ) \ wx'[t] == 0, gc m R Cos[psi[t]] Sin[theta[t]] + wx[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} + m R Cos[psi[t]] (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] wz[t])) + wz[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} + 2 m R Cos[psi[t]] Tan[theta[t]] (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] \ wz[t])) + wy[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} + m R Cos[psi[t]] Sin[psi[t]] Tan[theta[t]] 2 \ 2 (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] wz[t])) + (Iy + m R \ Cos[psi[t]] ) wy'[t] - 2 m R Sin[2 psi[t]] wz'[t] ------------------------- == 0, -(gc m R Sin[psi[t]] Sin[theta[t]]) + 2 wx[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} - m R Sin[psi[t]] (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] wz[t])) + wz[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} - m R Cos[psi[t]] Sin[psi[t]] Tan[theta[t]] (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] wz[t])) + wy[t] ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] \ wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] \ Cos[psi[t]] wx[t] - 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] Sin[theta[t]]}, \ {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}}} , {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], wy[t], wz[t], R Cos[psi[t]] wy[t] - R Sin[psi[t]] wz[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} - 2 m R Sin[psi[t]] Tan[theta[t]] (-(R Sin[psi[t]] wy[t]) - R Cos[psi[t]] \ wz[t])) - 2 m R Sin[2 psi[t]] wy'[t] 2 2 ------------------------- + (Iy + m R Sin[psi[t]] ) wz'[t] == 0} 2 \ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["Save[\"disk2.dat\",p,q,V,X,H,Cp,Fp,M,Eqns];\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "Since x,y,z locate the center of gravity it is of interest to compute the \ contact point."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "ChnBodyList={{CenterOfMass,{0,-R*Sin[psi],-R*Cos[psi]},Mass,InertiaMatrix}};\ "], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "{ContactPoint}=Simplify[Transpose[EndEffector[ChnBodyList,X][[{1,2,3},{4}]]]/\ .Sols]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {{x + (R*Sin[phi - theta])/2 - (R*Sin[phi + theta])/2, y - (R*Cos[phi - theta])/2 + (R*Cos[phi + theta])/2, -R}} \ \>", "\<\ R Sin[phi - theta] R Sin[phi + theta] R Cos[phi - \ theta] R Cos[phi + theta] {{x + ------------------ - ------------------, y - ------------------ + \ ------------------, 2 2 2 \ 2 -R}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "Consider the equations of motion as given by the list Eqns. Careful \ inspection of the equations suggests that representation of the angular \ velocity in a fame that does not rotate about the body-x axis may simplify \ them. Thus, we transform the angular velocity coordinates, wy, wz, via the \ relations:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "a1=wyy[t]==Cos[psi[t]]*wy[t]-Sin[psi[t]]*wz[t]\n\ a2=wzz[t]==Sin[psi[t]]*wy[t]+Cos[psi[t]]*wz[t]\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"wyy\" is similar to existing symbol \"Wyy\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \"wyy\" is similar to existing symbol \"Wyy\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData["\<\ wyy[t] == Cos[psi[t]]*wy[t] - Sin[psi[t]]*wz[t] \ \>", "\<\ wyy[t] == Cos[psi[t]] wy[t] - Sin[psi[t]] wz[t]\ \>"], "Output", PageWidth->Infinity, Evaluatable->False], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"wzz\" is similar to existing symbol \"Wzz\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \"wzz\" is similar to existing symbol \"Wzz\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData["\<\ wzz[t] == Sin[psi[t]]*wy[t] + Cos[psi[t]]*wz[t] \ \>", "\<\ wzz[t] == Sin[psi[t]] wy[t] + Cos[psi[t]] wz[t]\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "The inverse transformation rules are obtained using Mathematica's Solve \ function"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Trans=Simplify[Flatten[Solve[{a1,a2},{wy[t],wz[t]}]]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {wy[t] -> Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], wz[t] -> -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t]} \ \>", "\<\ {wy[t] -> Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], wz[t] -> -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["<Infinity, AspectRatioFixed->True], Cell[TextData[ "The transformed equations are obtained with the function \ StateTransformation:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["StateVars={psi,theta,phi,x,y,wx,wy,wz}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {psi, theta, phi, x, y, wx, wy, wz} \ \>", "\<\ {psi, theta, phi, x, y, wx, wy, wz}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "TrEqns=StateTransformation[Eqns,StateVars,Trans,{wyy,wzz},t]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {Derivative[1][theta][t] == wyy[t], Derivative[1][phi][t] == \ Sec[theta[t]]*wzz[t], Derivative[1][x][t] == (R*(2*Sin[phi[t]]*wx[t] + Cos[phi[t] - \ theta[t]]*wyy[t] + Cos[phi[t] + theta[t]]*wyy[t]))/2, Derivative[1][y][t] == -(R*(2*Cos[phi[t]]*wx[t] - Sin[phi[t] - \ theta[t]]*wyy[t] - Sin[phi[t] + theta[t]]*wyy[t]))/2, Derivative[1][wx][t] == -(({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} \ . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4}} [{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], \ -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], \ Cos[psi[t]]*Cos[theta[t]]}} }, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*(wx[t] + Cos[psi[t]]*wyy[t] - \ Sin[psi[t]]*wyy[t] + Cos[psi[t]]*wzz[t] + Sin[psi[t]]*wzz[t]))/(J + m*R^2)), Derivative[1][wyy][t] == (Sec[psi[t]]* (-(Iy*Sin[psi[t]]*wyy[t]) + Iy*Cos[psi[t]]*wzz[t] + \ m*R^2*Cos[psi[t]]*wzz[t])* (-wx[t] - Tan[theta[t]]*wzz[t]))/(Iy + m*R^2) - (Sec[psi[t]]*(-(gc*m*R*Sin[psi[t] - theta[t]]) + gc*m*R*Sin[psi[t] + \ theta[t]] + 2*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4 , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wx[t] + 2*Cos[psi[t]]*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4 , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wyy[t] - 2*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4 , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, \ 0}}*Sin[psi[t]]*wyy[t] + 2*Cos[psi[t]]*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4 , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*wzz[t] + 2*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4 , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, \ 0}}*Sin[psi[t]]*wzz[t] - 2*m*R^2*Cos[psi[t]]*wx[t]*wzz[t] + m*R^2*Sec[theta[t]]*Sin[psi[t] - theta[t]]*wzz[t]^2 - m*R^2*Sec[theta[t]]*Sin[psi[t] + theta[t]]*wzz[t]^2))/(2*(Iy + \ m*R^2)) + (Iy*Tan[psi[t]]*(({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] \ + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - \ d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + \ w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + \ w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t])) /4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t])) /4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t])) /4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, \ 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, \ 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, \ 0, 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], \ x[t], y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*(Cos[psi[t]] + \ Sin[psi[t]])* (wx[t] + Cos[psi[t]]*wyy[t] - Sin[psi[t]]*wyy[t] + \ Cos[psi[t]]*wzz[t] + Sin[psi[t]]*wzz[t]))/Iy + wyy[t]*(-wx[t] - \ Tan[theta[t]]*wzz[t])))/ (Iy + m*R^2), Derivative[1][wzz][t] == -(({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]*wx[t]) + \ d*R*Cos[theta3]*Cos[psi[t]]*wx[t] + a*R*Sin[psi[t]]*wx[t] + d*R*Sin[theta2]*Sin[psi[t]]*wx[t] - d*R*Sin[theta3]*Sin[psi[t]]*wx[t]), -((2*mw + \ m1)*R*Sin[psi[t]]*wx[t]), -((2*mw + m1)*R*Cos[psi[t]]*wx[t]), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, \ -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] + 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]*wx[t] - \ 2*R*Sin[theta2]*Sin[psi[t]]*wx[t]))/4\\ , 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] - 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4, -(d*mw*(w1 + \ w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]*wx[t] + \ 2*R*Sin[theta3]*Sin[psi[t]]*wx[t]))/4} }[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, \ {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]]*Tan[theta[t]], Cos[psi[t]]*Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]]*Cos[theta[t]], -(Cos[psi[t]]*Sin[phi[t]]) + \ Cos[phi[t]]*Sin[psi[t]]*Sin[theta[t]], Sin[phi[t]]*Sin[psi[t]] + \ Cos[phi[t]]*Cos[psi[t]]*Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]]*Sin[phi[t]], Cos[phi[t]]*Cos[psi[t]] + \ Sin[phi[t]]*Sin[psi[t]]*Sin[theta[t]], -(Cos[phi[t]]*Sin[psi[t]]) + \ Cos[psi[t]]*Sin[phi[t]]*Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]]*wyy[t] + Sin[psi[t]]*wzz[t], -(Sin[psi[t]]*wyy[t]) + Cos[psi[t]]*wzz[t], R*wyy[t], \ -(R*Cos[psi[t]]*wx[t]), R*Sin[psi[t]]*wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*(Cos[psi[t]] + Sin[psi[t]])* (wx[t] + Cos[psi[t]]*wyy[t] - Sin[psi[t]]*wyy[t] + \ Cos[psi[t]]*wzz[t] + Sin[psi[t]]*wzz[t]))/Iy) - wyy[t]*(-wx[t] - Tan[theta[t]]*wzz[t]), \ Derivative[1][psi][t] == wx[t] + Tan[theta[t]]*wzz[t]} \ \>", "\<\ {theta'[t] == wyy[t], phi'[t] == Sec[theta[t]] wzz[t], x'[t] == (R (2 Sin[phi[t]] wx[t] + Cos[phi[t] - theta[t]] wyy[t] + Cos[phi[t] + theta[t]] wyy[t])) / 2, y'[t] == -(R (2 Cos[phi[t]] wx[t] - Sin[phi[t] - theta[t]] wyy[t] - Sin[phi[t] + theta[t]] wyy[t])) / 2, wx'[t] == -(({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} \ . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) / 4 , 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4 }}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] Sin[theta[t]], \ -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] \ Cos[theta[t]]}} }, {psi[t], theta[t], phi[t], x[t], y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (wx[t] + Cos[psi[t]] wyy[t] - Sin[psi[t]] \ wyy[t] + 2 Cos[psi[t]] wzz[t] + Sin[psi[t]] wzz[t])) / (J + m R )), wyy'[t] == (Sec[psi[t]] (-(Iy Sin[psi[t]] wyy[t]) + Iy Cos[psi[t]] wzz[t] + \ 2 \ 2 m R Cos[psi[t]] wzz[t]) (-wx[t] - Tan[theta[t]] wzz[t])) / (Iy + m \ R ) - (Sec[psi[t]] (-(gc m R Sin[psi[t] - theta[t]]) + gc m R Sin[psi[t] + \ theta[t]] + 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) \\ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, \ 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wx[t] + 2 Cos[psi[t]] {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) \\ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, \ 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wyy[t] - 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) \\ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, \ 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} Sin[psi[t]] \ wyy[t] + 2 Cos[psi[t]] {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) \\ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, \ 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} wzz[t] + 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) \\ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) \\ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, \ 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} Sin[psi[t]] \ wzz[t] - 2 2 m R Cos[psi[t]] wx[t] wzz[t] + 2 2 m R Sec[theta[t]] Sin[psi[t] - theta[t]] wzz[t] - 2 2 2 m R Sec[theta[t]] Sin[psi[t] + theta[t]] wzz[t] )) / (2 (Iy + m R \ )) + (Iy Tan[psi[t]] (({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] \ + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) \ Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) \ Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) \ Sin[theta3]) / 4, -----------------------------, \ -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, \ 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, \ 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, \ 0, 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], \ x[t], y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[psi[t]] + \ Sin[psi[t]]) (wx[t] + Cos[psi[t]] wyy[t] - Sin[psi[t]] wyy[t] + Cos[psi[t]] \ wzz[t] + Sin[psi[t]] wzz[t])) / Iy + wyy[t] (-wx[t] - Tan[theta[t]] \ wzz[t]))) / 2 (Iy + m R ), wzz'[t] == -(({{1, 0, 0, 0, -(R Cos[psi[t]]), R \ Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} \ . {{mw (-(d R Cos[theta2] Cos[psi[t]] wx[t]) + d R Cos[theta3] \ Cos[psi[t]] wx[t] + a R Sin[psi[t]] wx[t] + d R Sin[theta2] Sin[psi[t]] wx[t] - d R Sin[theta3] Sin[psi[t]] wx[t]), -((2 mw + m1) R \ Sin[psi[t]] wx[t]), -((2 mw + m1) R Cos[psi[t]] wx[t]), (d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2])) / 2, -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) / 2}, {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, \ -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] wx[t] + d mw (w1 + w2) \ Cos[theta2] 2 R Sin[theta2] Sin[psi[t]] wx[t])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] wx[t] - 2 R Sin[theta2] \ Sin[psi[t]] wx[t])) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] wx[t] - -(d mw (w1 + w3) \ Cos[theta3]) 2 R Sin[theta3] Sin[psi[t]] wx[t])) / 4, \ -----------------------------, 2 -(d mw (w1 + w3) Sin[theta3]) -----------------------------, 0, 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] wx[t] + 2 R Sin[theta3] \ Sin[psi[t]] wx[t])) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, Sin[psi[t]] Tan[theta[t]], Cos[psi[t]] Tan[theta[t]], 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Sec[theta[t]] Sin[psi[t]], Cos[psi[t]] Sec[theta[t]], 0, 0, \ 0}, {0, 0, 0, Cos[phi[t]] Cos[theta[t]], -(Cos[psi[t]] Sin[phi[t]]) + Cos[phi[t]] Sin[psi[t]] \ Sin[theta[t]], Sin[phi[t]] Sin[psi[t]] + Cos[phi[t]] Cos[psi[t]] \ Sin[theta[t]]}, {0, 0, 0, Cos[theta[t]] Sin[phi[t]], Cos[phi[t]] Cos[psi[t]] + Sin[phi[t]] Sin[psi[t]] \ Sin[theta[t]], -(Cos[phi[t]] Sin[psi[t]]) + Cos[psi[t]] Sin[phi[t]] \ Sin[theta[t]]}, {0, 0, 0, -Sin[theta[t]], Cos[theta[t]] Sin[psi[t]], Cos[psi[t]] Cos[theta[t]]}}}, {psi[t], theta[t], phi[t], x[t], \ y[t], z}, {wx[t], Cos[psi[t]] wyy[t] + Sin[psi[t]] wzz[t], -(Sin[psi[t]] wyy[t]) + Cos[psi[t]] wzz[t], R wyy[t], -(R \ Cos[psi[t]] wx[t]), R Sin[psi[t]] wx[t]}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[psi[t]] + Sin[psi[t]]) (wx[t] + Cos[psi[t]] wyy[t] - Sin[psi[t]] wyy[t] + Cos[psi[t]] \ wzz[t] + Sin[psi[t]] wzz[t])) / Iy) - wyy[t] (-wx[t] - Tan[theta[t]] \ wzz[t]), psi'[t] == wx[t] + Tan[theta[t]] wzz[t]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "These equations are obviously equivalent to thedisk equations given by \ Neimark and Fufaev. Another derivation is given by Meirovitch. To compare our \ equations with his, it is necessary to reduce them to second order form by \ eliminating the quasi-velociites (thus, we get Lagrange's equations), and to \ perform a minor transformation of anglular coordinates:"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Rules1=Flatten[Solve[TrEqns[[{1,2,8}]],{wx[t],wyy[t],wzz[t]}]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {wx[t] -> -(Sin[theta[t]]*Derivative[1][phi][t]) + \ Derivative[1][psi][t], wyy[t] -> Derivative[1][theta][t], wzz[t] -> \ Cos[theta[t]]*Derivative[1][phi][t]} \ \>", "\<\ {wx[t] -> -(Sin[theta[t]] phi'[t]) + psi'[t], wyy[t] -> \ theta'[t], wzz[t] -> Cos[theta[t]] phi'[t]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "Rules2={wx'[t]->D[wx[t]/.Rules1,t],\n wyy'[t]->D[wyy[t]/.Rules1,t],\n \ wzz'[t]->D[wzz[t]/.Rules1,t]}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {Derivative[1][wx][t] -> -(Cos[theta[t]]*Derivative[1][phi][t]*Derivative[1][theta][t]) - Sin[theta[t]]*Derivative[2][phi][t] + Derivative[2][psi][t], Derivative[1][wyy][t] -> Derivative[2][theta][t], Derivative[1][wzz][t] -> -(Sin[theta[t]]*Derivative[1][phi][t]*Derivative[1][theta][t]) + Cos[theta[t]]*Derivative[2][phi][t]} \ \>", "\<\ {wx'[t] -> -(Cos[theta[t]] phi'[t] theta'[t]) - Sin[theta[t]] \ phi''[t] + psi''[t], wyy'[t] -> theta''[t], wzz'[t] -> -(Sin[theta[t]] phi'[t] theta'[t]) + Cos[theta[t]] phi''[t]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "Rules3={theta[t]->th[t]-Pi/2,\n theta'[t]->th'[t],\n \ theta''[t]->th''[t]}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {theta[t] -> -Pi/2 + th[t], Derivative[1][theta][t] -> \ Derivative[1][th][t], Derivative[2][theta][t] -> Derivative[2][th][t]} \ \>", "\<\ -Pi {theta[t] -> --- + th[t], theta'[t] -> th'[t], theta''[t] -> th''[t]} 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "Rules4={phi[t]->-pi/2+ph[t],\n phi'[t]->ph'[t],\n \ phi''[t]->ph''[t]}"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {phi[t] -> -pi/2 + ph[t], Derivative[1][phi][t] -> Derivative[1][ph][t], \ Derivative[2][phi][t] -> Derivative[2][ph][t]} \ \>", "\<\ -pi {phi[t] -> --- + ph[t], phi'[t] -> ph'[t], phi''[t] -> ph''[t]} 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "LagEqns=Simplify[TrEqns[[{5,6,7}]]/.Rules1/.Rules2/.Rules3/.Rules4]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {-(Sin[th[t]]*Derivative[1][ph][t]*Derivative[1][th][t]) + Cos[th[t]]*Derivative[2][ph][t] + Derivative[2][psi][t] == -(({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])) \ + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, \ m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + \ Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - \ Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}* (Cos[th[t]]*Derivative[1][ph][t] + \ Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t] \ + Cos[psi[t]]*Derivative[1][th][t] - \ Sin[psi[t]]*Derivative[1][th][t]))/(J + m*R^2)) , Derivative[2][th][t] == -((Sec[psi[t]]*Derivative[1][psi][t]* (Iy*Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + m*R^2*Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Iy*Sin[psi[t]]*Derivative[1][th][t]))/(Iy + m*R^2)) - (Sec[psi[t]]*(-(gc*m*R*Cos[psi[t] - th[t]]) - gc*m*R*Cos[psi[t] + th[t]] \ + 2*Cos[psi[t]]*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), \ -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, \ (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}\\ , {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, \ 0}}*Sin[th[t]]*Derivative[1][ph][t] + 2*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), \ -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, \ (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}\\ , {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, \ 0}}*Sin[psi[t]]*Sin[th[t]]* Derivative[1][ph][t] + m*R^2*Cos[psi[t] - th[t]]*Sin[th[t]]* Derivative[1][ph][t]^2 + m*R^2*Cos[psi[t] + th[t]]*Sin[th[t]]* Derivative[1][ph][t]^2 + 2* {{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), \ -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, \ (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}\\ , {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - 2*m*R^2*Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + 2*Cos[psi[t]]*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, \ 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), \ -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, \ (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}\\ , {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, \ 0}}*Derivative[1][th][t] - 2*{{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + \ a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - \ d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), \ -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, \ {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, \ {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, \ (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, \ 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}\\ , {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*Sin[psi[t]]* Derivative[1][th][t]))/(2*(Iy + m*R^2)) + (Iy*Tan[psi[t]]*(-(Derivative[1][psi][t]*Derivative[1][th][t]) + ({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, \ R*Cos[psi[t]], 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])) + d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ + a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) \ - d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + \ w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, \ 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + \ w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*b*w1*Sin[theta2] + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + \ w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + \ 2*b*w1*Sin[theta3] + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + \ w3)*Sin[theta3])/2, 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, \ 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, \ {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - ph[t]]*Sin[psi[t]]}, {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*(Cos[psi[t]] + \ Sin[psi[t]])* (Cos[th[t]]*Derivative[1][ph][t] + Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t] + Cos[psi[t]]*Derivative[1][th][t] - \ Sin[psi[t]]*Derivative[1][th][t]))/Iy))/ (Iy + m*R^2), Cos[th[t]]*Derivative[1][ph][t]*Derivative[1][th][t] + Sin[th[t]]*Derivative[2][ph][t] == Derivative[1][psi][t]*Derivative[1][th][t] - ({{1, 0, 0, 0, -(R*Cos[psi[t]]), R*Sin[psi[t]]}, {0, 1, 0, R*Cos[psi[t]], \ 0, 0}, {0, 0, 1, -(R*Sin[psi[t]]), 0, 0}} . {{mw*(-(d*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])) + \ d*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + a*R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t]) + d*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - d*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), -((2*mw + m1)*R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(-2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + 2*b*w1*Sin[theta2] \ + b*w2*Sin[theta2] - 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, (d*mw*(w1 + w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(a*w1*Cos[theta2] - b*w1*Sin[theta2] + 2*R*Cos[theta2]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - 2*R*Sin[theta2]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, 0}, {(d*mw*(2*a*w1*Cos[theta3] + a*w3*Cos[theta3] + 2*b*w1*Sin[theta3] \ + b*w3*Sin[theta3] + 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) - 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, \ 0, (d*mw*(-(a*w1*Cos[theta3]) - b*w1*Sin[theta3] - 2*R*Cos[theta3]*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t]) + 2*R*Sin[theta3]*Sin[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])))/4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, m, \ 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]]*Sin[psi[t]]), -(Cos[psi[t]]*Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]]*Sin[psi[t]], Cos[psi[t]]*Csc[th[t]], 0, 0, 0}, {0, 0, 0, Cos[pi/2 - ph[t]]*Sin[th[t]], Cos[psi[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Cos[th[t]]*Sin[psi[t]], -(Cos[pi/2 - ph[t]]*Cos[psi[t]]*Cos[th[t]]) - Sin[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, -(Sin[pi/2 - ph[t]]*Sin[th[t]]), Cos[pi/2 - ph[t]]*Cos[psi[t]] + Cos[th[t]]*Sin[pi/2 - \ ph[t]]*Sin[psi[t]], Cos[psi[t]]*Cos[th[t]]*Sin[pi/2 - ph[t]] - Cos[pi/2 - \ ph[t]]*Sin[psi[t]]}, {0, 0, 0, Cos[th[t]], Sin[psi[t]]*Sin[th[t]], \ Cos[psi[t]]*Sin[th[t]]}}}, {psi[t], -Pi/2 + th[t], -pi/2 + ph[t], x[t], y[t], z}, {Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t], Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + \ Cos[psi[t]]*Derivative[1][th][t], Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] - \ Sin[psi[t]]*Derivative[1][th][t], R*Derivative[1][th][t], -(R*Cos[psi[t]]* (Cos[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t])), R*Sin[psi[t]]*(Cos[th[t]]*Derivative[1][ph][t] + \ Derivative[1][psi][t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R*Cos[psi[t]], \ -(R*Sin[psi[t]])}, {-(R*Cos[psi[t]]), 0, 0}, {R*Sin[psi[t]], 0, 0}}*(Cos[psi[t]] + \ Sin[psi[t]])* (Cos[th[t]]*Derivative[1][ph][t] + \ Cos[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Sin[psi[t]]*Sin[th[t]]*Derivative[1][ph][t] + Derivative[1][psi][t] \ + Cos[psi[t]]*Derivative[1][th][t] - \ Sin[psi[t]]*Derivative[1][th][t]))/Iy} \ \>", "\<\ {-(Sin[th[t]] ph'[t] th'[t]) + Cos[th[t]] ph''[t] + psi''[t] == -(({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) + \ d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) \ / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) \ / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) \ / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) \ / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, \ m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + \ Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[th[t]] ph'[t] + Cos[psi[t]] Sin[th[t]] ph'[t] + Sin[psi[t]] Sin[th[t]] ph'[t] + psi'[t] + Cos[psi[t]] th'[t] - \ Sin[psi[t]] th'[t]) 2 ) / (J + m R )), th''[t] == -((Sec[psi[t]] psi'[t] 2 (Iy Cos[psi[t]] Sin[th[t]] ph'[t] + m R Cos[psi[t]] Sin[th[t]] \ ph'[t] - 2 Iy Sin[psi[t]] th'[t])) / (Iy + m R )) - (Sec[psi[t]] (-(gc m R Cos[psi[t] - th[t]]) - gc m R Cos[psi[t] + th[t]] \ + 2 Cos[psi[t]] {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) \ + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} Sin[th[t]] \ ph'[t] + 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) \ + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} Sin[psi[t]] \ Sin[th[t]] ph'[t] \\ 2 2 + m R Cos[psi[t] - th[t]] Sin[th[t]] ph'[t] + 2 2 m R Cos[psi[t] + th[t]] Sin[th[t]] ph'[t] + 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) \ + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[th[t]] \ ph'[t] + psi'[t]) \\ 2 - 2 m R Cos[psi[t]] Sin[th[t]] ph'[t] (Cos[th[t]] ph'[t] + \ psi'[t]) + 2 Cos[psi[t]] {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) \ + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} th'[t] - 2 {{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R \ Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) \ + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, \ {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, \ {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, \ 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} Sin[psi[t]] \ th'[t])) / 2 (2 (Iy + m R )) + (Iy Tan[psi[t]] (-(psi'[t] th'[t]) + ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, \ 0}} . {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t])) + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) \ - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) \ Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, \ 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) \ Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 \ Sin[theta2] + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]) + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]) - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 \ Sin[theta3] + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]) - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, \ 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]) + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + \ psi'[t]))) / 4}}[ {{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, \ 0}, {0, 0, 0, m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, \ 0, 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, \ pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[psi[t]] + \ Sin[psi[t]]) (Cos[th[t]] ph'[t] + Cos[psi[t]] Sin[th[t]] ph'[t] + Sin[psi[t]] Sin[th[t]] ph'[t] + psi'[t] + Cos[psi[t]] th'[t] - 2 Sin[psi[t]] th'[t])) / Iy)) / (Iy + m R ), Cos[th[t]] ph'[t] th'[t] + Sin[th[t]] ph''[t] == psi'[t] th'[t] - ({{1, 0, 0, 0, -(R Cos[psi[t]]), R Sin[psi[t]]}, {0, 1, 0, R Cos[psi[t]], 0, 0}, {0, 0, 1, -(R Sin[psi[t]]), 0, 0}} . \ {{mw (-(d R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])) + d R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + a R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + d R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - d R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), -((2 mw + m1) R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), d mw (2 w1 + w2) (a Cos[theta2] - b Sin[theta2]) ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (-2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 b w1 Sin[theta2] \ + b w2 Sin[theta2] - 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) / \ 4, d mw (w1 + w2) Cos[theta2] d mw (w1 + w2) Sin[theta2] --------------------------, --------------------------, 2 2 (d mw (a w1 Cos[theta2] - b w1 Sin[theta2] + 2 R Cos[theta2] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - 2 R Sin[theta2] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) / \ 4, 0}, {(d mw (2 a w1 Cos[theta3] + a w3 Cos[theta3] + 2 b w1 Sin[theta3] \ + b w3 Sin[theta3] + 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) - 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) / \ 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 (d mw (-(a w1 Cos[theta3]) - b w1 Sin[theta3] - 2 R Cos[theta3] Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]) + 2 R Sin[theta3] Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t]))) / \ 4}}[{{J, 0, 0, 0, 0, 0}, {0, Iy, 0, 0, 0, 0}, {0, 0, Iy, 0, 0, 0}, {0, 0, 0, \ m, 0, 0}, {0, 0, 0, 0, m, 0}, {0, 0, 0, 0, 0, m}}, {{{1, -(Cot[th[t]] Sin[psi[t]]), -(Cos[psi[t]] Cot[th[t]]), 0, 0, \ 0}, {0, Cos[psi[t]], -Sin[psi[t]], 0, 0, 0}, {0, Csc[th[t]] Sin[psi[t]], Cos[psi[t]] Csc[th[t]], 0, 0, 0}, pi {0, 0, 0, Cos[-- - ph[t]] Sin[th[t]], 2 pi pi Cos[psi[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] Cos[th[t]] \ Sin[psi[t]], 2 2 pi pi -(Cos[-- - ph[t]] Cos[psi[t]] Cos[th[t]]) - Sin[-- - ph[t]] \ Sin[psi[t]]}, 2 2 pi {0, 0, 0, -(Sin[-- - ph[t]] Sin[th[t]]), 2 pi pi Cos[-- - ph[t]] Cos[psi[t]] + Cos[th[t]] Sin[-- - ph[t]] \ Sin[psi[t]], 2 2 pi pi Cos[psi[t]] Cos[th[t]] Sin[-- - ph[t]] - Cos[-- - ph[t]] \ Sin[psi[t]]}, 2 2 {0, 0, 0, Cos[th[t]], Sin[psi[t]] Sin[th[t]], Cos[psi[t]] \ Sin[th[t]]}}}, -Pi -pi {psi[t], --- + th[t], --- + ph[t], x[t], y[t], z}, 2 2 {Cos[th[t]] ph'[t] + psi'[t], Sin[psi[t]] Sin[th[t]] ph'[t] + \ Cos[psi[t]] th'[t], Cos[psi[t]] Sin[th[t]] ph'[t] - Sin[psi[t]] th'[t], R th'[t], -(R Cos[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])), R Sin[psi[t]] (Cos[th[t]] ph'[t] + psi'[t])}] . {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0, R Cos[psi[t]], -(R \ Sin[psi[t]])}, {-(R Cos[psi[t]]), 0, 0}, {R Sin[psi[t]], 0, 0}} (Cos[psi[t]] + \ Sin[psi[t]]) (Cos[th[t]] ph'[t] + Cos[psi[t]] Sin[th[t]] ph'[t] + Sin[psi[t]] \ Sin[th[t]] ph'[t] + psi'[t] + Cos[psi[t]] th'[t] - Sin[psi[t]] th'[t])) / Iy} \ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "These equations are easily confirmed to be equivalent to those given by \ Meirovitch."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Timing Considerations"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Introductory Remarks"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "The computer time required for building a model can vary greatly depending \ on the computer and its configuration. But it is also very much dependent on \ the system being analysed and the way in which the system parametric data is \ defined. Excessive execution time can sometimes arise from suprisingly minor \ factors in the formulation, whereas obvious indicators of system complexity \ such as the number of degrees of freedom may be of little consequence. The \ number of symbolic parameters used in the data definition can be significant, \ but even more important is how those parameters are introduced. \n\nThe \ following paragraphs illustrate some of these considerations and show the \ value of simplifying the kinematic expressions by replacing recurrent \ expressions by temporary variables. All of the times shown below were \ obtained using a Macintosh Quadra 800 with 24 MB od Ram."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Replacements for Repeated Expressions"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "When a set of expressions contain many repetitions of of some subexpression \ then it makes sense to introduce a temporary variable for it and perform any \ required operations on that expression only once. This is, of course, a \ standard procedure for making numerical computation more efficient. TSi \ Dynamics includes a function, KinematicReplacements, which provides a limited \ extension of this idea to symbolic model assembly. \n\nKinematicReplacements \ analyzes the combined set of expressions in the lists V,X,H (after the joint \ kinematics are computed) for repeated expressions and replaces them with \ temporary variables. In addition to the new V,X,H lists, a set of inverse \ replacement rules are returned which allows the original V,X,H to be \ recovered whenever desired. Meanwhile, computations can be performed using \ the \"simplified\" kinematic expressions. The repeated expressions that are \ replaced are of two types: (i) expressions that depend on configuration \ coordinates as well as symbolic system data parameters, and (ii) expressions \ that depend only on symbolic system data parameters. Expressions that depend \ on system coordinates must be reintroduced before certain operations (such as \ differentiation with respect to a coordinate) are performed. To facilitate \ this, the inverse replacement rules are returned in two sets: rules1 and \ rules2. The application of rules1 restores all expressions containing the \ configuration coordinates. rules2 must be applied after rules1 to restore all \ parametric expressions.\n\nNormally, the function KinematicReplacements would \ be employed after the computation of V,X,H using Joints. The simplified V,X,H \ are then used throughout the calculation of generalized forces and potential \ energy functions. The coordinated dependent expressions are then restored by \ applying rules1 before using the functions CreateModel or CreateModelSim. \ Then the parameter dependent expressions can be restored by applying rules2.\n\ \nThis process will have marginal affect on simple problems and is likely to \ produce slightly longer computation times. But it can have a dramatic affect \ on computation times for more complex problems."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Partial Suspension System Revisited"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "To see how this process works we will take another look at the Partial \ Suspension System example describe above. The computations are performed in \ two ways: without and then with the use of KinematicReplacements as \ described in the preceeding paragraphs. Timing data is obtained in both cases \ and it will be seen that the use of the KinematicReplacements strategy \ slightly slows things down."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["Joint Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "(* Joint 1 - planar motion *)\nr1={3};\nq1={theta1,y,z};p1={w1,vy,vz};\n\ H1={{1,0,0},{0,0,0},{0,0,0},{0,0,0},{0,1,0},{0,0,1}};\n(* Joint 2 - \ suspension *)\nr2={1};\nq2={theta2};p2={w2};\nH2={{1},{0},{0},{0},{0},{0}};\n\ (* Joint 3 - suspension *)\nr3={1};\nH3={{1},{0},{0},{0},{0},{0}};\n\ q3={theta3};p3={w3};\n\nJointLst={{r1,H1,q1,p1},{r2,H2,q2,p2},{r3,H3,q3,p3}};\ \n{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics \ \>", "\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["Body Data"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Body 1 - Chassis *)\ncm1={0,0,0};\n\ out1={{2,{0,b/2,a/2}},{3,{0,-b/2,a/2}},{6,{0,b/2+a/2,-a/2}},{7,{0,-b/2-a/2,-a/\ 2}}};\nI1=DiagonalMatrix[{Ixx,Iyy,Izz}];\n\n(* Body 2 - Wheel & Axle *)\n\ cm2={0,d,0};\nout2={{4,{0,d,R}},{8,{0,Sqrt[2]*a,0}}};\n\ I2=DiagonalMatrix[{Wxx,Wyy,Wzz}];\n\n(* Body 3 - Wheel & Axle *)\n\ cm3={0,-d,0};\nout3={{5,{0,-d,R}},{9,{0,-Sqrt[2]*a,0}}};\n\ I3=DiagonalMatrix[{Wxx,Wyy,Wzz}];\n\n\ BodyLst={{cm1,out1,m1,I1},{cm2,out2,mw,I2},{cm3,out3,mw,I3}};\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["System Structure"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["TreeLst={{{1,1},{2,2}},{{1,1},{3,3}}};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Potential Energy Constructions"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Define Tire PE in terms of Space Coordinates *)\n\n\ yy={{x2,y2,z2},{x3,y3,z3}};\nPot=(k/2)*(z2-z20)^2+(k/2)*(z3-z30)^2;"], "Input",\ PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "(* Obtain Tire PE in terms of generalized Coordinates *)\n\n\ TirePE=LeafPotential[BodyLst,TreeLst,X,H,Pot,yy];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["TirePE"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])^2)/2 + (k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])^2)/2 \ \>", "\<\ a Cos[theta1] b \ Sin[theta1] (k Power[z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 d Sin[theta1 + theta2], 2]) / 2 + a Cos[theta1] b Sin[theta1] (k Power[z - z30 + ------------- + R Cos[theta1 + theta3] - ------------- - \ 2 2 d Sin[theta1 + theta3], 2]) / 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "SpringPot[x_]:=(kdel/2)*(x-ls)^2\nq=Join[q1,q2,q3];\n\ SpringPE1=SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q];\n\ SpringPE2=SpringPotential[7,9,SpringPot,TreeLst,BodyLst,X,q];\n\n\ Timing[SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q]]\n\n\ PE=TirePE+SpringPE1+SpringPE2;\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {5.326999999999651*Second, (kdel*(-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2)^2)/2} \ \>", "\<\ 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] \ Sin[theta2])] 2 kdel (-ls + \ -------------------------------------------------------------) 2 {5.327 Second, \ ---------------------------------------------------------------------------} 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["SpringPE1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (kdel*(-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2)^2)/2 \ \>", "\<\ 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] 2 kdel (-ls + -------------------------------------------------------------) 2 --------------------------------------------------------------------------- 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["SpringPE2"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ (kdel*(-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta3] - \ 8*2^(1/2)*Sin[theta3]))^(1/2)/2)^2)/2 \ \>", "\<\ 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] 2 kdel (-ls + -------------------------------------------------------------) 2 --------------------------------------------------------------------------- 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData["Generalized Force"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["Q={0,0,0,-c*w2,-c*w3};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Gravity"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["g=-gc;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Obtain Equations of Motion"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Timing[CreateModelSim[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H]]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ {153.6259999999998*Second, {{{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, \ {0, Sin[theta1], Cos[theta1]}}, {{1}}, {{1}}}, {{{1, 0, 0, 0}, {0, Cos[theta1], -Sin[theta1], y}, {0, Sin[theta1], \ Cos[theta1], z}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta2], -Sin[theta2], 0}, {0, Sin[theta2], Cos[theta2], 0}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta3], -Sin[theta3], 0}, {0, Sin[theta3], \ Cos[theta3], 0}, {0, 0, 0, 1}}}, {{{1, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 1, 0}, \ {0, 0, 1}}, {{1}, {0}, {0}, {0}, {0}, {0}}, {{1}, {0}, {0}, {0}, {0}, {0}}}, {{Ixx + (a^2*mw)/2 + (b^2*mw)/2 + 2*d^2*mw + 2*Wxx + b*d*mw*Cos[theta2] + b*d*mw*Cos[theta3] + a*d*mw*Sin[theta2] - a*d*mw*Sin[theta3], -(mw*(a + d*Sin[theta2] - d*Sin[theta3])), d*mw*(Cos[theta2] - \ Cos[theta3]), d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2}, {-(mw*(a + d*Sin[theta2] - d*Sin[theta3])), 2*mw + m1, 0, \ -(d*mw*Sin[theta2]), d*mw*Sin[theta3]}, {d*mw*(Cos[theta2] - Cos[theta3]), 0, 2*mw + m1, \ d*mw*Cos[theta2], -(d*mw*Cos[theta3])}, {d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + \ (a*d*mw*Sin[theta2])/2, -(d*mw*Sin[theta2]), d*mw*Cos[theta2], d^2*mw + Wxx, 0}, {d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2, \ d*mw*Sin[theta3], -(d*mw*Cos[theta3]), 0, d^2*mw + Wxx}}, {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - \ d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - \ b*w1*Sin[theta2]))/ 4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + \ a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/4}} [{{Ixx + (a^2*mw)/2 + (b^2*mw)/2 + 2*d^2*mw + 2*Wxx + b*d*mw*Cos[theta2] \ + b*d*mw*Cos[theta3] + a*d*mw*Sin[theta2] - a*d*mw*Sin[theta3], -(mw*(a + d*Sin[theta2] - d*Sin[theta3])), d*mw*(Cos[theta2] - \ Cos[theta3]), d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2}, {-(mw*(a + d*Sin[theta2] - d*Sin[theta3])), 2*mw + m1, 0, \ -(d*mw*Sin[theta2]), d*mw*Sin[theta3]}, {d*mw*(Cos[theta2] - Cos[theta3]), 0, 2*mw + m1, \ d*mw*Cos[theta2], -(d*mw*Cos[theta3])}, {d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, -(d*mw*Sin[theta2]), d*mw*Cos[theta2], d^2*mw \ + Wxx, 0}, {d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2, \ d*mw*Sin[theta3], -(d*mw*Cos[theta3]), 0, d^2*mw + Wxx}}, {{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], \ Cos[theta1]}}, {{1}}, {{1}}}, {theta1, y, z, theta2, theta3}, {w1, vy, vz, w2, w3}], {gc*mw*(-(d*Cos[theta1 + theta2]) + d*Cos[theta1 + theta3] + \ a*Sin[theta1]) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*((b*Cos[theta1])/2 + d*Cos[theta1 + theta2] - \ (a*Sin[theta1])/2 - R*Sin[theta1 + theta2]) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(b*Cos[theta1])/2 - d*Cos[theta1 + theta3] \ - (a*Sin[theta1])/2 - R*Sin[theta1 + theta3]), Sin[theta1]*(-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + \ k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + \ theta2] - d*k*Sin[theta1 + theta3]), Cos[theta1]* (-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + \ theta2] - d*k*Sin[theta1 + theta3]), c*w2 - d*gc*mw*Cos[theta1 + theta2] + (2^(1/2)*a^2*kdel*(2*Cos[theta2] + Sin[theta2])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta2] + 8*2^(1/2)*Sin[theta2]))^(1/2) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*(d*Cos[theta1 + theta2] - R*Sin[theta1 + \ theta2]), c*w3 + d*gc*mw*Cos[theta1 + theta3] - (2^(1/2)*a^2*kdel*(2*Cos[theta3] - Sin[theta3])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta3] - \ 8*2^(1/2)*Sin[theta3]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta3] - 8*2^(1/2)*Sin[theta3]))^(1/2) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(d*Cos[theta1 + theta3]) - R*Sin[theta1 + \ theta3])}, {w1, vy, vz, w2, w3}, {theta1, y, z, theta2, theta3}}} \ \>", "\<\ {153.626 Second, {{{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], Cos[theta1]}}, {{1}}, {{1}}}, {{{1, 0, 0, 0}, {0, Cos[theta1], -Sin[theta1], y}, {0, Sin[theta1], \ Cos[theta1], z}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta2], -Sin[theta2], 0}, {0, Sin[theta2], Cos[theta2], 0}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta3], -Sin[theta3], 0}, {0, Sin[theta3], \ Cos[theta3], 0}, {0, 0, 0, 1}}}, {{{1, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 1, 0}, \ {0, 0, 1}}, {{1}, {0}, {0}, {0}, {0}, {0}}, {{1}, {0}, {0}, {0}, {0}, {0}}}, 2 2 a mw b mw 2 {{Ixx + ----- + ----- + 2 d mw + 2 Wxx + b d mw Cos[theta2] + b d mw \ Cos[theta3] + 2 2 a d mw Sin[theta2] - a d mw Sin[theta3], -(mw (a + d Sin[theta2] - d \ Sin[theta3])), d mw (Cos[theta2] - Cos[theta3]), 2 b d mw Cos[theta2] a d mw Sin[theta2] d mw + Wxx + ------------------ + ------------------, 2 2 2 b d mw Cos[theta3] a d mw Sin[theta3] d mw + Wxx + ------------------ - ------------------}, 2 2 {-(mw (a + d Sin[theta2] - d Sin[theta3])), 2 mw + m1, 0, -(d mw \ Sin[theta2]), d mw Sin[theta3]}, {d mw (Cos[theta2] - Cos[theta3]), 0, 2 mw + m1, d mw \ Cos[theta2], 2 b d mw Cos[theta2] a d mw \ Sin[theta2] -(d mw Cos[theta3])}, {d mw + Wxx + ------------------ + \ ------------------, 2 2 2 -(d mw Sin[theta2]), d mw Cos[theta2], d mw + Wxx, 0}, 2 b d mw Cos[theta3] a d mw Sin[theta3] {d mw + Wxx + ------------------ - ------------------, d mw Sin[theta3], \ 2 2 2 -(d mw Cos[theta3]), 0, d mw + Wxx}}, {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz \ Sin[theta3]), d mw (2 w1 + w2) (a Cos[theta2] - b \ Sin[theta2]) -((2 mw + m1) vz), (2 mw + m1) vy, \ ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz \ Sin[theta2] + d mw (w1 + w2) \ Cos[theta2] 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) -----------------------------------------------------------------------\ ----------, 0} 4 , {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ------------------------------------------------------------------------\ --------}} 4 2 2 a mw b mw 2 [{{Ixx + ----- + ----- + 2 d mw + 2 Wxx + b d mw Cos[theta2] + b d mw \ Cos[theta3] + 2 2 a d mw Sin[theta2] - a d mw Sin[theta3], -(mw (a + d Sin[theta2] - d \ Sin[theta3])), d mw (Cos[theta2] - Cos[theta3]), 2 b d mw Cos[theta2] a d mw Sin[theta2] d mw + Wxx + ------------------ + ------------------, 2 2 2 b d mw Cos[theta3] a d mw Sin[theta3] d mw + Wxx + ------------------ - ------------------}, 2 2 {-(mw (a + d Sin[theta2] - d Sin[theta3])), 2 mw + m1, 0, -(d mw \ Sin[theta2]), d mw Sin[theta3]}, {d mw (Cos[theta2] - Cos[theta3]), 0, 2 mw + m1, d \ mw Cos[theta2], 2 b d mw Cos[theta2] a d mw \ Sin[theta2] -(d mw Cos[theta3])}, {d mw + Wxx + ------------------ + \ ------------------, 2 2 2 -(d mw Sin[theta2]), d mw Cos[theta2], d mw + Wxx, 0}, 2 b d mw Cos[theta3] a d mw Sin[theta3] {d mw + Wxx + ------------------ - ------------------, d mw \ Sin[theta3], 2 2 2 -(d mw Cos[theta3]), 0, d mw + Wxx}}, {{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], \ Cos[theta1]}}, {{1}}, {{1}}}, {theta1, y, z, theta2, theta3}, {w1, vy, vz, w2, w3}], {gc mw (-(d Cos[theta1 + theta2]) + d Cos[theta1 + theta3] + a \ Sin[theta1]) + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 b Cos[theta1] a \ Sin[theta1] d Sin[theta1 + theta2]) (------------- + d Cos[theta1 + theta2] - \ ------------- - 2 \ 2 a Cos[theta1] R Sin[theta1 + theta2]) + k (z - z30 + ------------- + R Cos[theta1 + \ theta3] - 2 b Sin[theta1] ------------- - d Sin[theta1 + theta3]) 2 -(b Cos[theta1]) a Sin[theta1] (---------------- - d Cos[theta1 + theta3] - ------------- - R \ Sin[theta1 + theta3]), 2 2 Sin[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + \ k R Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + \ theta2] - d k Sin[theta1 + theta3]), Cos[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + k R Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + \ theta2] - d k Sin[theta1 + theta3]), c w2 - d gc mw Cos[theta1 + theta2] + 2 (Sqrt[2] a kdel (2 Cos[theta2] + Sin[theta2]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] (-ls + \ -------------------------------------------------------------)) / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 d Sin[theta1 + theta2]) (d Cos[theta1 + theta2] - R Sin[theta1 + \ theta2]), c w3 + d gc mw Cos[theta1 + theta3] - 2 (Sqrt[2] a kdel (2 Cos[theta3] - Sin[theta3]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] (-ls + \ -------------------------------------------------------------)) / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] + a Cos[theta1] b Sin[theta1] k (z - z30 + ------------- + R Cos[theta1 + theta3] - ------------- - 2 2 d Sin[theta1 + theta3]) (-(d Cos[theta1 + theta3]) - R Sin[theta1 + \ theta3])}, {w1, vy, vz, w2, w3}, {theta1, y, z, theta2, theta3}}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData[ "{V,X,H,rules1,rules2}=KinematicReplacements[V,X,H,q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"rules1\" is similar to existing symbol \"Rules1\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"rules1\" is similar to existing symbol \"Rules1\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ General::spell1: Possible spelling error: new symbol name \"rules2\" is similar to existing symbol \"Rules2\". \ \>", "\<\ General::spell1: Possible spelling error: new symbol name \ \"rules2\" is similar to existing symbol \"Rules2\".\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["rules1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {t1 -> Cos[theta1], t2 -> Cos[theta2], t3 -> Cos[theta3]} \ \>", "\<\ {t1 -> Cos[theta1], t2 -> Cos[theta2], t3 -> Cos[theta3]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[TextData["rules2"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {} \ \>", "\<\ {}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False] }, Closed]], Cell[TextData[ "(* Define Tire PE in terms of Space Coordinates *)\n\n\ yy={{x2,y2,z2},{x3,y3,z3}};\nPot=(k/2)*(z2-z20)^2+(k/2)*(z3-z30)^2;"], "Input",\ PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData[ "(* Obtain Tire PE in terms of generalized Coordinates *)\n\n\ TirePE=LeafPotential[BodyLst,TreeLst,X,H,Pot,yy];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "SpringPot[x_]:=(kdel/2)*(x-ls)^2\nq=Join[q1,q2,q3];\n\ SpringPE1=SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q];\n\ SpringPE2=SpringPotential[7,9,SpringPot,TreeLst,BodyLst,X,q];\n\n\ Timing[SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q]]\n\n\ PE=TirePE+SpringPE1+SpringPE2;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {10.82000000000032*Second, (kdel*(-ls + (a^2*(-1 - 2*t1^2 + Cos[2*theta1])* (-9 + 4*2^(1/2)*t2 - 8*t2^2 + 4*Cos[2*theta2] - \ 8*2^(1/2)*Sin[theta2]))^(1/2)/ (2*2^(1/2)))^2)/2} \ \>", "\<\ 2 2 {10.82 Second, (kdel Power[-ls + Sqrt[a (-1 - 2 t1 + Cos[2 theta1]) 2 (-9 + 4 Sqrt[2] t2 - 8 t2 + 4 Cos[2 theta2] - 8 Sqrt[2] \ Sin[theta2])] / (2 Sqrt[2]), 2]) / 2}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["SpringPE1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ (kdel*(-ls + (a^2*(-1 - 2*t1^2 + Cos[2*theta1])* (-9 + 4*2^(1/2)*t2 - 8*t2^2 + 4*Cos[2*theta2] - \ 8*2^(1/2)*Sin[theta2]))^(1/2)/ (2*2^(1/2)))^2)/2 \ \>", "\<\ 2 2 (kdel Power[-ls + Sqrt[a (-1 - 2 t1 + Cos[2 theta1]) 2 (-9 + 4 Sqrt[2] t2 - 8 t2 + 4 Cos[2 theta2] - 8 Sqrt[2] \ Sin[theta2])] / (2 Sqrt[2]), 2]) / 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[CellGroupData[{ Cell[TextData["SpringPE2"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ (kdel*(-ls + (a^2*(-1 - 2*t1^2 + Cos[2*theta1])* (-9 + 4*2^(1/2)*t3 - 8*t3^2 + 4*Cos[2*theta3] + \ 8*2^(1/2)*Sin[theta3]))^(1/2)/ (2*2^(1/2)))^2)/2 \ \>", "\<\ 2 2 (kdel Power[-ls + Sqrt[a (-1 - 2 t1 + Cos[2 theta1]) 2 (-9 + 4 Sqrt[2] t3 - 8 t3 + 4 Cos[2 theta3] + 8 Sqrt[2] \ Sin[theta3])] / (2 Sqrt[2]), 2]) / 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData["{PE,Q,V,X,H}={PE,Q,V,X,H}/.rules1;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Timing[CreateModelSim[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H]/.rules2]"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function \ \>", "\<\ Computing Potential Functions Computing Inertia Matrix Computing Poincare Function\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}], Cell[OutputFormData[ "\<\ {178.4519999999998*Second, {{{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, \ {0, Sin[theta1], Cos[theta1]}}, {{1}}, {{1}}}, {{{1, 0, 0, 0}, {0, Cos[theta1], -Sin[theta1], y}, {0, Sin[theta1], \ Cos[theta1], z}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta2], -Sin[theta2], 0}, {0, Sin[theta2], Cos[theta2], 0}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta3], -Sin[theta3], 0}, {0, Sin[theta3], \ Cos[theta3], 0}, {0, 0, 0, 1}}}, {{{1, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 1, 0}, \ {0, 0, 1}}, {{1}, {0}, {0}, {0}, {0}, {0}}, {{1}, {0}, {0}, {0}, {0}, {0}}}, {{Ixx + (a^2*mw)/2 + (b^2*mw)/2 + 2*d^2*mw + 2*Wxx + b*d*mw*Cos[theta2] + b*d*mw*Cos[theta3] + a*d*mw*Sin[theta2] - a*d*mw*Sin[theta3], -(mw*(a + d*Sin[theta2] - d*Sin[theta3])), d*mw*(Cos[theta2] - \ Cos[theta3]), d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2}, {-(mw*(a + d*Sin[theta2] - d*Sin[theta3])), 2*mw + m1, 0, \ -(d*mw*Sin[theta2]), d*mw*Sin[theta3]}, {d*mw*(Cos[theta2] - Cos[theta3]), 0, 2*mw + m1, \ d*mw*Cos[theta2], -(d*mw*Cos[theta3])}, {d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + \ (a*d*mw*Sin[theta2])/2, -(d*mw*Sin[theta2]), d*mw*Cos[theta2], d^2*mw + Wxx, 0}, {d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2, \ d*mw*Sin[theta3], -(d*mw*Cos[theta3]), 0, d^2*mw + Wxx}}, {{mw*(a*vz + d*vy*Cos[theta2] - d*vy*Cos[theta3] + d*vz*Sin[theta2] - \ d*vz*Sin[theta3]), -((2*mw + m1)*vz), (2*mw + m1)*vy, (d*mw*(2*w1 + w2)*(a*Cos[theta2] - b*Sin[theta2]))/2, -(d*mw*(2*w1 + w3)*(a*Cos[theta3] + b*Sin[theta3]))/2}, {d*mw*w1*(-Cos[theta2] + Cos[theta3]), 0, -((2*mw + m1)*w1), -(d*mw*(2*w1 + w2)*Cos[theta2]), d*mw*(2*w1 + w3)*Cos[theta3]}, {-(mw*w1*(a + d*Sin[theta2] - d*Sin[theta3])), (2*mw + m1)*w1, 0, -(d*mw*(2*w1 + w2)*Sin[theta2]), d*mw*(2*w1 + w3)*Sin[theta3]}, {(d*mw*(2*vy*Cos[theta2] - 2*a*w1*Cos[theta2] - a*w2*Cos[theta2] + \ 2*vz*Sin[theta2] + 2*b*w1*Sin[theta2] + b*w2*Sin[theta2]))/4, (d*mw*(w1 + \ w2)*Cos[theta2])/2, (d*mw*(w1 + w2)*Sin[theta2])/2, (d*mw*(-2*vy*Cos[theta2] + a*w1*Cos[theta2] - 2*vz*Sin[theta2] - \ b*w1*Sin[theta2]))/ 4, 0}, {(d*mw*(-2*vy*Cos[theta3] + 2*a*w1*Cos[theta3] + \ a*w3*Cos[theta3] - 2*vz*Sin[theta3] + 2*b*w1*Sin[theta3] + b*w3*Sin[theta3]))/4, -(d*mw*(w1 + w3)*Cos[theta3])/2, -(d*mw*(w1 + w3)*Sin[theta3])/2, 0, (d*mw*(2*vy*Cos[theta3] - a*w1*Cos[theta3] + 2*vz*Sin[theta3] - \ b*w1*Sin[theta3]))/4}} [{{Ixx + (a^2*mw)/2 + (b^2*mw)/2 + 2*d^2*mw + 2*Wxx + b*d*mw*Cos[theta2] \ + b*d*mw*Cos[theta3] + a*d*mw*Sin[theta2] - a*d*mw*Sin[theta3], -(mw*(a + d*Sin[theta2] - d*Sin[theta3])), d*mw*(Cos[theta2] - \ Cos[theta3]), d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2}, {-(mw*(a + d*Sin[theta2] - d*Sin[theta3])), 2*mw + m1, 0, \ -(d*mw*Sin[theta2]), d*mw*Sin[theta3]}, {d*mw*(Cos[theta2] - Cos[theta3]), 0, 2*mw + m1, \ d*mw*Cos[theta2], -(d*mw*Cos[theta3])}, {d^2*mw + Wxx + (b*d*mw*Cos[theta2])/2 + (a*d*mw*Sin[theta2])/2, -(d*mw*Sin[theta2]), d*mw*Cos[theta2], d^2*mw \ + Wxx, 0}, {d^2*mw + Wxx + (b*d*mw*Cos[theta3])/2 - (a*d*mw*Sin[theta3])/2, \ d*mw*Sin[theta3], -(d*mw*Cos[theta3]), 0, d^2*mw + Wxx}}, {{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], \ Cos[theta1]}}, {{1}}, {{1}}}, {theta1, y, z, theta2, theta3}, {w1, vy, vz, w2, w3}], {gc*mw*(-(d*Cos[theta1 + theta2]) + d*Cos[theta1 + theta3] + \ a*Sin[theta1]) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*((b*Cos[theta1])/2 + d*Cos[theta1 + theta2] - \ (a*Sin[theta1])/2 - R*Sin[theta1 + theta2]) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(b*Cos[theta1])/2 - d*Cos[theta1 + theta3] \ - (a*Sin[theta1])/2 - R*Sin[theta1 + theta3]), Sin[theta1]*(-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + \ k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + \ theta2] - d*k*Sin[theta1 + theta3]), Cos[theta1]* (-2*gc*mw - gc*m1 + 2*k*z - k*z20 - k*z30 + a*k*Cos[theta1] + k*R*Cos[theta1 + theta2] + k*R*Cos[theta1 + theta3] + d*k*Sin[theta1 + \ theta2] - d*k*Sin[theta1 + theta3]), c*w2 - d*gc*mw*Cos[theta1 + theta2] + (2^(1/2)*a^2*kdel*(2*Cos[theta2] + Sin[theta2])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta2] + \ 8*2^(1/2)*Sin[theta2]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta2] + 8*2^(1/2)*Sin[theta2]))^(1/2) + k*(z - z20 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta2] + \ (b*Sin[theta1])/2 + d*Sin[theta1 + theta2])*(d*Cos[theta1 + theta2] - R*Sin[theta1 + \ theta2]), c*w3 + d*gc*mw*Cos[theta1 + theta3] - (2^(1/2)*a^2*kdel*(2*Cos[theta3] - Sin[theta3])* (-ls + (a^2*(13 - 4*2^(1/2)*Cos[theta3] - \ 8*2^(1/2)*Sin[theta3]))^(1/2)/2))/ (a^2*(13 - 4*2^(1/2)*Cos[theta3] - 8*2^(1/2)*Sin[theta3]))^(1/2) + k*(z - z30 + (a*Cos[theta1])/2 + R*Cos[theta1 + theta3] - \ (b*Sin[theta1])/2 - d*Sin[theta1 + theta3])*(-(d*Cos[theta1 + theta3]) - R*Sin[theta1 + \ theta3])}, {w1, vy, vz, w2, w3}, {theta1, y, z, theta2, theta3}}} \ \>", "\<\ {178.452 Second, {{{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], Cos[theta1]}}, {{1}}, {{1}}}, {{{1, 0, 0, 0}, {0, Cos[theta1], -Sin[theta1], y}, {0, Sin[theta1], \ Cos[theta1], z}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta2], -Sin[theta2], 0}, {0, Sin[theta2], Cos[theta2], 0}, {0, 0, 0, 1}}, {{1, 0, 0, 0}, {0, Cos[theta3], -Sin[theta3], 0}, {0, Sin[theta3], \ Cos[theta3], 0}, {0, 0, 0, 1}}}, {{{1, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 1, 0}, \ {0, 0, 1}}, {{1}, {0}, {0}, {0}, {0}, {0}}, {{1}, {0}, {0}, {0}, {0}, {0}}}, 2 2 a mw b mw 2 {{Ixx + ----- + ----- + 2 d mw + 2 Wxx + b d mw Cos[theta2] + b d mw \ Cos[theta3] + 2 2 a d mw Sin[theta2] - a d mw Sin[theta3], -(mw (a + d Sin[theta2] - d \ Sin[theta3])), d mw (Cos[theta2] - Cos[theta3]), 2 b d mw Cos[theta2] a d mw Sin[theta2] d mw + Wxx + ------------------ + ------------------, 2 2 2 b d mw Cos[theta3] a d mw Sin[theta3] d mw + Wxx + ------------------ - ------------------}, 2 2 {-(mw (a + d Sin[theta2] - d Sin[theta3])), 2 mw + m1, 0, -(d mw \ Sin[theta2]), d mw Sin[theta3]}, {d mw (Cos[theta2] - Cos[theta3]), 0, 2 mw + m1, d mw \ Cos[theta2], 2 b d mw Cos[theta2] a d mw \ Sin[theta2] -(d mw Cos[theta3])}, {d mw + Wxx + ------------------ + \ ------------------, 2 2 2 -(d mw Sin[theta2]), d mw Cos[theta2], d mw + Wxx, 0}, 2 b d mw Cos[theta3] a d mw Sin[theta3] {d mw + Wxx + ------------------ - ------------------, d mw Sin[theta3], \ 2 2 2 -(d mw Cos[theta3]), 0, d mw + Wxx}}, {{mw (a vz + d vy Cos[theta2] - d vy Cos[theta3] + d vz Sin[theta2] - d vz \ Sin[theta3]), d mw (2 w1 + w2) (a Cos[theta2] - b \ Sin[theta2]) -((2 mw + m1) vz), (2 mw + m1) vy, \ ------------------------------------------------, 2 -(d mw (2 w1 + w3) (a Cos[theta3] + b Sin[theta3])) ---------------------------------------------------}, 2 {d mw w1 (-Cos[theta2] + Cos[theta3]), 0, -((2 mw + m1) w1), -(d mw (2 w1 + w2) Cos[theta2]), d mw (2 w1 + w3) Cos[theta3]}, {-(mw w1 (a + d Sin[theta2] - d Sin[theta3])), (2 mw + m1) w1, 0, -(d mw (2 w1 + w2) Sin[theta2]), d mw (2 w1 + w3) Sin[theta3]}, {(d mw (2 vy Cos[theta2] - 2 a w1 Cos[theta2] - a w2 Cos[theta2] + 2 vz \ Sin[theta2] + d mw (w1 + w2) \ Cos[theta2] 2 b w1 Sin[theta2] + b w2 Sin[theta2])) / 4, \ --------------------------, 2 d mw (w1 + w2) Sin[theta2] --------------------------, 2 d mw (-2 vy Cos[theta2] + a w1 Cos[theta2] - 2 vz Sin[theta2] - b w1 \ Sin[theta2]) -----------------------------------------------------------------------\ ----------, 0} 4 , {(d mw (-2 vy Cos[theta3] + 2 a w1 Cos[theta3] + a w3 Cos[theta3] - 2 vz Sin[theta3] + 2 b w1 Sin[theta3] + b w3 Sin[theta3])) / 4, -(d mw (w1 + w3) Cos[theta3]) -(d mw (w1 + w3) Sin[theta3]) -----------------------------, -----------------------------, 0, 2 2 d mw (2 vy Cos[theta3] - a w1 Cos[theta3] + 2 vz Sin[theta3] - b w1 \ Sin[theta3]) ------------------------------------------------------------------------\ --------}} 4 2 2 a mw b mw 2 [{{Ixx + ----- + ----- + 2 d mw + 2 Wxx + b d mw Cos[theta2] + b d mw \ Cos[theta3] + 2 2 a d mw Sin[theta2] - a d mw Sin[theta3], -(mw (a + d Sin[theta2] - d \ Sin[theta3])), d mw (Cos[theta2] - Cos[theta3]), 2 b d mw Cos[theta2] a d mw Sin[theta2] d mw + Wxx + ------------------ + ------------------, 2 2 2 b d mw Cos[theta3] a d mw Sin[theta3] d mw + Wxx + ------------------ - ------------------}, 2 2 {-(mw (a + d Sin[theta2] - d Sin[theta3])), 2 mw + m1, 0, -(d mw \ Sin[theta2]), d mw Sin[theta3]}, {d mw (Cos[theta2] - Cos[theta3]), 0, 2 mw + m1, d \ mw Cos[theta2], 2 b d mw Cos[theta2] a d mw \ Sin[theta2] -(d mw Cos[theta3])}, {d mw + Wxx + ------------------ + \ ------------------, 2 2 2 -(d mw Sin[theta2]), d mw Cos[theta2], d mw + Wxx, 0}, 2 b d mw Cos[theta3] a d mw Sin[theta3] {d mw + Wxx + ------------------ - ------------------, d mw \ Sin[theta3], 2 2 2 -(d mw Cos[theta3]), 0, d mw + Wxx}}, {{{1, 0, 0}, {0, Cos[theta1], -Sin[theta1]}, {0, Sin[theta1], \ Cos[theta1]}}, {{1}}, {{1}}}, {theta1, y, z, theta2, theta3}, {w1, vy, vz, w2, w3}], {gc mw (-(d Cos[theta1 + theta2]) + d Cos[theta1 + theta3] + a \ Sin[theta1]) + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 b Cos[theta1] a \ Sin[theta1] d Sin[theta1 + theta2]) (------------- + d Cos[theta1 + theta2] - \ ------------- - 2 \ 2 a Cos[theta1] R Sin[theta1 + theta2]) + k (z - z30 + ------------- + R Cos[theta1 + \ theta3] - 2 b Sin[theta1] ------------- - d Sin[theta1 + theta3]) 2 -(b Cos[theta1]) a Sin[theta1] (---------------- - d Cos[theta1 + theta3] - ------------- - R \ Sin[theta1 + theta3]), 2 2 Sin[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + \ k R Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + \ theta2] - d k Sin[theta1 + theta3]), Cos[theta1] (-2 gc mw - gc m1 + 2 k z - k z20 - k z30 + a k Cos[theta1] + k R Cos[theta1 + theta2] + k R Cos[theta1 + theta3] + d k Sin[theta1 + \ theta2] - d k Sin[theta1 + theta3]), c w2 - d gc mw Cos[theta1 + theta2] + 2 (Sqrt[2] a kdel (2 Cos[theta2] + Sin[theta2]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] (-ls + \ -------------------------------------------------------------)) / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta2] + 8 Sqrt[2] Sin[theta2])] + a Cos[theta1] b Sin[theta1] k (z - z20 + ------------- + R Cos[theta1 + theta2] + ------------- + 2 2 d Sin[theta1 + theta2]) (d Cos[theta1 + theta2] - R Sin[theta1 + \ theta2]), c w3 + d gc mw Cos[theta1 + theta3] - 2 (Sqrt[2] a kdel (2 Cos[theta3] - Sin[theta3]) 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] (-ls + \ -------------------------------------------------------------)) / 2 2 Sqrt[a (13 - 4 Sqrt[2] Cos[theta3] - 8 Sqrt[2] Sin[theta3])] + a Cos[theta1] b Sin[theta1] k (z - z30 + ------------- + R Cos[theta1 + theta3] - ------------- - 2 2 d Sin[theta1 + theta3]) (-(d Cos[theta1 + theta3]) - R Sin[theta1 + \ theta3])}, {w1, vy, vz, w2, w3}, {theta1, y, z, theta2, theta3}}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Tractor Semi-Trailer"], "Subsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "\nAn example which benefits significantly from the use of replacement rules \ is a tractor semi-trailer. The goal of a recent study was to show how \ deformable tires, tire parameters, and front end alignment parameters affect \ turning radius of a tractor semi-trailer. This example uses a simple model \ for a deformable pneumatic tire, namely, a single deformation parameter \ (proportional to sideslip angle), so that the cornering force is given by\n\n \ F=-kappa*arctan[vy/vx]\n\nOnly the computation of \ the right front tire cornoring force is given below. "], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["Main body (tractor) joint"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Joint 1 *)\nr1={3}; q1={theta,x,y}; p1={wth,vx,vy};\n\ H1={{0,0,0},{0,0,0},{1,0,0},{0,1,0},{0,0,1},{0,0,0}};\n"], "Input", PageWidth->Infinity, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Tractor front end joints"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Define castor sr (right) and sl (left) and camber tr (right) and tl (left). \ Note that positive castor and camber implies spindle points down and forward \ and inward."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Joint 2 right front spindle *)\nr2={1}; q2={delta2}; p2={wdel2}; \n\ H2=Transpose[{{-Sin[sr],-Sin[tr],1,0,0,0}}];\n\n(* Joint 3 left front spindle \ *)\nr3={1}; q3={delta3}; p3={wdel3}; \n\ H3=Transpose[{{-Sin[sl],Sin[tl],1,0,0,0}}];"], "Input", PageWidth->Infinity, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Tractor-Semi joint"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "(* Joint 4 5th wheel *)\nr4={1}; q4={delta4}; p4={wdel4};\n\ H4=Transpose[{{0,0,1,0,0,0}}];\n\n\ JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2},{r3,H3,q3,p3},{r4,H4,q4,p4}};"], "Input",\ PageWidth->Infinity, AspectRatioFixed->True] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Body Data"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "Tractor body frame is located midway between rear axles on body centerline \ and at axle hight. It is right handed with x-forward, y-left, and z-up. \n\n\t\ wb = wheelbase (186\")\n\t R=tire radius (12\")\n\t \ 2b=track (72-11/16\")\n\t pin= pin location from rear bogie (8\ \")\n\t d=tractor rear axle location from rear bogie (28\")\n\t \ tb=trailor base (388\")\n\t kappa=tire coefficient (approx \ 14,000 N?)"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "wb=186*.0254; R=12*.0254; b=(72+11/16)*.0254;\npin=8*.0254; d=28*.0254; \ tb=388*.0254;\nJtracxx=1.414*10^3; Jtracyy=5.684*10^4; \ Jtraczz=5.698*10^4;Jtracxz=1.96*10^3;\nJtrailxx=2.202*10^5; \ Jtrailyy=4.852*10^5; Jtrailzz=5.065*10^5;\nmtrac=1.15*10^4; \ mtrail=4.376*10^4; mtire=40;\nIxx=mtire*(R^2)/2; \ Iyy=mtire*(3*R^2+(11*.0254)^2)/12; Izz=Iyy;\n\n(* Body 1 tractor *)\n\ cm1={2.4466,0,.9052};\n\ out1={{2,{wb,-b,0}},{3,{wb,b,0}},{4,{pin,0,0}},{5,{d,-b,-R}},{6,{d,b,-R}},{7,{\ -d,-b,-R}},{8,{-d,b,-R}}};\n\ I1={{Jtracxx,0,Jtracxz},{0,Jtracyy,0},{Jtracxz,0,Jtraczz}};\n\n(* Body 2 \ right front wheel *)\ncm2={0,0,0}; out2={{9,{sr*R,tr*R,-R}}};\n\ I2=DiagonalMatrix[{Ixx,Iyy,Izz}];\n\n(* Body 3 left front wheel *)\n\ cm3={0,0,0}; out3={{10,{sl*R,-tl*R,-R}}};\nI3=DiagonalMatrix[{Ixx,Iyy,Izz}];\n\ \n(* Body 4 trailor *)\ncm4={-5.77,0,1.0592}; \n\ out4={{11,{-tb,-b,-R}},{12,{-tb,b,-R}}};\n\ I4=DiagonalMatrix[{Jtrailxx,Jtrailyy,Jtrailzz}];\n\n\ BodyLst={{cm1,out1,mtrac,I1},{cm2,out2,mtire,I2},{cm3,out3,mtire,I3},{cm4,\ out4,mtrail,I4}};\nTreeLst={{{1,1},{2,2}},{{1,1},{3,3}},{{1,1},{4,4}}};\n\n\ q={theta,x,y,delta2,delta3,delta4};\np={wth,vx,vy,wdel2,wdel3,wdel4};"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ General::spell: Possible spelling error: new symbol name \"Jtracxz\" is similar to existing symbols {Jtracxx, Jtraczz}. \ \>", "\<\ General::spell: Possible spelling error: new symbol name \ \"Jtracxz\" is similar to existing symbols {Jtracxx, Jtraczz}.\ \>"], "Message", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Computations Without Camber"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["tr=0.0; tl=0.0;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Joint Calculations"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics Computing joint 4 kinematics \ \>", "\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics Computing joint 4 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Tire Force Computations\nFor comparison, we compute the front right tire \ force two ways: (i) without replacements, and (ii) with replacements. In this \ case, there is an improvement from 166 sec to 4 sec."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "(* Front right tire *)\nChnLst={{1,1},{2,2}};\n\ Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};\n\ VelNames={w9x,w9y,w9z,v9x,v9y,v9z};\nTerminalNode=9;\n\ Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]"]\ , "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {166.4249999999999*Second, {-(kappa*ArcTan[(wdel2*(0.3048*sr - \ 0.3048*Sin[sr]) + (vy*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) + \ 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (2*(1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2))) \ - (I/2*vx*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.1524*(-2.*Sin[sr]*(1. + Sin[sr]^2)^(1/2) + 2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (0.1524*sr*(2.*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.92313125*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))/ (0.*wdel2 + (0.5*vx*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (0.5*I*vy*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.92313125*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.1524*I*(2.*I*Sin[sr]*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] + 2*I*Sin[sr]^3*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))]* ((0.1524*(-2.*Sin[sr]*(1. + Sin[sr]^2)^(1/2) + 2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (0.1524*sr*(2.*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (2.3622*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) \ + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) - (0.92313125*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))), (I/2*kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) + (vy*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) + \ 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (2*(1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2))) \ - (I/2*vx*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.1524*(-2.*Sin[sr]*(1. + Sin[sr]^2)^(1/2) + 2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (0.1524*sr*(2.*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.92313125*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))/ (0.*wdel2 + (0.5*vx*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (0.5*I*vy*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.92313125*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.1524*I*(2.*I*Sin[sr]*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] + 2*I*Sin[sr]^3*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))]* (-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)), -(kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) + (vy*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) \ + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (2*(1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))) - (I/2*vx*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.1524*(-2.*Sin[sr]*(1. + Sin[sr]^2)^(1/2) + 2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (0.1524*sr*(2.*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.92313125*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))/ (0.*wdel2 + (0.5*vx*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]* (1. + Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (0.5*I*vy*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.92313125*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.1524*I*(2.*I*Sin[sr]*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] + 2*I*Sin[sr]^3*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))]* (2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (2*(1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2))), -(kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) + (vy*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + Sin[sr]^2)^(1/2) + \ 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (2*(1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2))) \ - (I/2*vx*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.1524*(-2.*Sin[sr]*(1. + Sin[sr]^2)^(1/2) + 2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (0.1524*sr*(2.*(1. + Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.92313125*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))/ (0.*wdel2 + (0.5*vx*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + (0.5*I*vy*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) + wth*((0.92313125*(2.*Cos[delta2*(1. + Sin[sr]^2)^(1/2)]*(1. + \ Sin[sr]^2)^(1/2) + 2.*Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ + (2.3622*I*(-2.*I*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] - 2.*I*Sin[sr]^2*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + Sin[sr]^2)^(1/2)) \ - (0.1524*I*(2.*I*Sin[sr]*Sin[delta2*(1. + Sin[sr]^2)^(1/2)] + 2*I*Sin[sr]^3*Sin[delta2*(1. + Sin[sr]^2)^(1/2)]))/ (1.*(1. + Sin[sr]^2)^(1/2) + Sin[sr]^2*(1. + \ Sin[sr]^2)^(1/2))))]* (0.3048*sr - 0.3048*Sin[sr])), 0, 0}} \ \>", "\<\ {166.425 Second, {-(kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 \ Sin[sr]) + 2 2 (vy (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] \ ])) / 2 2 2 (2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])) - I 2 (- vx (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 wth ((0.1524 (-2. Sin[sr] Sqrt[1. + Sin[sr] ] + 2 \ 2 2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.1524 sr (2. Sqrt[1. + Sin[sr] ] + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 2 (2.3622 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] \ + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.923131 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]))) / 2 \ 2 (0. wdel2 + (0.5 vx (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.5 I vy (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 \ 2 wth ((0.923131 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (2.3622 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.1524 I (2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]] + 3 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])))] 2 ((0.1524 (-2. Sin[sr] Sqrt[1. + Sin[sr] ] + 2 2 2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] ])) \ / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.1524 sr (2. Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] ])) \ / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 2 (2.3622 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] ])) \ / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.923131 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]))), I (- kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) + 2 2 2 (vy (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] \ ])) / 2 2 2 (2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])) - I 2 (- vx (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 wth ((0.1524 (-2. Sin[sr] Sqrt[1. + Sin[sr] ] + 2 \ 2 2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.1524 sr (2. Sqrt[1. + Sin[sr] ] + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 2 (2.3622 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] \ + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.923131 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]))) / 2 \ 2 (0. wdel2 + (0.5 vx (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.5 I vy (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 \ 2 wth ((0.923131 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (2.3622 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.1524 I (2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]] + 3 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])))] 2 2 \ 2 (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 I Sin[sr] Sin[delta2 \ Sqrt[1. + Sin[sr] ]]) 2 2 2 ) / (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]), -(kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) + 2 2 (vy (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] \ ])) / 2 2 2 (2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])) - I 2 (- vx (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 wth ((0.1524 (-2. Sin[sr] Sqrt[1. + Sin[sr] ] + 2 \ 2 2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.1524 sr (2. Sqrt[1. + Sin[sr] ] + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 2 (2.3622 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] \ + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.923131 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]))) / 2 \ 2 (0. wdel2 + (0.5 vx (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.5 I vy (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 \ 2 wth ((0.923131 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (2.3622 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.1524 I (2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]] + 3 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])))] 2 2 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])), -(kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) + 2 2 (vy (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] + 2 2 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + Sin[sr] \ ])) / 2 2 2 (2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])) - I 2 (- vx (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 wth ((0.1524 (-2. Sin[sr] Sqrt[1. + Sin[sr] ] + 2 \ 2 2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.1524 sr (2. Sqrt[1. + Sin[sr] ] + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 2 (2.3622 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + Sin[sr] ] \ + 2 2 \ 2 2 Cos[delta2 Sqrt[1. + Sin[sr] ]] Sin[sr] Sqrt[1. + \ Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.923131 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]))) / 2 \ 2 (0. wdel2 + (0.5 vx (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (0.5 I vy (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 \ 2 wth ((0.923131 (2. Cos[delta2 Sqrt[1. + Sin[sr] ]] Sqrt[1. + \ Sin[sr] ] + 2 2 2. Sin[sr] Sqrt[1. + Sin[sr] ])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) + 2 (2.3622 I (-2. I Sin[delta2 Sqrt[1. + Sin[sr] ]] - 2 2 2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ]) - 2 (0.1524 I (2. I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]] + 3 2 2 I Sin[sr] Sin[delta2 Sqrt[1. + Sin[sr] ]])) / 2 2 2 (1. Sqrt[1. + Sin[sr] ] + Sin[sr] Sqrt[1. + Sin[sr] ])))] (0.3048 sr - 0.3048 Sin[sr])), 0, 0}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "{V,X,H,rules1,rules2}=KinematicReplacements[V,X,H,q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["rules1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {t3 -> Sin[(delta3*t12)/2], t4 -> Sin[delta3*t12], t5 -> \ Sin[(delta2*t15)/2], t6 -> Sin[delta2*t15], t7 -> Cos[delta4], t8 -> Cos[theta], t9 -> \ Cos[delta3*t12], t10 -> Cos[delta2*t15], t17 -> Sin[(delta3*t12)/2]^2, t18 -> \ Sin[(delta2*t15)/2]^2} \ \>", "\<\ delta3 t12 delta2 \ t15 {t3 -> Sin[----------], t4 -> Sin[delta3 t12], t5 -> Sin[----------], 2 2 t6 -> Sin[delta2 t15], t7 -> Cos[delta4], t8 -> Cos[theta], t9 -> \ Cos[delta3 t12], delta3 t12 2 delta2 t15 2 t10 -> Cos[delta2 t15], t17 -> Sin[----------] , t18 -> Sin[----------] } 2 2\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "(* Front right tire *)\nChnLst={{1,1},{2,2}};\n\ Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};\n\ VelNames={w9x,w9y,w9z,v9x,v9y,v9z};\nTerminalNode=9;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]\ "], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {4.286000000000102*Second, {-(kappa*(4.724400000000001*t10*t15*t16 + 4.724400000000001*t10*t14*t15*t16 + 0.3048*sr*(1.*t15 + \ t10*t14*t15)*t16 - 0.3048*t15*t16*t2 + 0.3048*t10*t15*t16*t2 - 1.8462625*t16*t6 - \ 1.8462625*t14*t16*t6) *ArcTan[(-(t16*(1.*t6 + t14*t6)*vx) + (1.*t10*t15 + \ t10*t14*t15)*t16*vy + (0.3048*sr - 0.3048*t2)*wdel2 + (4.724400000000001*t10*t15*t16 + 4.724400000000001*t10*t14*t15*t16 \ + 0.3048*sr*(1.*t15 + t10*t14*t15)*t16 - 0.3048*t15*t16*t2 + 0.3048*t10*t15*t16*t2 - 1.8462625*t16*t6 - \ 1.8462625*t14*t16*t6)*wth)/ ((1.*t10*t15*t16 + 1.*t14*t15*t16)*vx + (1.*t16*t6 + \ 1.*t14*t16*t6)*vy + (1.8462625*t10*t15*t16 + 1.8462625*t14*t15*t16 + \ 4.724400000000001*t16*t6 + 4.724400000000001*t14*t16*t6 + 0.3048*t16*t2*t6 + \ 0.3048*t16*t2^3*t6)*wth)]), kappa*t16*(1.*t6 + t14*t6)*ArcTan[(-(t16*(1.*t6 + t14*t6)*vx) + (1.*t10*t15 + t10*t14*t15)*t16*vy + (0.3048*sr - 0.3048*t2)*wdel2 + (4.724400000000001*t10*t15*t16 + 4.724400000000001*t10*t14*t15*t16 + 0.3048*sr*(1.*t15 + t10*t14*t15)*t16 - 0.3048*t15*t16*t2 + 0.3048*t10*t15*t16*t2 - 1.8462625*t16*t6 - \ 1.8462625*t14*t16*t6)*wth)/ ((1.*t10*t15*t16 + 1.*t14*t15*t16)*vx + (1.*t16*t6 + 1.*t14*t16*t6)*vy \ + (1.8462625*t10*t15*t16 + 1.8462625*t14*t15*t16 + \ 4.724400000000001*t16*t6 + 4.724400000000001*t14*t16*t6 + 0.3048*t16*t2*t6 + \ 0.3048*t16*t2^3*t6)*wth)], -(kappa*(1.*t10*t15 + t10*t14*t15)*t16* ArcTan[(-(t16*(1.*t6 + t14*t6)*vx) + (1.*t10*t15 + t10*t14*t15)*t16*vy \ + (0.3048*sr - 0.3048*t2)*wdel2 + (4.724400000000001*t10*t15*t16 + 4.724400000000001*t10*t14*t15*t16 \ + 0.3048*sr*(1.*t15 + t10*t14*t15)*t16 - 0.3048*t15*t16*t2 + 0.3048*t10*t15*t16*t2 - 1.8462625*t16*t6 - \ 1.8462625*t14*t16*t6)*wth)/ ((1.*t10*t15*t16 + 1.*t14*t15*t16)*vx + (1.*t16*t6 + \ 1.*t14*t16*t6)*vy + (1.8462625*t10*t15*t16 + 1.8462625*t14*t15*t16 + \ 4.724400000000001*t16*t6 + 4.724400000000001*t14*t16*t6 + 0.3048*t16*t2*t6 + \ 0.3048*t16*t2^3*t6)*wth)]), -(kappa*(0.3048*sr - 0.3048*t2)*ArcTan[(-(t16*(1.*t6 + t14*t6)*vx) + (1.*t10*t15 + t10*t14*t15)*t16*vy + (0.3048*sr - 0.3048*t2)*wdel2 + \ (4.724400000000001*t10*t15*t16 + 4.724400000000001*t10*t14*t15*t16 \ + 0.3048*sr*(1.*t15 + t10*t14*t15)*t16 - 0.3048*t15*t16*t2 + 0.3048*t10*t15*t16*t2 - 1.8462625*t16*t6 - \ 1.8462625*t14*t16*t6)*wth)/ ((1.*t10*t15*t16 + 1.*t14*t15*t16)*vx + (1.*t16*t6 + \ 1.*t14*t16*t6)*vy + (1.8462625*t10*t15*t16 + 1.8462625*t14*t15*t16 + \ 4.724400000000001*t16*t6 + 4.724400000000001*t14*t16*t6 + 0.3048*t16*t2*t6 + \ 0.3048*t16*t2^3*t6)*wth)]), 0, 0}} \ \>", "\<\ {4.286 Second, {-(kappa (4.7244 t10 t15 t16 + 4.7244 t10 t14 t15 \ t16 + 0.3048 sr (1. t15 + t10 t14 t15) t16 - 0.3048 t15 t16 t2 + 0.3048 t10 \ t15 t16 t2 - 1.84626 t16 t6 - 1.84626 t14 t16 t6) ArcTan[(-(t16 (1. t6 + t14 t6) vx) + (1. t10 t15 + t10 t14 t15) t16 vy \ + (0.3048 sr - 0.3048 t2) wdel2 + (4.7244 t10 t15 t16 + 4.7244 t10 t14 t15 t16 + 0.3048 sr (1. t15 + t10 t14 t15) t16 - 0.3048 t15 t16 t2 + 0.3048 t10 t15 t16 t2 - 1.84626 t16 t6 - 1.84626 t14 t16 t6) \ wth) / ((1. t10 t15 t16 + 1. t14 t15 t16) vx + (1. t16 t6 + 1. t14 t16 t6) \ vy + (1.84626 t10 t15 t16 + 1.84626 t14 t15 t16 + 4.7244 t16 t6 + 4.7244 \ t14 t16 t6 + 3 0.3048 t16 t2 t6 + 0.3048 t16 t2 t6) wth)]), kappa t16 (1. t6 + t14 t6) ArcTan[(-(t16 (1. t6 + t14 t6) vx) + (1. t10 t15 + t10 t14 t15) t16 vy + (0.3048 sr - 0.3048 t2) wdel2 + (4.7244 t10 t15 t16 + 4.7244 t10 t14 t15 t16 + 0.3048 sr (1. t15 + t10 t14 t15) t16 - 0.3048 t15 t16 t2 + 0.3048 t10 t15 t16 t2 - 1.84626 t16 t6 - 1.84626 t14 t16 t6) wth) \ / ((1. t10 t15 t16 + 1. t14 t15 t16) vx + (1. t16 t6 + 1. t14 t16 t6) vy \ + (1.84626 t10 t15 t16 + 1.84626 t14 t15 t16 + 4.7244 t16 t6 + 4.7244 \ t14 t16 t6 + 3 0.3048 t16 t2 t6 + 0.3048 t16 t2 t6) wth)], -(kappa (1. t10 t15 + t10 t14 t15) t16 ArcTan[(-(t16 (1. t6 + t14 t6) vx) + (1. t10 t15 + t10 t14 t15) t16 vy \ + (0.3048 sr - 0.3048 t2) wdel2 + (4.7244 t10 t15 t16 + 4.7244 t10 t14 t15 t16 + 0.3048 sr (1. t15 + t10 t14 t15) t16 - 0.3048 t15 t16 t2 + 0.3048 t10 t15 t16 t2 - 1.84626 t16 t6 - 1.84626 t14 t16 t6) \ wth) / ((1. t10 t15 t16 + 1. t14 t15 t16) vx + (1. t16 t6 + 1. t14 t16 t6) \ vy + (1.84626 t10 t15 t16 + 1.84626 t14 t15 t16 + 4.7244 t16 t6 + 4.7244 \ t14 t16 t6 + 3 0.3048 t16 t2 t6 + 0.3048 t16 t2 t6) wth)]), -(kappa (0.3048 sr - 0.3048 t2) ArcTan[(-(t16 (1. t6 + t14 t6) vx) + (1. t10 t15 + t10 t14 t15) t16 vy + (0.3048 sr - 0.3048 t2) wdel2 + \ (4.7244 t10 t15 t16 + 4.7244 t10 t14 t15 t16 + 0.3048 sr (1. t15 + t10 t14 t15) t16 - 0.3048 t15 t16 t2 + 0.3048 t10 t15 t16 t2 - 1.84626 t16 t6 - 1.84626 t14 t16 t6) \ wth) / ((1. t10 t15 t16 + 1. t14 t15 t16) vx + (1. t16 t6 + 1. t14 t16 t6) \ vy + (1.84626 t10 t15 t16 + 1.84626 t14 t15 t16 + 4.7244 t16 t6 + 4.7244 \ t14 t16 t6 + 3 0.3048 t16 t2 t6 + 0.3048 t16 t2 t6) wth)]), 0, 0}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Computations With Small Numerical Camber"], "Subsubsection", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData["tr=0.01; tl=0.01;"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[TextData["Joint Calculations"], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["{V,X,H}=Joints[JointLst];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics Computing joint 4 kinematics \ \>", "\<\ Computing joint 1 kinematics Computing joint 2 kinematics Computing joint 3 kinematics Computing joint 4 kinematics\ \>"], "Print", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "Tire Force Computations\nAs before, compute the front right tire forces. In \ this case, there is an improvement from 597 sec to 18 sec."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "(* Front right tire *)\nChnLst={{1,1},{2,2}};\n\ Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};\n\ VelNames={w9x,w9y,w9z,v9x,v9y,v9z};\nTerminalNode=9;\n\ Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]"]\ , "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {597.3119999999994*Second, {-(kappa*ArcTan[(wdel2*(0.3048*sr - \ 0.3048*Sin[sr]) - (100.0016666861113*vy*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (2*vx*(1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-472.4478740918645*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (0.3048*sr*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ - (3.692525*(1 + Sin[sr]^2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (30.48050800592673*(0.00999983333416667*Sin[sr] - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr] + 0.00999983333416667*Sin[sr]^3 - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr]^3 - 0.0000999966667111108*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^4*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] - Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(3/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))/ (-(5.079974600046347*10^-8)*wdel2 - (100.0016666861113*vx*(-0.004374927083697916* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 0.004999916667083333*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] - 0.0006249895833854166*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[4*sr] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.00750062496312581*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (10000.33334000011*vy*(8.74956250946843*10^-7* (6.000399986666844 - 2*Cos[2*sr])^(1/2)*Sin[sr] - 1.249937501349163*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ + 0.00007499999986670947* Sin[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 6.249791669438176*10^-6* Sin[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.00007499999986670947* Sin[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249791669438176*10^-6* Sin[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.0002375070824385261*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] ))/((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-0.003048*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ + (1.846262499999999*(14.*(6.000399986666844 - 2*Cos[2*sr])^(1/2) \ - 16.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 2.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[4*sr] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 24.00239992000106*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.7164798899986483*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[sr] - 0.1023542699995228*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 61.4156326059134*Sin[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 5.117798796211564*Sin[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 61.4156326059134*Sin[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 5.117798796211564*Sin[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 194.4886365636054*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2]) )/((6.000399986666844 - 2*Cos[2*sr])^(1/2)* (38.00239992000107 - 24.00079997333369*Cos[2*sr] + \ 2*Cos[4*sr])) - (0.6096*(1 + Sin[sr]^2)* (-(Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]* (1.000099996666711 + Sin[sr]^2)^(1/2)) - 0.00999983333416667*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]) *Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2])/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))]* ((-472.4478740918645*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (0.3048*sr*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2 + \ Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) - (3.692525*(1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 \ - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (30.48050800592673*(0.00999983333416667*Sin[sr] - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr] + 0.00999983333416667*Sin[sr]^3 - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr]^3 - 0.0000999966667111108*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^4*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] - Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(3/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8))), (2*kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) - (100.0016666861113*vy*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (2*vx*(1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-472.4478740918645*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (0.3048*sr*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ - (3.692525*(1 + Sin[sr]^2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (30.48050800592673*(0.00999983333416667*Sin[sr] - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr] + 0.00999983333416667*Sin[sr]^3 - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr]^3 - 0.0000999966667111108*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^4*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] - Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(3/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))/ (-(5.079974600046347*10^-8)*wdel2 - (100.0016666861113*vx*(-0.004374927083697916* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 0.004999916667083333*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] - 0.0006249895833854166*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[4*sr] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.00750062496312581*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (10000.33334000011*vy*(8.74956250946843*10^-7* (6.000399986666844 - 2*Cos[2*sr])^(1/2)*Sin[sr] - 1.249937501349163*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ + 0.00007499999986670947* Sin[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 6.249791669438176*10^-6* Sin[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.00007499999986670947* Sin[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249791669438176*10^-6* Sin[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.0002375070824385261*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] ))/((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-0.003048*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ + (1.846262499999999*(14.*(6.000399986666844 - 2*Cos[2*sr])^(1/2) \ - 16.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 2.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[4*sr] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 24.00239992000106*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.7164798899986483*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[sr] - 0.1023542699995228*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 61.4156326059134*Sin[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 5.117798796211564*Sin[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 61.4156326059134*Sin[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 5.117798796211564*Sin[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 194.4886365636054*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2]) )/((6.000399986666844 - 2*Cos[2*sr])^(1/2)* (38.00239992000107 - 24.00079997333369*Cos[2*sr] + \ 2*Cos[4*sr])) - (0.6096*(1 + Sin[sr]^2)* (-(Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]* (1.000099996666711 + Sin[sr]^2)^(1/2)) - 0.00999983333416667*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]) *Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2])/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))]* (1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)), (100.0016666861113*kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) - (100.0016666861113*vy*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (2*vx*(1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-472.4478740918645*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (0.3048*sr*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ - (3.692525*(1 + Sin[sr]^2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (30.48050800592673*(0.00999983333416667*Sin[sr] - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr] + 0.00999983333416667*Sin[sr]^3 - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr]^3 - 0.0000999966667111108*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^4*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] - Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(3/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))/ (-(5.079974600046347*10^-8)*wdel2 - (100.0016666861113*vx*(-0.004374927083697916* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 0.004999916667083333*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] - 0.0006249895833854166*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[4*sr] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.00750062496312581*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (10000.33334000011*vy*(8.74956250946843*10^-7* (6.000399986666844 - 2*Cos[2*sr])^(1/2)*Sin[sr] - 1.249937501349163*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ + 0.00007499999986670947* Sin[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 6.249791669438176*10^-6* Sin[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.00007499999986670947* Sin[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249791669438176*10^-6* Sin[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.0002375070824385261*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] ))/((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-0.003048*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ + (1.846262499999999*(14.*(6.000399986666844 - 2*Cos[2*sr])^(1/2) \ - 16.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 2.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[4*sr] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 24.00239992000106*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.7164798899986483*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[sr] - 0.1023542699995228*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 61.4156326059134*Sin[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 5.117798796211564*Sin[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 61.4156326059134*Sin[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 5.117798796211564*Sin[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 194.4886365636054*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2]) )/((6.000399986666844 - 2*Cos[2*sr])^(1/2)* (38.00239992000107 - 24.00079997333369*Cos[2*sr] + \ 2*Cos[4*sr])) - (0.6096*(1 + Sin[sr]^2)* (-(Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]* (1.000099996666711 + Sin[sr]^2)^(1/2)) - 0.00999983333416667*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]) *Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2])/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))]* (-(7.499625008114926*10^-7)*(6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)), -(kappa*ArcTan[(wdel2*(0.3048*sr - 0.3048*Sin[sr]) - (100.0016666861113*vy*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (2*vx*(1 + Sin[sr]^2)*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-472.4478740918645*(-(7.499625008114926*10^-7)* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 2.499875002707*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.003749937500312499*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.0003124947916927083*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.01187480208432291*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (0.3048*sr*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ - (3.692525*(1 + Sin[sr]^2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]* (1.000099996666711*Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2] + Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]^2 - 0.00999983333416667*Sin[sr]*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) - (30.48050800592673*(0.00999983333416667*Sin[sr] - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr] + 0.00999983333416667*Sin[sr]^3 - 0.00999983333416667*Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]* Sin[sr]^3 - 0.0000999966667111108*(1.000099996666711 + \ Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + Sin[sr]^4*(1.000099996666711 + Sin[sr]^2)^(1/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] - Sin[sr]^2*(1.000099996666711 + Sin[sr]^2)^(3/2)* Sin[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)]))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))/ (-(5.079974600046347*10^-8)*wdel2 - (100.0016666861113*vx*(-0.004374927083697916* (6.000399986666844 - 2*Cos[2*sr])^(1/2) + 0.004999916667083333*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[2*sr] - 0.0006249895833854166*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Cos[4*sr] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.001250104160520968*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 0.00750062496312581*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2]))/ ((2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + (10000.33334000011*vy*(8.74956250946843*10^-7* (6.000399986666844 - 2*Cos[2*sr])^(1/2)*Sin[sr] - 1.249937501349163*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ + 0.00007499999986670947* Sin[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 6.249791669438176*10^-6* Sin[4*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.374781254732044*10^-7*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] \ - 0.00007499999986670947* Sin[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249687506745816*10^-8*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 6.249791669438176*10^-6* Sin[4*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 0.0002375070824385261*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] ))/((2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)* (1.000099996666711 + Sin[sr]^2)^(1/2)) + wth*((-0.003048*(1 + 0.0000999966667111108* Cos[delta2*(1.000099996666711 + Sin[sr]^2)^(1/2)] + \ Sin[sr]^2 + Cos[delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2)]*Sin[sr]^2* (1.000099996666711 + Sin[sr]^2)))/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + Cos[4*sr]/8) \ + (1.846262499999999*(14.*(6.000399986666844 - 2*Cos[2*sr])^(1/2) \ - 16.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[2*sr] + 2.*(6.000399986666844 - 2*Cos[2*sr])^(1/2)*Cos[4*sr] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 4.000399986666844*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[2*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 24.00239992000106*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Cos[(delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.7164798899986483*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[sr] - 0.1023542699995228*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)*Sin[3*sr] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 61.4156326059134*Sin[2*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr - (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 5.117798796211564*Sin[4*sr - (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] - 0.3582399449991466*(6.000399986666844 - 2*Cos[2*sr])^(1/2)* Sin[sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] - 61.4156326059134*Sin[2*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 0.05117713499976142*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2)* Sin[3*sr + (delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2] + 5.117798796211564*Sin[4*sr + (delta2*(6.000399986666844 - 2*Cos[2*sr])^(1/2))/2] + 194.4886365636054*Sin[(delta2*(6.000399986666844 - \ 2*Cos[2*sr])^(1/2))/2]) )/((6.000399986666844 - 2*Cos[2*sr])^(1/2)* (38.00239992000107 - 24.00079997333369*Cos[2*sr] + \ 2*Cos[4*sr])) - (0.6096*(1 + Sin[sr]^2)* (-(Cos[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]*Sin[sr]* (1.000099996666711 + Sin[sr]^2)^(1/2)) - 0.00999983333416667*Sin[(delta2*(1.000099996666711 + \ Sin[sr]^2)^(1/2))/2]) *Sin[(delta2*(1.000099996666711 + Sin[sr]^2)^(1/2))/2])/ (2.375149995000067 - 1.500049998333355*Cos[2*sr] + \ Cos[4*sr]/8)))]* (0.3048*sr - 0.3048*Sin[sr])), 0, 0}} \ \>", "\<\ {597.312 Second, {-(kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 \ Sin[sr]) - -7 (100.002 vy (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) 2 Cos[4 sr] \ 2 / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (2 vx (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 -7 wth ((-472.448 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 2 \ 2 (0.3048 sr (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] \ + Sin[sr] + 2 2 \ 2 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] (1.0001 + \ Sin[sr] ))) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (3.69252 (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 (30.4805 (0.00999983 Sin[sr] - 2 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] + 3 0.00999983 Sin[sr] - 2 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] - 2 0.0000999967 Sqrt[1.0001 + Sin[sr] ] 2 Sin[delta2 Sqrt[1.0001 + Sin[sr] ]] + 2 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 4 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] - 2 2 3/2 \ 2 Sin[sr] (1.0001 + Sin[sr] ) Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]])) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------))) / 8 -8 (-5.07997 10 wdel2 - (100.002 vx (-0.00437493 Sqrt[6.0004 - 2 Cos[2 sr]] + 0.00499992 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] - 0.00062499 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] + 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] + 2 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.00750062 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------]) 2 Cos[4 sr] \ 2 ) / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 -7 (10000.3 vy (8.74956 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - -7 1.24994 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr - ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr - ---------------------------------] - 2 -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr + ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000237507 Sin[---------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 2 wth ((-0.003048 (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] \ ]] + 2 2 2 Sin[sr] + Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] 2 \ Cos[4 sr] (1.0001 + Sin[sr] ))) / (2.37515 - 1.50005 Cos[2 sr] + \ ---------) + \ 8 (1.84626 (14. Sqrt[6.0004 - 2 Cos[2 sr]] - 16. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 2. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] - 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] + 2 24.0024 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------] + 2 0.71648 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - 0.102354 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr - ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr - ---------------------------------] - 2 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr + ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 194.489 Sin[---------------------------------])) / 2 (Sqrt[6.0004 - 2 Cos[2 sr]] (38.0024 - 24.0008 Cos[2 sr] + 2 \ Cos[4 sr])) - 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (0.6096 (1 + Sin[sr] ) (-(Cos[------------------------------] \ Sin[sr] 2 2 Sqrt[1.0001 + Sin[sr] ]) - 2 delta2 Sqrt[1.0001 + Sin[sr] ] 0.00999983 Sin[------------------------------]) 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------]) / 2 Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------)))] 8 -7 ((-472.448 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 \ Cos[2 sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) \\ 2 Cos[4 sr] 2 / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 2 \ 2 (0.3048 sr (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] + \ Sin[sr] + 2 2 2 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] (1.0001 + Sin[sr] \ ))) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (3.69252 (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 \ 2 2 delta2 \ Sqrt[1.0001 + Sin[sr] ] 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] \ Sin[------------------------------]) 2 Cos[4 sr] \ 2 ) / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 (30.4805 (0.00999983 Sin[sr] - 2 \ 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] + \ 0.00999983 Sin[sr] - 2 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] - 2 \ 2 0.0000999967 Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 2 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 4 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] - 2 2 3/2 \ 2 Sin[sr] (1.0001 + Sin[sr] ) Sin[delta2 Sqrt[1.0001 + Sin[sr] \ ]])) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------))), 8 (2 kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) - -7 (100.002 vy (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) 2 Cos[4 sr] \ 2 / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (2 vx (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 -7 wth ((-472.448 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 2 \ 2 (0.3048 sr (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] \ + Sin[sr] + 2 2 \ 2 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] (1.0001 + \ Sin[sr] ))) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (3.69252 (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 (30.4805 (0.00999983 Sin[sr] - 2 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] + 3 0.00999983 Sin[sr] - 2 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] - 2 0.0000999967 Sqrt[1.0001 + Sin[sr] ] 2 Sin[delta2 Sqrt[1.0001 + Sin[sr] ]] + 2 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 4 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] - 2 2 3/2 \ 2 Sin[sr] (1.0001 + Sin[sr] ) Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]])) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------))) / 8 -8 (-5.07997 10 wdel2 - (100.002 vx (-0.00437493 Sqrt[6.0004 - 2 Cos[2 sr]] + 0.00499992 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] - 0.00062499 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] + 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] + 2 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.00750062 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------]) 2 Cos[4 sr] \ 2 ) / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 -7 (10000.3 vy (8.74956 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - -7 1.24994 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr - ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr - ---------------------------------] - 2 -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr + ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000237507 Sin[---------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 2 wth ((-0.003048 (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] \ ]] + 2 2 2 Sin[sr] + Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] 2 \ Cos[4 sr] (1.0001 + Sin[sr] ))) / (2.37515 - 1.50005 Cos[2 sr] + \ ---------) + \ 8 (1.84626 (14. Sqrt[6.0004 - 2 Cos[2 sr]] - 16. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 2. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] - 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] + 2 24.0024 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------] + 2 0.71648 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - 0.102354 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr - ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr - ---------------------------------] - 2 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr + ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 194.489 Sin[---------------------------------])) / 2 (Sqrt[6.0004 - 2 Cos[2 sr]] (38.0024 - 24.0008 Cos[2 sr] + 2 \ Cos[4 sr])) - 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (0.6096 (1 + Sin[sr] ) (-(Cos[------------------------------] \ Sin[sr] 2 2 Sqrt[1.0001 + Sin[sr] ]) - 2 delta2 Sqrt[1.0001 + Sin[sr] ] 0.00999983 Sin[------------------------------]) 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------]) / 2 Cos[4 sr] 2 (2.37515 - 1.50005 Cos[2 sr] + ---------)))] (1 + Sin[sr] ) 8 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 \ 2 2 delta2 Sqrt[1.0001 + \ Sin[sr] ] 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] \ Sin[------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] ]), 8 (100.002 kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) - -7 (100.002 vy (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) 2 Cos[4 sr] \ 2 / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (2 vx (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 -7 wth ((-472.448 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 2 \ 2 (0.3048 sr (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] \ + Sin[sr] + 2 2 \ 2 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] (1.0001 + \ Sin[sr] ))) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (3.69252 (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 (30.4805 (0.00999983 Sin[sr] - 2 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] + 3 0.00999983 Sin[sr] - 2 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] - 2 0.0000999967 Sqrt[1.0001 + Sin[sr] ] 2 Sin[delta2 Sqrt[1.0001 + Sin[sr] ]] + 2 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 4 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] - 2 2 3/2 \ 2 Sin[sr] (1.0001 + Sin[sr] ) Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]])) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------))) / 8 -8 (-5.07997 10 wdel2 - (100.002 vx (-0.00437493 Sqrt[6.0004 - 2 Cos[2 sr]] + 0.00499992 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] - 0.00062499 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] + 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] + 2 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.00750062 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------]) 2 Cos[4 sr] \ 2 ) / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 -7 (10000.3 vy (8.74956 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - -7 1.24994 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr - ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr - ---------------------------------] - 2 -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr + ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000237507 Sin[---------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 2 wth ((-0.003048 (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] \ ]] + 2 2 2 Sin[sr] + Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] 2 \ Cos[4 sr] (1.0001 + Sin[sr] ))) / (2.37515 - 1.50005 Cos[2 sr] + \ ---------) + \ 8 (1.84626 (14. Sqrt[6.0004 - 2 Cos[2 sr]] - 16. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 2. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] - 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] + 2 24.0024 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------] + 2 0.71648 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - 0.102354 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr - ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr - ---------------------------------] - 2 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr + ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 194.489 Sin[---------------------------------])) / 2 (Sqrt[6.0004 - 2 Cos[2 sr]] (38.0024 - 24.0008 Cos[2 sr] + 2 \ Cos[4 sr])) - 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (0.6096 (1 + Sin[sr] ) (-(Cos[------------------------------] \ Sin[sr] 2 2 Sqrt[1.0001 + Sin[sr] ]) - 2 delta2 Sqrt[1.0001 + Sin[sr] ] 0.00999983 Sin[------------------------------]) 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------]) / 2 Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------)))] 8 -7 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 \ sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] ]), 8 -(kappa ArcTan[(wdel2 (0.3048 sr - 0.3048 Sin[sr]) - -7 (100.002 vy (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------])) 2 Cos[4 sr] \ 2 / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (2 vx (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 -7 wth ((-472.448 (-7.49963 10 Sqrt[6.0004 - 2 Cos[2 sr]] + -7 2.49988 10 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr - ---------------------------------] + 2 0.00374994 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 0.000312495 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr + ---------------------------------] - 2 0.0118748 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 2 \ 2 (0.3048 sr (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] \ + Sin[sr] + 2 2 \ 2 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] (1.0001 + \ Sin[sr] ))) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------) - 8 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (3.69252 (1 + Sin[sr] ) Sin[------------------------------] 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (1.0001 Cos[------------------------------] + 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] 2 Cos[------------------------------] Sin[sr] - 2 2 0.00999983 Sin[sr] Sqrt[1.0001 + Sin[sr] ] 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------])) / 2 Cos[4 sr] \ 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) - 8 (30.4805 (0.00999983 Sin[sr] - 2 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] + 3 0.00999983 Sin[sr] - 2 3 0.00999983 Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] - 2 0.0000999967 Sqrt[1.0001 + Sin[sr] ] 2 Sin[delta2 Sqrt[1.0001 + Sin[sr] ]] + 2 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] + 4 2 \ 2 Sin[sr] Sqrt[1.0001 + Sin[sr] ] Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]] - 2 2 3/2 \ 2 Sin[sr] (1.0001 + Sin[sr] ) Sin[delta2 Sqrt[1.0001 + \ Sin[sr] ]])) / Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------))) / 8 -8 (-5.07997 10 wdel2 - (100.002 vx (-0.00437493 Sqrt[6.0004 - 2 Cos[2 sr]] + 0.00499992 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] - 0.00062499 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] + 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] + 2 0.0012501 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - \ 2 Cos[2 sr]] 0.00750062 Sqrt[6.0004 - 2 Cos[2 sr]] \ Cos[---------------------------------]) 2 Cos[4 sr] \ 2 ) / ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + \ Sin[sr] ]) + 8 -7 (10000.3 vy (8.74956 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - -7 1.24994 10 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr - ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr - ---------------------------------] - 2 -7 4.37478 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000075 Sin[2 sr + ---------------------------------] + 2 -8 6.24969 10 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 -6 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 6.24979 10 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 0.000237507 Sin[---------------------------------])) / 2 Cos[4 sr] 2 ((2.37515 - 1.50005 Cos[2 sr] + ---------) Sqrt[1.0001 + Sin[sr] \ ]) + 8 2 wth ((-0.003048 (1 + 0.0000999967 Cos[delta2 Sqrt[1.0001 + Sin[sr] \ ]] + 2 2 2 Sin[sr] + Cos[delta2 Sqrt[1.0001 + Sin[sr] ]] Sin[sr] 2 \ Cos[4 sr] (1.0001 + Sin[sr] ))) / (2.37515 - 1.50005 Cos[2 sr] + \ ---------) + \ 8 (1.84626 (14. Sqrt[6.0004 - 2 Cos[2 sr]] - 16. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr] + 2. Sqrt[6.0004 - 2 Cos[2 sr]] Cos[4 sr] - 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr - ---------------------------------] - 2 4.0004 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[2 sr + ---------------------------------] + 2 24.0024 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Cos[---------------------------------] + 2 0.71648 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr] - 0.102354 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr] - 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr - ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr - ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr - ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr - ---------------------------------] - 2 0.35824 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[sr + ---------------------------------] - 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 61.4156 Sin[2 sr + ---------------------------------] + 2 0.0511771 Sqrt[6.0004 - 2 Cos[2 sr]] delta2 Sqrt[6.0004 - 2 Cos[2 sr]] Sin[3 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 5.1178 Sin[4 sr + ---------------------------------] + 2 delta2 Sqrt[6.0004 - 2 Cos[2 sr]] 194.489 Sin[---------------------------------])) / 2 (Sqrt[6.0004 - 2 Cos[2 sr]] (38.0024 - 24.0008 Cos[2 sr] + 2 \ Cos[4 sr])) - 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] (0.6096 (1 + Sin[sr] ) (-(Cos[------------------------------] \ Sin[sr] 2 2 Sqrt[1.0001 + Sin[sr] ]) - 2 delta2 Sqrt[1.0001 + Sin[sr] ] 0.00999983 Sin[------------------------------]) 2 2 delta2 Sqrt[1.0001 + Sin[sr] ] Sin[------------------------------]) / 2 Cos[4 sr] (2.37515 - 1.50005 Cos[2 sr] + ---------)))] (0.3048 sr - \ 0.3048 Sin[sr])), 8 0, 0}} \ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "{V,X,H,rules1,rules2}=KinematicReplacements[V,X,H,q];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData["rules1"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData[ "\<\ {t5 -> Sin[sl - (delta3*t47)/2], t6 -> Sin[2*sl - (delta3*t47)/2], t7 -> Sin[3*sl - (delta3*t47)/2], t8 -> Sin[4*sl - (delta3*t47)/2], t9 -> Sin[sl + (delta3*t47)/2], t10 -> Sin[2*sl + (delta3*t47)/2], t11 -> Sin[3*sl + (delta3*t47)/2], t12 -> Sin[4*sl + (delta3*t47)/2], t13 -> Sin[(delta3*t47)/2], t14 -> Sin[sr - (delta2*t49)/2], t15 -> Sin[2*sr - (delta2*t49)/2], t16 -> Sin[3*sr - (delta2*t49)/2], t17 -> Sin[4*sr - (delta2*t49)/2], t18 -> Sin[sr + (delta2*t49)/2], t19 -> Sin[2*sr + (delta2*t49)/2], t20 -> Sin[3*sr + (delta2*t49)/2], t21 -> Sin[4*sr + (delta2*t49)/2], t22 -> Sin[(delta2*t49)/2], t23 -> Sin[(delta3*t55)/2], t24 -> Sin[delta3*t55], t25 -> \ Sin[(delta2*t61)/2], t26 -> Sin[delta2*t61], t27 -> Cos[delta4], t32 -> Cos[theta], t33 -> Cos[2*sl - (delta3*t47)/2], t34 -> Cos[4*sl - (delta3*t47)/2], t35 -> Cos[2*sl + (delta3*t47)/2], t36 -> Cos[4*sl + (delta3*t47)/2], t37 -> Cos[(delta3*t47)/2], t38 -> Cos[2*sr - (delta2*t49)/2], t39 -> Cos[4*sr - (delta2*t49)/2], t40 -> Cos[2*sr + (delta2*t49)/2], t41 -> Cos[4*sr + (delta2*t49)/2], t42 -> Cos[(delta2*t49)/2], t43 -> Cos[(delta3*t55)/2], t44 -> Cos[delta3*t55], t45 -> \ Cos[(delta2*t61)/2], t46 -> Cos[delta2*t61]} \ \>", "\<\ delta3 t47 delta3 t47 \ delta3 t47 {t5 -> Sin[sl - ----------], t6 -> Sin[2 sl - ----------], t7 -> Sin[3 sl - \ ----------], 2 2 \ 2 delta3 t47 delta3 t47 \ delta3 t47 t8 -> Sin[4 sl - ----------], t9 -> Sin[sl + ----------], t10 -> Sin[2 sl + \ ----------], 2 2 \ 2 delta3 t47 delta3 t47 \ delta3 t47 t11 -> Sin[3 sl + ----------], t12 -> Sin[4 sl + ----------], t13 -> \ Sin[----------], 2 2 \ 2 delta2 t49 delta2 t49 t14 -> Sin[sr - ----------], t15 -> Sin[2 sr - ----------], 2 2 delta2 t49 delta2 t49 t16 -> Sin[3 sr - ----------], t17 -> Sin[4 sr - ----------], 2 2 delta2 t49 delta2 t49 t18 -> Sin[sr + ----------], t19 -> Sin[2 sr + ----------], 2 2 delta2 t49 delta2 t49 \ delta2 t49 t20 -> Sin[3 sr + ----------], t21 -> Sin[4 sr + ----------], t22 -> \ Sin[----------], 2 2 \ 2 delta3 t55 delta2 t61 t23 -> Sin[----------], t24 -> Sin[delta3 t55], t25 -> Sin[----------], 2 2 t26 -> Sin[delta2 t61], t27 -> Cos[delta4], t32 -> Cos[theta], delta3 t47 delta3 t47 t33 -> Cos[2 sl - ----------], t34 -> Cos[4 sl - ----------], 2 2 delta3 t47 delta3 t47 \ delta3 t47 t35 -> Cos[2 sl + ----------], t36 -> Cos[4 sl + ----------], t37 -> \ Cos[----------], 2 2 \ 2 delta2 t49 delta2 t49 t38 -> Cos[2 sr - ----------], t39 -> Cos[4 sr - ----------], 2 2 delta2 t49 delta2 t49 \ delta2 t49 t40 -> Cos[2 sr + ----------], t41 -> Cos[4 sr + ----------], t42 -> \ Cos[----------], 2 2 \ 2 delta3 t55 delta2 t61 t43 -> Cos[----------], t44 -> Cos[delta3 t55], t45 -> Cos[----------], 2 2 t46 -> Cos[delta2 t61]}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]], Cell[TextData[ "(* Front right tire *)\nChnLst={{1,1},{2,2}};\n\ Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};\n\ VelNames={w9x,w9y,w9z,v9x,v9y,v9z};\nTerminalNode=9;\n\ Q1=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];"], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[CellGroupData[{ Cell[TextData[ "Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]\ "], "Input", PageWidth->Infinity, AspectRatioFixed->True], Cell[OutputFormData["\<\ {18.83900000000026*Second, {-(kappa*(0.3048*sr*t50* (1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + \ t57)) - 472.4478740918645*(-(7.499625008114926*10^-7)*t49 + \ 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - 0.01187480208432291*t42*t49)*t50*t60 - 3.692525*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - 0.00999983333416667*t25*t3*t61) - \ 30.48050800592673*t50*(0.00999983333416667*t3 - \ 0.00999983333416667*t3*t46 + 0.00999983333416667*t58 - 0.00999983333416667*t46*t58 - 0.0000999966667111108*t26*t61 + t26*t57*t61 + t26*t59*t61 - \ t26*t57*t62))* ArcTan[(-2*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61)*vx - 100.0016666861113*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60*vy + (0.3048*sr - 0.3048*t3)*wdel2 + (0.3048*sr*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) - 472.4478740918645*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - \ 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 \ - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60 - 3.692525*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61) - 30.48050800592673*t50*(0.00999983333416667*t3 - \ 0.00999983333416667*t3*t46 + 0.00999983333416667*t58 - 0.00999983333416667*t46*t58 - 0.0000999966667111108*t26*t61 + t26*t57*t61 + t26*t59*t61 - \ t26*t57*t62))* wth)/(0.5*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - \ 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + \ 1.500149995000066*t42*t49)*t50*t60*vx + 10000.33334000011*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 \ - 1.249937501349163*10^-7*t4*t49)*t50*t60*vy - \ 5.079974600046347*10^-8*wdel2 + (-0.003048*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) + 47245.57483149651*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + \ 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + \ 8.74956250946843*10^-7*t3*t49 - 1.249937501349163*10^-7*t4*t49)*t50*t60 + 0.92313125*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60 - 0.6096*t25*t50*(1 + t57)*(-0.00999983333416667*t25 - \ t3*t45*t61))*wth)]), 2*kappa*t25*t50*(1 + t57)*t60*(1.000099996666711*t45 + t45*t57 - 0.00999983333416667*t25*t3*t61)* ArcTan[(-2*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61)*vx - 100.0016666861113*(-(7.499625008114926*10^-7)*t49 + \ 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - 0.01187480208432291*t42*t49)*t50*t60*vy + (0.3048*sr - \ 0.3048*t3)*wdel2 + (0.3048*sr*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) - 472.4478740918645*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60 - 3.692525*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61) - 30.48050800592673*t50*(0.00999983333416667*t3 - \ 0.00999983333416667*t3*t46 + 0.00999983333416667*t58 - 0.00999983333416667*t46*t58 - 0.0000999966667111108*t26*t61 + t26*t57*t61 + t26*t59*t61 - \ t26*t57*t62))*wth) /(0.5*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - \ 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60*vx \ + 10000.33334000011*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - 4.374781254732044*10^-7*t18*t49 \ + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 - 1.249937501349163*10^-7*t4*t49)*t50*t60*vy - \ 5.079974600046347*10^-8*wdel2 + (-0.003048*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) + 47245.57483149651*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 \ - 1.249937501349163*10^-7*t4*t49)*t50*t60 + 0.92313125*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60 - 0.6096*t25*t50*(1 + t57)*(-0.00999983333416667*t25 - \ t3*t45*t61))*wth)], 100.0016666861113*kappa*(-(7.499625008114926*10^-7)*t49 + \ 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - 0.01187480208432291*t42*t49)*t50*t60* ArcTan[(-2*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61)*vx - 100.0016666861113*(-(7.499625008114926*10^-7)*t49 + \ 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - 0.01187480208432291*t42*t49)*t50*t60*vy + (0.3048*sr - \ 0.3048*t3)*wdel2 + (0.3048*sr*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) - 472.4478740918645*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60 - 3.692525*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61) - 30.48050800592673*t50*(0.00999983333416667*t3 - \ 0.00999983333416667*t3*t46 + 0.00999983333416667*t58 - 0.00999983333416667*t46*t58 - 0.0000999966667111108*t26*t61 + t26*t57*t61 + t26*t59*t61 - \ t26*t57*t62))*wth) /(0.5*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - \ 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60*vx \ + 10000.33334000011*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - 4.374781254732044*10^-7*t18*t49 \ + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 - 1.249937501349163*10^-7*t4*t49)*t50*t60*vy - \ 5.079974600046347*10^-8*wdel2 + (-0.003048*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) + 47245.57483149651*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 \ - 1.249937501349163*10^-7*t4*t49)*t50*t60 + 0.92313125*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60 - 0.6096*t25*t50*(1 + t57)*(-0.00999983333416667*t25 - \ t3*t45*t61))*wth)], -(kappa*(0.3048*sr - 0.3048*t3)*ArcTan[(-2*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61)*vx - 100.0016666861113*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60*vy + (0.3048*sr - 0.3048*t3)*wdel2 + (0.3048*sr*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) - 472.4478740918645*(-(7.499625008114926*10^-7)*t49 + 2.499875002707*10^-7*t30*t49 + 0.003749937500312499*t38*t49 - \ 0.0003124947916927083*t39*t49 + 0.003749937500312499*t40*t49 \ - 0.0003124947916927083*t41*t49 - \ 0.01187480208432291*t42*t49)*t50*t60 - 3.692525*t25*t50*(1 + t57)*t60* (1.000099996666711*t45 + t45*t57 - \ 0.00999983333416667*t25*t3*t61) - 30.48050800592673*t50*(0.00999983333416667*t3 - \ 0.00999983333416667*t3*t46 + 0.00999983333416667*t58 - 0.00999983333416667*t46*t58 - 0.0000999966667111108*t26*t61 + t26*t57*t61 + t26*t59*t61 - \ t26*t57*t62))* wth)/(0.5*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - \ 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + \ 1.500149995000066*t42*t49)*t50*t60*vx + 10000.33334000011*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + 8.74956250946843*10^-7*t3*t49 \ - 1.249937501349163*10^-7*t4*t49)*t50*t60*vy - \ 5.079974600046347*10^-8*wdel2 + (-0.003048*t50*(1 + 0.0000999966667111108*t46 + t57 + t46*t57*(1.000099996666711 + t57)) + 47245.57483149651*(0.00007499999986670947*t15 - \ 6.249791669438176*10^-6*t17 - 0.00007499999986670947*t19 + 6.249791669438176*10^-6*t21 + 0.0002375070824385261*t22 - 4.374781254732044*10^-7*t14*t49 + \ 6.249687506745816*10^-8*t16*t49 - \ 4.374781254732044*10^-7*t18*t49 + 6.249687506745816*10^-8*t20*t49 + \ 8.74956250946843*10^-7*t3*t49 - 1.249937501349163*10^-7*t4*t49)*t50*t60 + 0.92313125*(0.875*t49 - 1.*t30*t49 + 0.125*t31*t49 - 0.2500249991666777*t38*t49 - 0.2500249991666777*t40*t49 + 1.500149995000066*t42*t49)*t50*t60 - 0.6096*t25*t50*(1 + t57)*(-0.00999983333416667*t25 - \ t3*t45*t61))*wth)]), 0, 0} } \ \>", "\<\ {18.839 Second, {-(kappa (0.3048 sr t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + t57)) - -7 -7 472.448 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - 0.0118748 t42 t49) t50 t60 - 3.69252 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 t25 \ t3 t61) - 30.4805 t50 (0.00999983 t3 - 0.00999983 t3 t46 + 0.00999983 t58 - 0.00999983 t46 t58 - 0.0000999967 t26 t61 + t26 t57 t61 + t26 t59 \ t61 - t26 t57 t62)) ArcTan[(-2 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 t25 t3 t61) vx - -7 -7 100.002 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - \ 0.0118748 t42 t49) t50 t60 vy + (0.3048 sr - 0.3048 t3) wdel2 + (0.3048 sr t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + \ t57)) - -7 -7 472.448 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 \ t38 t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 \ t49 - 0.0118748 t42 t49) t50 t60 - 3.69252 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 \ t25 t3 t61) - 30.4805 t50 (0.00999983 t3 - 0.00999983 t3 t46 + 0.00999983 t58 \ - 0.00999983 t46 t58 - 0.0000999967 t26 t61 + t26 t57 t61 + t26 \ t59 t61 - t26 t57 t62)) wth) / (0.5 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 vx + -6 \ -6 10000.3 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 \ 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 -8 1.24994 10 t4 t49) t50 t60 vy - 5.07997 10 wdel2 + (-0.003048 t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + \ t57)) + -6 \ -6 47245.6 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + \ 6.24979 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 \ - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 1.24994 10 t4 t49) t50 t60 + 0.923131 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 \ t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 - 0.6096 t25 t50 (1 + t57) (-0.00999983 t25 - t3 t45 t61)) wth)]), \ 2 kappa t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 t25 t3 \ t61) ArcTan[(-2 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 t25 \ t3 t61) vx - -7 -7 100.002 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - 0.0118748 t42 t49) t50 t60 vy + (0.3048 sr - 0.3048 t3) wdel2 + (0.3048 sr t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + t57)) \ - -7 -7 472.448 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 \ - 0.0118748 t42 t49) t50 t60 - 3.69252 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 \ t25 t3 t61) - 30.4805 t50 (0.00999983 t3 - 0.00999983 t3 t46 + 0.00999983 t58 - 0.00999983 t46 t58 - 0.0000999967 t26 t61 + t26 t57 t61 + t26 \ t59 t61 - t26 t57 t62)) wth) / (0.5 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 t49 - \ 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 vx + -6 \ -6 10000.3 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 10 \ t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 t49 \ - -7 -8 1.24994 10 t4 t49) t50 t60 vy - 5.07997 10 wdel2 + (-0.003048 t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + t57)) \ + -6 \ -6 47245.6 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 \ 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - \ -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 1.24994 10 t4 t49) t50 t60 + 0.923131 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 \ t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 - 0.6096 t25 t50 (1 + t57) (-0.00999983 t25 - t3 t45 t61)) wth)], -7 -7 100.002 kappa (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - \ 0.0118748 t42 t49) t50 t60 ArcTan[(-2 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - \ 0.00999983 t25 t3 t61) -7 -7 vx - 100.002 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 \ t38 t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - 0.0118748 t42 t49) t50 t60 vy + (0.3048 sr - 0.3048 t3) wdel2 + (0.3048 sr t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + t57)) \ - -7 -7 472.448 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 \ - 0.0118748 t42 t49) t50 t60 - 3.69252 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 \ t25 t3 t61) - 30.4805 t50 (0.00999983 t3 - 0.00999983 t3 t46 + 0.00999983 t58 - 0.00999983 t46 t58 - 0.0000999967 t26 t61 + t26 t57 t61 + t26 \ t59 t61 - t26 t57 t62)) wth) / (0.5 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 t49 - \ 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 vx + -6 \ -6 10000.3 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 10 \ t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 t49 \ - -7 -8 1.24994 10 t4 t49) t50 t60 vy - 5.07997 10 wdel2 + (-0.003048 t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + t57)) \ + -6 \ -6 47245.6 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 \ 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - \ -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 1.24994 10 t4 t49) t50 t60 + 0.923131 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 \ t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 - 0.6096 t25 t50 (1 + t57) (-0.00999983 t25 - t3 t45 t61)) wth)], -(kappa (0.3048 sr - 0.3048 t3) ArcTan[(-2 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 t25 t3 t61) vx - -7 -7 100.002 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 t38 \ t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 t49 - \ 0.0118748 t42 t49) t50 t60 vy + (0.3048 sr - 0.3048 t3) wdel2 + (0.3048 sr t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + \ t57)) - -7 -7 472.448 (-7.49963 10 t49 + 2.49988 10 t30 t49 + 0.00374994 \ t38 t49 - 0.000312495 t39 t49 + 0.00374994 t40 t49 - 0.000312495 t41 \ t49 - 0.0118748 t42 t49) t50 t60 - 3.69252 t25 t50 (1 + t57) t60 (1.0001 t45 + t45 t57 - 0.00999983 \ t25 t3 t61) - 30.4805 t50 (0.00999983 t3 - 0.00999983 t3 t46 + 0.00999983 t58 \ - 0.00999983 t46 t58 - 0.0000999967 t26 t61 + t26 t57 t61 + t26 \ t59 t61 - t26 t57 t62)) wth) / (0.5 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 vx + -6 \ -6 10000.3 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + 6.24979 \ 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 -8 1.24994 10 t4 t49) t50 t60 vy - 5.07997 10 wdel2 + (-0.003048 t50 (1 + 0.0000999967 t46 + t57 + t46 t57 (1.0001 + \ t57)) + -6 \ -6 47245.6 (0.000075 t15 - 6.24979 10 t17 - 0.000075 t19 + \ 6.24979 10 t21 + -7 -8 0.000237507 t22 - 4.37478 10 t14 t49 + 6.24969 10 t16 t49 \ - -7 -8 -7 4.37478 10 t18 t49 + 6.24969 10 t20 t49 + 8.74956 10 t3 \ t49 - -7 1.24994 10 t4 t49) t50 t60 + 0.923131 (0.875 t49 - 1. t30 t49 + 0.125 t31 t49 - 0.250025 t38 \ t49 - 0.250025 t40 t49 + 1.50015 t42 t49) t50 t60 - 0.6096 t25 t50 (1 + t57) (-0.00999983 t25 - t3 t45 t61)) wth)]), \ 0, 0}}\ \>"], "Output", PageWidth->Infinity, Evaluatable->False, LineSpacing->{1, 0}] }, Closed]] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[TextData["Bibliography"], "Section", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True], Cell[TextData[ "To use TSi Dynamics requires some knowledge about dynamics. For those users \ seeking more information on the concepts behind the TSi Dynamics modeling \ formalism, we list a few reference books. Some have been cited above in our \ examples.\n\nV. I. Arnold, Mathematical Methods of Classical Mechanics, \ Springer-Verlag: New York, 1989 (2nd ed.)\n\nS. H. Crandall, D. C. Karnopp, \ E. F. Kurtz, D. C. Pridmore-Brown, Dynamics of Mechanical and \ Electromechanial Systems, McGraw-Hill: New York, 1968.\n\nF. Gantmacher, \ Lectures in Analytical Mechanics, MIR: Moscow, 1975.\n\nJ. H. Ginsberg, \ Advanced Engineering Dynamics, Harper & Row: New York, 1988.\n\nH. Goldstein, \ Classical Mechanics, Addison-Wesley: Reading, 1981 (2nd ed.)\n\nD. T. \ Greenwood, Classical Mechanics, Prentice-Hall: Englewood Cliffs, 1977.\n\nT. \ R. Kane and D. A. Levinson, Dynamics: Theory and Application, McGraw-Hill: \ New York, 1985.\n\nC. Lanczos, The Variational Principles of Mechanics, \ University of Toronto Press: Toronto, 1970 (4th ed.)\n\nL.Meirovitch, Methods \ of Analytical Dynamics, McGraw-Hill: New York, 1970.\n\nJu. I. Neimark and N. \ A. Fufaev, Dynamics of Nonholonomic Systems, American Mathematical Society: \ Providence, 1972.\n\nR. M. Rosenberg, Analytical Dynamics of Discrete \ Systems, Plenum: New York, 1977."], "Text", Evaluatable->False, CellHorizontalScrolling->False, AspectRatioFixed->True] }, Closed]] }, Open ]] }, FrontEndVersion->"Macintosh 3.0", ScreenRectangle->{{0, 832}, {0, 604}}, WindowToolbars->{}, CellGrouping->Manual, WindowSize->{520, 509}, WindowMargins->{{Automatic, 68}, {Automatic, 16}}, PrivateNotebookOptions->{"ColorPalette"->{RGBColor, -1}}, ShowCellLabel->True, ShowCellTags->False, RenderingOptions->{"ObjectDithering"->True, "RasterDithering"->False}, MacintoshSystemPageSetup->"\<\ 00<0001804P000000]P2:?oQon82n@960dL5:0?l0080001804P000000]P2:001 0000I00000400`<300000BL?00400@00000000000000060001T1T00000000000 00000000000000000000000000000000\>" ] (*********************************************************************** Cached data follows. If you edit this Notebook file directly, not using Mathematica, you must remove the line containing CacheID at the top of the file. The cache data will then be recreated when you save this file from within Mathematica. ***********************************************************************) (*CellTagsOutline CellTagsIndex->{} *) (*CellTagsIndex CellTagsIndex->{} *) (*NotebookFileOutline Notebook[{ Cell[CellGroupData[{ Cell[1731, 51, 158, 4, 99, "Title", Evaluatable->False], Cell[CellGroupData[{ Cell[1914, 59, 123, 3, 51, "Section", Evaluatable->False], Cell[2040, 64, 1773, 25, 350, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[3850, 94, 126, 3, 31, "Section", Evaluatable->False], Cell[3979, 99, 619, 11, 110, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[4623, 114, 128, 2, 42, "Input"], Cell[4754, 118, 342, 12, 25, "Output", Evaluatable->False] }, Open ]], Cell[5111, 133, 349, 7, 62, "Text", Evaluatable->False], Cell[5463, 142, 189, 5, 46, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[5677, 151, 87, 2, 27, "Input"], Cell[5767, 155, 182, 8, 21, "Print", Evaluatable->False] }, Open ]] }, Closed]], Cell[CellGroupData[{ Cell[5998, 169, 137, 3, 31, "Section", Evaluatable->False], Cell[CellGroupData[{ Cell[6160, 176, 136, 3, 46, "Subsection", Evaluatable->False], Cell[6299, 181, 191, 5, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[6515, 190, 88, 2, 27, "Input"], Cell[6606, 194, 1343, 65, 333, "Print", Evaluatable->False] }, Closed]], Cell[7964, 262, 1837, 27, 459, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[9838, 294, 129, 3, 30, "Subsection", Evaluatable->False], Cell[CellGroupData[{ Cell[9992, 301, 132, 3, 42, "Subsubsection", Evaluatable->False], Cell[10127, 306, 1472, 21, 414, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[11636, 332, 123, 3, 28, "Subsubsection", Evaluatable->False], Cell[11762, 337, 1941, 27, 494, "Text", Evaluatable->False], Cell[13706, 366, 155, 4, 42, "Input"], Cell[13864, 372, 188, 5, 30, "Text", Evaluatable->False], Cell[14055, 379, 157, 4, 42, "Input"], Cell[14215, 385, 356, 7, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[14596, 396, 148, 4, 57, "Input"], Cell[14747, 402, 222, 18, 70, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[15006, 425, 84, 2, 24, "Input"], Cell[15093, 429, 194, 16, 70, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[15324, 450, 84, 2, 24, "Input"], Cell[15411, 454, 773, 29, 70, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[16221, 488, 84, 2, 24, "Input"], Cell[16308, 492, 345, 19, 70, "Print", Evaluatable->False] }, Closed]], Cell[16668, 514, 596, 10, 107, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[17289, 528, 212, 4, 102, "Input"], Cell[17504, 534, 164, 16, 21, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[17705, 555, 84, 2, 24, "Input"], Cell[17792, 559, 1022, 32, 99, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[18851, 596, 84, 2, 24, "Input"], Cell[18938, 600, 739, 27, 73, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[19714, 632, 84, 2, 24, "Input"], Cell[19801, 636, 388, 22, 34, "Print", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[20238, 664, 129, 3, 28, "Subsubsection", Evaluatable->False], Cell[20370, 669, 1075, 17, 318, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[21482, 691, 132, 3, 28, "Subsubsection", Evaluatable->False], Cell[21617, 696, 2993, 41, 814, "Text", Evaluatable->False] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[24671, 744, 147, 3, 31, "Section", Evaluatable->False], Cell[CellGroupData[{ Cell[24843, 751, 146, 3, 46, "Subsection", Evaluatable->False], Cell[24992, 756, 677, 18, 242, "Text", Evaluatable->False], Cell[25672, 776, 240, 5, 117, "Input"], Cell[25915, 783, 124, 3, 30, "Text", Evaluatable->False], Cell[26042, 788, 322, 7, 117, "Input"], Cell[26367, 797, 140, 3, 30, "Text", Evaluatable->False], Cell[26510, 802, 100, 2, 27, "Input"], Cell[26613, 806, 277, 6, 78, "Text", Evaluatable->False], Cell[26893, 814, 98, 2, 27, "Input"], Cell[CellGroupData[{ Cell[27016, 820, 148, 4, 27, "Input"], Cell[27167, 826, 465, 27, 86, "Print", Evaluatable->False] }, Closed]], Cell[27647, 856, 136, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[27808, 863, 96, 2, 27, "Input"], Cell[27907, 867, 116, 20, 39, "Print", Evaluatable->False] }, Closed]], Cell[28038, 890, 361, 7, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[28424, 901, 96, 2, 27, "Input"], Cell[28523, 905, 413, 33, 125, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[28973, 943, 97, 2, 27, "Input"], Cell[29073, 947, 301, 21, 39, "Print", Evaluatable->False] }, Closed]], Cell[29389, 971, 406, 8, 78, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[29820, 983, 154, 4, 57, "Input"], Cell[29977, 989, 245, 18, 21, "Message", Evaluatable->False] }, Closed]], Cell[30237, 1010, 156, 3, 30, "Text", Evaluatable->False], Cell[30396, 1015, 91, 2, 27, "Input"] }, Closed]], Cell[CellGroupData[{ Cell[30524, 1022, 144, 3, 30, "Subsection", Evaluatable->False], Cell[30671, 1027, 320, 6, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[31016, 1037, 111, 2, 27, "Input"], Cell[31130, 1041, 1456, 36, 194, "Output", Evaluatable->False] }, Closed]], Cell[32601, 1080, 184, 5, 27, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[32810, 1089, 137, 3, 27, "Input"], Cell[32950, 1094, 230, 10, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[33217, 1109, 114, 2, 24, "Input"], Cell[33334, 1113, 900, 24, 77, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[34271, 1142, 90, 2, 24, "Input"], Cell[34364, 1146, 141, 8, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[34542, 1159, 177, 4, 24, "Input"], Cell[34722, 1165, 208, 9, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[34967, 1179, 152, 4, 24, "Input"], Cell[35122, 1185, 217, 10, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[35376, 1200, 121, 2, 24, "Input"], Cell[35500, 1204, 1017, 26, 90, "Output", Evaluatable->False] }, Closed]], Cell[36532, 1233, 204, 5, 27, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[36761, 1242, 114, 2, 27, "Input"], Cell[36878, 1246, 489, 17, 51, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[37404, 1268, 113, 2, 24, "Input"], Cell[37520, 1272, 27807, 988, 193, 9002, 751, "GraphicsData", "PostScript", "Graphics", Evaluatable->False], Cell[65330, 2262, 129, 8, 25, "Output", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[65508, 2276, 132, 3, 30, "Subsection", Evaluatable->False], Cell[65643, 2281, 690, 11, 126, "Text", Evaluatable->False], Cell[66336, 2294, 91, 2, 27, "Input"], Cell[66430, 2298, 185, 5, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[66640, 2307, 205, 5, 72, "Input"], Cell[66848, 2314, 385, 21, 38, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[67270, 2340, 343, 7, 147, "Input"], Cell[67616, 2349, 364, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[67995, 2373, 204, 4, 57, "Input"] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[68248, 2383, 139, 3, 31, "Section", Evaluatable->False], Cell[CellGroupData[{ Cell[68412, 2390, 134, 3, 46, "Subsection", Evaluatable->False], Cell[68549, 2395, 596, 10, 110, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[69182, 2410, 139, 3, 46, "Subsection", Evaluatable->False], Cell[69324, 2415, 550, 9, 126, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[69899, 2428, 476, 8, 237, "Input"], Cell[70378, 2438, 303, 21, 47, "Print", Evaluatable->False] }, Closed]], Cell[70696, 2462, 117, 3, 30, "Text", Evaluatable->False], Cell[70816, 2467, 559, 10, 267, "Input"], Cell[71378, 2479, 192, 5, 62, "Text", Evaluatable->False], Cell[71573, 2486, 114, 2, 27, "Input"], Cell[71690, 2490, 683, 11, 158, "Text", Evaluatable->False], Cell[72376, 2503, 202, 5, 72, "Input"], Cell[72581, 2510, 310, 6, 62, "Text", Evaluatable->False], Cell[72894, 2518, 188, 4, 57, "Input"], Cell[CellGroupData[{ Cell[73107, 2526, 82, 2, 27, "Input"], Cell[73192, 2530, 903, 32, 116, "Output", Evaluatable->False] }, Closed]], Cell[74110, 2565, 637, 11, 110, "Text", Evaluatable->False], Cell[74750, 2578, 296, 6, 117, "Input"], Cell[CellGroupData[{ Cell[75071, 2588, 85, 2, 27, "Input"], Cell[75159, 2592, 526, 23, 69, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[75722, 2620, 85, 2, 27, "Input"], Cell[75810, 2624, 526, 23, 69, "Output", Evaluatable->False] }, Closed]], Cell[76351, 2650, 483, 9, 110, "Text", Evaluatable->False], Cell[76837, 2661, 98, 2, 27, "Input"], Cell[76938, 2665, 222, 5, 78, "Text", Evaluatable->False], Cell[77163, 2672, 82, 2, 27, "Input"], Cell[77248, 2676, 309, 6, 78, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[77582, 2686, 156, 4, 27, "Input"], Cell[77741, 2692, 295, 21, 47, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[78073, 2718, 77, 2, 27, "Input"], Cell[78153, 2722, 2409, 65, 311, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[80599, 2792, 93, 2, 27, "Input"], Cell[80695, 2796, 1980, 54, 259, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[82712, 2855, 77, 2, 27, "Input"], Cell[82792, 2859, 4727, 112, 623, "Output", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[87568, 2977, 145, 3, 46, "Subsection", Evaluatable->False], Cell[87716, 2982, 538, 9, 94, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[88279, 2995, 141, 3, 42, "Subsubsection", Evaluatable->False], Cell[88423, 3000, 1653, 24, 366, "Text", Evaluatable->False], Cell[90079, 3026, 106, 2, 27, "Input"], Cell[90188, 3030, 159, 3, 30, "Text", Evaluatable->False], Cell[90350, 3035, 120, 2, 27, "Input"], Cell[90473, 3039, 145, 3, 30, "Text", Evaluatable->False], Cell[90621, 3044, 249, 6, 57, "Input"], Cell[90873, 3052, 267, 6, 46, "Text", Evaluatable->False], Cell[91143, 3060, 233, 5, 117, "Input"], Cell[91379, 3067, 166, 4, 42, "Input"], Cell[91548, 3073, 341, 7, 62, "Text", Evaluatable->False], Cell[91892, 3082, 117, 2, 27, "Input"], Cell[92012, 3086, 170, 4, 30, "Text", Evaluatable->False], Cell[92185, 3092, 93, 2, 27, "Input"], Cell[92281, 3096, 157, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[92463, 3103, 193, 4, 72, "Input"], Cell[92659, 3109, 376, 20, 34, "Message", Evaluatable->False] }, Closed]], Cell[93050, 3132, 134, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[93209, 3139, 135, 3, 27, "Input"], Cell[93347, 3144, 1706, 43, 220, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[95090, 3192, 135, 3, 27, "Input"], Cell[95228, 3197, 1706, 43, 220, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[96971, 3245, 99, 2, 27, "Input"], Cell[97073, 3249, 372, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[97482, 3275, 251, 5, 147, "Input"], Cell[97736, 3282, 376, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[98149, 3308, 121, 2, 42, "Input"], Cell[98273, 3312, 372, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[98682, 3338, 151, 4, 27, "Input"], Cell[98836, 3344, 465, 27, 86, "Print", Evaluatable->False] }, Closed]], Cell[99316, 3374, 143, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[99484, 3381, 77, 2, 27, "Input"], Cell[99564, 3385, 5118, 110, 472, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[104719, 3500, 90, 2, 27, "Input"], Cell[104812, 3504, 8792, 185, 1195, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[113641, 3694, 78, 2, 27, "Input"], Cell[113722, 3698, 986, 38, 181, "Output", Evaluatable->False] }, Closed]], Cell[114723, 3739, 236, 5, 46, "Text", Evaluatable->False], Cell[114962, 3746, 118, 2, 27, "Input"], Cell[115083, 3750, 149, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[115257, 3757, 95, 2, 27, "Input"], Cell[115355, 3761, 1153, 35, 129, "Output", Evaluatable->False] }, Closed]] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[116581, 3804, 122, 3, 31, "Section", Evaluatable->False], Cell[CellGroupData[{ Cell[116728, 3811, 134, 3, 46, "Subsection", Evaluatable->False], Cell[116865, 3816, 2526, 35, 478, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[119428, 3856, 135, 3, 30, "Subsection", Evaluatable->False], Cell[119566, 3861, 372, 7, 62, "Text", Evaluatable->False], Cell[119941, 3870, 251, 5, 147, "Input"], Cell[CellGroupData[{ Cell[120217, 3879, 468, 8, 132, "Input"], Cell[120688, 3889, 362, 21, 34, "Message", Evaluatable->False], Cell[121053, 3912, 362, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[121430, 3936, 117, 2, 42, "Input"], Cell[CellGroupData[{ Cell[121572, 3942, 151, 4, 27, "Input"], Cell[121726, 3948, 465, 27, 86, "Print", Evaluatable->False] }, Closed]], Cell[122206, 3978, 200, 4, 42, "Input"], Cell[122409, 3984, 110, 2, 27, "Input"], Cell[CellGroupData[{ Cell[122544, 3990, 93, 2, 27, "Input"], Cell[122640, 3994, 272, 19, 25, "Output", Evaluatable->False] }, Closed]], Cell[122927, 4016, 417, 8, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[123369, 4028, 135, 3, 27, "Input"], Cell[123507, 4033, 358, 21, 34, "Message", Evaluatable->False], Cell[123868, 4056, 16360, 352, 1715, "Output", Evaluatable->False] }, Closed]], Cell[140243, 4411, 447, 8, 78, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[140715, 4423, 141, 3, 27, "Input"], Cell[140859, 4428, 15942, 345, 1780, "Output", Evaluatable->False] }, Closed]], Cell[156816, 4776, 181, 5, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[157022, 4785, 114, 2, 27, "Input"], Cell[157139, 4789, 17963, 353, 1975, "Output", Evaluatable->False] }, Closed]], Cell[175117, 5145, 286, 6, 46, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[175428, 5155, 122, 2, 27, "Input"], Cell[175553, 5159, 14816, 288, 1624, "Output", Evaluatable->False] }, Closed]], Cell[190384, 5450, 245, 5, 46, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[190666, 5460, 134, 3, 30, "Subsection", Evaluatable->False], Cell[190803, 5465, 354, 7, 62, "Text", Evaluatable->False], Cell[191160, 5474, 272, 5, 147, "Input"], Cell[191435, 5481, 464, 8, 132, "Input"], Cell[191902, 5491, 119, 2, 42, "Input"], Cell[CellGroupData[{ Cell[192046, 5497, 151, 4, 27, "Input"], Cell[192200, 5503, 465, 27, 86, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[192702, 5535, 104, 2, 24, "Input"], Cell[192809, 5539, 243, 18, 21, "Message", Evaluatable->False] }, Closed]], Cell[193067, 5560, 118, 2, 39, "Input"], Cell[193188, 5564, 90, 2, 27, "Input"], Cell[193281, 5568, 200, 4, 42, "Input"], Cell[193484, 5574, 110, 2, 27, "Input"], Cell[CellGroupData[{ Cell[193619, 5580, 93, 2, 27, "Input"], Cell[193715, 5584, 268, 19, 25, "Output", Evaluatable->False] }, Closed]], Cell[193998, 5606, 201, 5, 27, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[194224, 5615, 139, 3, 27, "Input"], Cell[194366, 5620, 26168, 513, 3236, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[220571, 6138, 93, 2, 24, "Input"], Cell[220667, 6142, 24591, 477, 3028, "Output", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[245307, 6625, 181, 5, 30, "Subsection", Evaluatable->False], Cell[245491, 6632, 585, 10, 94, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[246101, 6646, 103, 2, 42, "Input"], Cell[246207, 6650, 360, 21, 34, "Output", Evaluatable->False] }, Closed]], Cell[246582, 6674, 154, 4, 72, "Input"], Cell[246739, 6680, 292, 6, 78, "Text", Evaluatable->False], Cell[247034, 6688, 260, 5, 57, "Input"], Cell[247297, 6695, 131, 3, 30, "Text", Evaluatable->False], Cell[247431, 6700, 119, 2, 42, "Input"], Cell[247553, 6704, 153, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[247731, 6711, 151, 4, 27, "Input"], Cell[247885, 6717, 407, 25, 73, "Print", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[248329, 6747, 102, 2, 27, "Input"], Cell[248434, 6751, 239, 18, 21, "Message", Evaluatable->False] }, Closed]], Cell[248688, 6772, 116, 2, 42, "Input"], Cell[248807, 6776, 439, 8, 78, "Text", Evaluatable->False], Cell[249249, 6786, 88, 2, 27, "Input"], Cell[CellGroupData[{ Cell[249362, 6792, 154, 4, 42, "Input"], Cell[249519, 6798, 376, 21, 34, "Message", Evaluatable->False], Cell[249898, 6821, 290, 19, 25, "Output", Evaluatable->False] }, Closed]], Cell[250203, 6843, 301, 6, 46, "Text", Evaluatable->False], Cell[250507, 6851, 162, 4, 27, "Input"], Cell[250672, 6857, 233, 5, 46, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[250930, 6866, 80, 2, 27, "Input"], Cell[251013, 6870, 133, 16, 25, "Output", Evaluatable->False] }, Closed]], Cell[251161, 6889, 584, 10, 94, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[251770, 6903, 127, 2, 27, "Input"], Cell[251900, 6907, 358, 21, 34, "Message", Evaluatable->False], Cell[252261, 6930, 169, 16, 25, "Output", Evaluatable->False] }, Closed]], Cell[252445, 6949, 304, 6, 62, "Text", Evaluatable->False], Cell[252752, 6957, 99, 2, 27, "Input"], Cell[252854, 6961, 116, 2, 27, "Input"], Cell[252973, 6965, 149, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[253147, 6972, 128, 2, 27, "Input"], Cell[253278, 6976, 62178, 1225, 6616, "Output", Evaluatable->False] }, Closed]], Cell[315471, 8204, 121, 2, 42, "Input"], Cell[315595, 8208, 199, 5, 30, "Text", Evaluatable->False], Cell[315797, 8215, 155, 4, 27, "Input"], Cell[CellGroupData[{ Cell[315977, 8223, 162, 4, 27, "Input"], Cell[316142, 8229, 505, 24, 52, "Output", Evaluatable->False] }, Closed]], Cell[316662, 8256, 417, 8, 78, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[317104, 8268, 175, 4, 57, "Input"], Cell[317282, 8274, 352, 20, 34, "Message", Evaluatable->False], Cell[317637, 8296, 203, 16, 25, "Output", Evaluatable->False], Cell[317843, 8314, 352, 20, 34, "Message", Evaluatable->False], Cell[318198, 8336, 203, 16, 25, "Output", Evaluatable->False] }, Closed]], Cell[318416, 8355, 191, 5, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[318632, 8364, 130, 3, 27, "Input"], Cell[318765, 8369, 344, 19, 38, "Output", Evaluatable->False] }, Closed]], Cell[319124, 8391, 88, 2, 27, "Input"], Cell[319215, 8395, 188, 5, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[319428, 8404, 114, 2, 27, "Input"], Cell[319545, 8408, 179, 16, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[319761, 8429, 137, 3, 27, "Input"], Cell[319901, 8434, 57433, 1163, 5355, "Output", Evaluatable->False] }, Closed]], Cell[377349, 9600, 473, 8, 78, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[377847, 9612, 139, 3, 27, "Input"], Cell[377989, 9617, 399, 23, 38, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[378425, 9645, 192, 4, 57, "Input"], Cell[378620, 9651, 672, 27, 51, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[379329, 9683, 166, 4, 57, "Input"], Cell[379498, 9689, 342, 21, 43, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[379877, 9715, 161, 4, 57, "Input"], Cell[380041, 9721, 326, 21, 43, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[380404, 9747, 147, 4, 27, "Input"], Cell[380554, 9753, 80829, 1625, 7942, "Output", Evaluatable->False] }, Closed]], Cell[461398, 11381, 194, 5, 30, "Text", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[461641, 11392, 132, 3, 31, "Section", Evaluatable->False], Cell[CellGroupData[{ Cell[461798, 11399, 134, 3, 46, "Subsection", Evaluatable->False], Cell[461935, 11404, 1008, 15, 206, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[462980, 11424, 151, 3, 30, "Subsection", Evaluatable->False], Cell[463134, 11429, 2329, 32, 462, "Text", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[465500, 11466, 149, 3, 30, "Subsection", Evaluatable->False], Cell[465652, 11471, 512, 9, 94, "Text", Evaluatable->False], Cell[466167, 11482, 118, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[466310, 11489, 476, 8, 237, "Input"], Cell[466789, 11499, 303, 21, 47, "Print", Evaluatable->False] }, Closed]], Cell[467107, 11523, 117, 3, 30, "Text", Evaluatable->False], Cell[467227, 11528, 559, 10, 267, "Input"], Cell[467789, 11540, 124, 3, 30, "Text", Evaluatable->False], Cell[467916, 11545, 114, 2, 27, "Input"], Cell[468033, 11549, 138, 3, 30, "Text", Evaluatable->False], Cell[468174, 11554, 202, 5, 72, "Input"], Cell[468379, 11561, 188, 4, 57, "Input"], Cell[CellGroupData[{ Cell[468592, 11569, 82, 2, 27, "Input"], Cell[468677, 11573, 903, 32, 116, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[469617, 11610, 360, 7, 147, "Input"], Cell[469980, 11619, 659, 26, 69, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[470676, 11650, 85, 2, 27, "Input"], Cell[470764, 11654, 526, 23, 69, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[471327, 11682, 85, 2, 27, "Input"], Cell[471415, 11686, 526, 23, 69, "Output", Evaluatable->False] }, Closed]], Cell[471956, 11712, 125, 3, 30, "Text", Evaluatable->False], Cell[472084, 11717, 98, 2, 27, "Input"], Cell[472185, 11721, 115, 3, 30, "Text", Evaluatable->False], Cell[472303, 11726, 82, 2, 27, "Input"], Cell[472388, 11730, 134, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[472547, 11737, 138, 3, 27, "Input"], Cell[472688, 11742, 295, 21, 47, "Print", Evaluatable->False], Cell[472986, 11765, 14329, 301, 1767, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[487352, 12071, 130, 3, 27, "Input"], Cell[487485, 12076, 366, 21, 34, "Message", Evaluatable->False], Cell[487854, 12099, 366, 21, 34, "Message", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[488257, 12125, 82, 2, 27, "Input"], Cell[488342, 12129, 227, 18, 25, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[488606, 12152, 82, 2, 27, "Input"], Cell[488691, 12156, 113, 16, 25, "Output", Evaluatable->False] }, Closed]], Cell[488819, 12175, 202, 5, 72, "Input"], Cell[489024, 12182, 188, 4, 57, "Input"], Cell[CellGroupData[{ Cell[489237, 12190, 358, 7, 132, "Input"], Cell[489598, 12199, 613, 26, 77, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[490248, 12230, 85, 2, 27, "Input"], Cell[490336, 12234, 540, 25, 77, "Output", Evaluatable->False] }, Closed]], Cell[CellGroupData[{ Cell[490913, 12264, 85, 2, 27, "Input"], Cell[491001, 12268, 540, 25, 77, "Output", Evaluatable->False] }, Closed]], Cell[491556, 12296, 110, 2, 27, "Input"], Cell[CellGroupData[{ Cell[491691, 12302, 149, 4, 27, "Input"], Cell[491843, 12308, 295, 21, 47, "Print", Evaluatable->False], Cell[492141, 12331, 14329, 301, 1767, "Output", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[506519, 12638, 134, 3, 30, "Subsection", Evaluatable->False], Cell[506656, 12643, 702, 11, 174, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[507383, 12658, 142, 3, 42, "Subsubsection", Evaluatable->False], Cell[507528, 12663, 191, 4, 72, "Input"] }, Closed]], Cell[CellGroupData[{ Cell[507756, 12672, 141, 3, 42, "Subsubsection", Evaluatable->False], Cell[507900, 12677, 277, 6, 46, "Text", Evaluatable->False], Cell[508180, 12685, 313, 6, 117, "Input"] }, Closed]], Cell[CellGroupData[{ Cell[508530, 12696, 135, 3, 42, "Subsubsection", Evaluatable->False], Cell[508668, 12701, 243, 6, 87, "Input"] }, Closed]], Cell[CellGroupData[{ Cell[508948, 12712, 126, 3, 28, "Subsubsection", Evaluatable->False], Cell[509077, 12717, 592, 10, 174, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[509694, 12731, 1252, 22, 462, "Input"], Cell[510949, 12755, 384, 21, 34, "Message", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[511382, 12782, 144, 3, 28, "Subsubsection", Evaluatable->False], Cell[511529, 12787, 91, 2, 27, "Input"], Cell[511623, 12791, 126, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[511774, 12798, 101, 2, 27, "Input"], Cell[511878, 12802, 361, 23, 60, "Print", Evaluatable->False] }, Closed]], Cell[512254, 12828, 316, 6, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[512595, 12838, 305, 7, 102, "Input"], Cell[512903, 12847, 32694, 604, 4029, "Output", Evaluatable->False] }, Closed]], Cell[545612, 13454, 130, 3, 27, "Input"], Cell[CellGroupData[{ Cell[545767, 13461, 82, 2, 27, "Input"], Cell[545852, 13465, 902, 32, 103, "Output", Evaluatable->False] }, Closed]], Cell[546769, 13500, 223, 5, 87, "Input"], Cell[CellGroupData[{ Cell[547017, 13509, 155, 4, 27, "Input"], Cell[547175, 13515, 5803, 135, 532, "Output", Evaluatable->False] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[553027, 13656, 157, 3, 28, "Subsubsection", Evaluatable->False], Cell[553187, 13661, 93, 2, 27, "Input"], Cell[553283, 13665, 126, 3, 30, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[553434, 13672, 101, 2, 27, "Input"], Cell[553538, 13676, 361, 23, 60, "Print", Evaluatable->False] }, Closed]], Cell[553914, 13702, 245, 5, 62, "Text", Evaluatable->False], Cell[CellGroupData[{ Cell[554184, 13711, 305, 7, 102, "Input"], Cell[554492, 13720, 116507, 2224, 14897, "Output", Evaluatable->False] }, Closed]], Cell[671014, 15947, 130, 3, 27, "Input"], Cell[CellGroupData[{ Cell[671169, 15954, 82, 2, 27, "Input"], Cell[671254, 15958, 4385, 95, 545, "Output", Evaluatable->False] }, Closed]], Cell[675654, 16056, 302, 7, 102, "Input"], Cell[CellGroupData[{ Cell[675981, 16067, 155, 4, 27, "Input"], Cell[676139, 16073, 24225, 494, 2079, "Output", Evaluatable->False] }, Closed]] }, Closed]] }, Closed]] }, Closed]], Cell[CellGroupData[{ Cell[700437, 16575, 123, 3, 31, "Section", Evaluatable->False], Cell[700563, 16580, 1430, 21, 70, "Text", Evaluatable->False] }, Closed]] }, Open ]] } ] *) (*********************************************************************** End of Mathematica Notebook file. ***********************************************************************)