TSi ProPac: Dynamics

Harry G. Kwatny

Department of Mechanical Engineering & Mechanics
Drexel University

Chris LaVigna

Techno-Sciences, Incorporated

Off[General::spell];
Off[General::spell1];

Introduction

TSi ProPac: Dynamics is a Mathematica package containing functions that support the assembly of mathematical models and simulation models for mechanical systems.  In its current form, 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. 

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 Poincare's form of Lagrange's equations that admits the standard Lagrange equations as a special case. However, Poincare's form allows the exploitation of quasi-velocities which can significantly simplify the equations of motion.

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. Dynamics includes functions that assist in performing simulations using the NDSolve function in Mathematica and other functions to construct a C-code subroutine that compiles as an S-Function for use with MATLAB/SIMULINK.

Getting Started

This notebook illustrates the tools provided in Dynamics including a few new functions contained in Version 3.0. Dynamics is contained in the directory ProPac. ProPac can be placed anywhwere you like, but a search path must be defined for it. It is most convenient to place ProPac in the AddOns\Applications directiory, i.e, C:\Program Files\Wolfram Research\Mathematica\3.0\AddOns\Applications. Once ProPac isloaded all of the functions in Dynamics as well as other packages included in TSi ProPac will be available for use and loaded as when needed.

In Mathematica 3 ProPac can be loaded with the command

<<ProPac`

With Mathematica 2.2 use the command ProPac`Master`.

The Basics of TSi Dynamics

What's in the Package?

To find out what functions are available in Dynamics simply use the commands:

Dynamics

?Dynamics

?GeneralizedForce

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:
   Joints: generates all kinematic functions for list of joints
   CreateModel: creates the dynamic model parameters for tree structures from system data   
  EndEffector & NodeVelocity: creates the position, orientation and velocity of a body frame located at specified nodes as functions of configuration coordinates
   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
   GeneralizedForce, Spring & 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 two are specific for a spring (defined by a local potential function) and a damper (defined by a local disspation function) connected between specified system nodes
   AlgebraicConstraints & DifferentialConstraints: these account for algebraic and differential constraints added to a tree structure
   MakeODEs & CreateModelMEX: 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.
After describing the basic data structures used by TSi Dynamics we will illustrate the use of these and some other functions.

Data Structures

Tree Structures

Dynamics builds models for mechanical systems that have an underlying tree topology. Chain structures are a special case. Systems with closed loops are accommodated by adding constraints to the underlying tree.
A tree consisting of n bodies also contains n joints. Every system contains a base reference frame which is designated body '0'. Otherwise, bodies and joints can be numbered arbitrarily. Joint data and body data are organized into lists by the analyst, i.e.:
	JointList={JointData_1,...,BodyData_n}
	BodyList={BodyData_1,...,BodyData_n}
The structure ofthe individual data objects will be described below. Joints and bodies are implicitly numbered by there position in the data lists.
Each body contains a unique 'inboard' node, corresponding to (the outboard side of) a joint through which the body connects to an inner branch of the tree, or to the root (body 0). Each body may also contain outboard nodes. The outboard nodes are distinguished body locations that may be associated with a joint location (the inboard side of the joint), a sensor location, a point of application of an external force or any other feature of interest. Since one joint connects the tree to the root (the root node may be considered an outboard node of body 0), there must be at least n-1 outboard nodes among the n bodies corresponding to the remaining n-1 joints. These are the n 'outboard joint nodes'. The outboard joint nodes must be numbered 1 through n and must correspond to the associated joint number. The specific association of numbers to joints is not essential but by convention the root node is normally assigned the number 1. The remaining outboard notes can be numbered arbitrarily. The inboard nodes need not be assigned numbers.
In summary there are two important book-keeping principles:
	• joints and bodies are numbered according to their position in the data lists,
	• output joint nodes must be numbered consistently with their repective joints.
A tree is composed of a set of defining chains. For instance consider a tree composed of the following sequences of bodies:
                                       0,1,2,4      0,1,2,3,5      0,1,2,3,6
All defining chains 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 - each pair consisting of a body and its inboard joint: {inboard joint, body}. For example:
            {{1,1},{2,2},{5,4}}      {{1,1},{2,2},{3,3},{4,5}}      {{1,1},{2,2},{3,3},{6,6}}
indicates that body 5 connects to body 3 at joint 4, and body 6 connects to body 3 at joint 6. Recall that each joint is uniquely associated with an outboard node of a particular body.
A tree is defined by the data structure:
    Tree = {list of chains}
    chain = ordered list of pairs {inboard joint, body}
              =  {{first inboard joint, first body},...,{last inboard joint, last body}}

Reference Frames

It is assumed that there is a single inertially fixed reference frame whose origin is the inboard side of the root  joint.  Each body has a primary reference frame, fixed in the body with origin at the inboard node. Body data is defined in this frame. As appropriate, there may be other body fixed frames as well with origins at outboard node locations. Normally, the axes of these frames are parallel to the primary frame when the body is undeformed. For the system as a whole, there is a 'reference configuration' corresponding to the nominal joint configurations (associated with zero joint motion parameters) and undeformed bodies. In the reference configuration all reference frames (body and space) are alligned. The analyst sets up the reference configuration when choosing body frame orientations for joint and body data definitions. It is recommended that the analyst begin by defining a physically meaningful reference configuration from which data definitions will logically follow.

Joints

Joints constrain the relative motion between two bodies, or more precisely, between two reference frames  - 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.
Simple 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 (Δw, ΔV) 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 necessary 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.
A k-frame compound joint with n degrees of freedom is defined by the data structure:
                                                            {r, H,q,p} 
where 
   r = k-vector whose elements define the number of degrees of freedom for each simple joint, with n = r1 +.. + rk. 
   H = [H1 .. Hk], a matrix composed of the k joint map matrices of the simple joints.
   q = n-vector of joint coordinate names.
   p = n-vector of joint quasi-velocity names.  
Let us consider some elementary examples.
A two degree of freedom revolute joint as a simple joint (spherical joint):

r1={2};H1={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};
q1={a1x,a1z};p1={w1x,w1z};

A two degree of freedom revolute joint as a compound joint (universal joint):

r2={1,1};H2={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};
q2={a2x,a2z};p2={w2x,w2z};

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.

JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2}};

{V,X,H}=Joints[JointLst];

Print[V]

Print[X]

Print[H]

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

r3={6}; H3=IdentityMatrix[6];
q3={ax,ay,az,x,y,z}; p3={wx,wy,wz,ux,uy,uz};

JointLst={{r3,H3,q3,p3}};

{V,X,H}=Joints[JointLst];

Print[V]

Print[X]

Print[H]

Rigid Bodies

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:
1) There is a distinguished point that corresponds to the inboard joint of the body. The body frame has its origin located there. 
2) The center of mass and all other points of interest (nodes) including outboard joint locations are defined in the body frame.
3) The inertia matrix is defined in the body frame and it is the inertia matrix about the center of mass.
The 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:
                                    {com,{out1,..,outk},m,Inertia}
where 
   com is the center of mass location, 
   outi = {node number, location} for the ith outboard node, 
   m is the mass, and 
   Inertia is the 3x3 inertia tensor (about the center of mass). It may also be specified as a 6x6 spatial inertia tensor. In this      case it is assumed specified about the inboard joint. This type of specification is useful, for example, when dealing with submerged bodies where a fluid loaded inertia needs to be specified.

Flexible Bodies

Dynamics accommodates flexible bodies which satisfy the following assumptions:
- Local deformations are small, so that linear stiffness (quadratic strain energy) and dissipation (quadratic dissipation function) relationships apply.
 -  Body deformations can be characterized by a finite set of deformation coordinates, denoted by x. 
-  Body frame center of mass location and joint locations and orientations can be defined as (affine) linear functions of the deformation coordinates.
Any 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.
The 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
                                                   y = C.{1, x}
where C is a 3x(n+1) matrix. This is the type of information required to locate the center of gravity in the deformed body.
More 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
                                                  Y = C.{1,x}
where C is a 6x(n+1) matrix.
A flexible body with k outboard nodes and n deformation coordinates is defined by the data structure:
                       {Ccom,{out1,..,outk},m,{M(x),B,K},x,v}
where Ccom is a 3x(n+1) matrix that defines the center of mass location, 
   outi = {node number, Couti}, where Couti is a 6x(n+1) matrix that defines the orientation and location of the ith outboard node
   m is the mass 
   M(x) is the inertia matrix, B is the dissipation matrix, K is the stiffness matrix
   x is an n-vector of deformation coordinate names
   v is an n-vector of deformation velocity names

A First Example: The Double Pendulum

Building the Equations of Motion

Building a model in TSi Dynamics generally proceeds in the following way:
  - Define joint data.
  - Define body data.
  - Define interconnection structure.
  - Define potential energy function and generalized forces.
  - Assemble the model.
The following is a simple example.
Define Joint Data

r1={1};H1={{1},{0},{0},{0},{0},{0}};
q1={a1x};p1={w1x};

r2={1};H2={{1},{0},{0},{0},{0},{0}};
q2={a2x};p2={w2x};

JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2}};

Define Body Data

com1={0,0,-l1}; mass1=m1; out1={2,{0,0,-l1}};
Inertia1=DiagonalMatrix[{0,0,0}];

com2={0,0,-l2}; mass2=m2; out2={3,{0,0,-l2}};
Inertia2=DiagonalMatrix[{0,0,0}];

BodyLst={{com1,{out1},mass1,Inertia1},{com2,{out2},mass2,Inertia2}};

Define Interconnection Structure

TreeLst={{{1,1},{2,2}}};

Define Potential Energy
In this case only gravity contributes to the potential energy. The only generalized forces are external torques acting at the two joints.

g=gc; PE=0; Q={T1,T2};

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointLst,BodyLst,TreeLst,g,PE,Q];

We can look at some results.

Print[MatrixForm[V]]

Recall that V returns as a list of kinematic matrices- 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.

Print[MatrixForm[M]]

Print[MatrixForm[Fp]]

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.

DeleteFile["dbl_pend.dat"];
Save["dbl_pend.dat",p,q,V,X,H,Cp,Fp,M];

Of course, the data is reloaded by the standard:

<<dbl_pend.dat;

Simulation: Numerical Solution

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.

Equations=MakeODEs[p,q,V,M,Cp,Fp,t]

Now, replace parameter symbols by numbers and set up initial conditions.

DataReplacements={m1->1,l1->2,m2->1,l2->1,gc->1,T1->0,T2->0}

Equations1=Equations/.DataReplacements

vars=Join[p,q]

InitialConditions=Table[ToExpression[StringJoin[ToString[vars[[i]]],"[0]==0"]],{i,Length[vars]}]

InitialConditions={w1x[0] == 0, w2x[0] == 0, a1x[0] == .1, a2x[0] == .2}

Equations2=Join[Equations1,InitialConditions]

Finally, integrate the equations using the Mathematica function NDSolve and plot the results.

sols=NDSolve[Equations2,vars,{t,0,40}]

Plot[Evaluate[a1x[t]/.sols],{t,0,40}]

Plot[Evaluate[a2x[t]/.sols],{t,0,40}]

Creating MEX-Files

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.

We can use the TSi Dynamics function EndEffector to define the functions.

ChainLst={{1,1},{2,2}};
TerminalNode=3;
Xout=EndEffector[ChainLst,TerminalNode,BodyLst,X];
yout={Xout[[2,4]],Xout[[3,4]]}

Inputs = Q;
Outputs = yout;
MEXFilename = "dbl_pend.c";
paramvec={m1,l1,m2,l2,gc};
PassedParams = {"X0","m1","l1","m2","l2","gc"};
PassedParamsDimensions = {{2*Length[p],1},{1,1},{1,1},{1,1},{1,1},{1,1}};
CreateModelMEX[p,q,Inputs,Outputs,PassedParams,PassedParamsDimensions,
               V,Cp,Fp,M,MEXFilename];

Compiling & Using MEX Files

Detailed information on compiling and using MEX files with MATLAB/SIMULINK can be found in the appropriate MATLAB references. CreateModelMEX (and also CreateControllerMEX) assemble C source code that needs to be compiled. During the compilation process the compiled code will be linked with additional code segments and libraries provided with ProPac or MATLAB or your compiler. It is necessary that this code be available at the time of compilation. The easiest way to proceed is to perform the compilation from within MATLAB using scripts provided with MATLAB (either cmex with MATLAB 4 or mex with MATLAB 5). This should automatically define the locations of all required MATLAB or compiler code segments. In addition to the file created by ProPac (for example: dbl_pend.c) you will need the files linsolv.c, f2c.h and trigfun.h - all provided with ProPac. 
	One way to compile dbl_pend.c  is to place the files dbl_pend.c, linsolv.c, f2c.h and trigfun.h in a common directory, for example C:\DoublePend. Then in the MATLAB command window set the current directory to C:\DoublePend to insure that the four required files are in the search path:
	cd C:\DoublePend
then use the command
	mex dbl_pend.c linsolv.c
in MATLAB 5, or
	!cmex dbl_pend.c linsolv.c
in MATLAB 4.2.
	If you have followed the MATLAB installation instructions as appropriate for your compiler this should work fine. Otherwise, you can reinstall MATLAB 5 and follow the instructions (which setsup the appropriate paths in mex), or in MATLAB 4.2 you can edit cmex as described therein.

Building More Complex Models

Introductory Remarks

TSi Dynamics provides several routes to developing models. The most direct is to use the functions CreateModel or CreateModelSpecial. These two functions work essentially the same way but produce models in slightly different form. The former 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.

Partial Suspension System

We consider a simple split axle suspension in planar motion. The system consists of a chassis, two axles, two tires, two springs - 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).
Joint Data

(* Joint 1 - planar motion *)
r1={3};
q1={theta1,y,z};p1={w1,vy,vz};
H1={{1,0,0},{0,0,0},{0,0,0},{0,0,0},{0,1,0},{0,0,1}};
(* Joint 2 - suspension *)
r2={1};
q2={theta2};p2={w2};
H2={{1},{0},{0},{0},{0},{0}};
(* Joint 3 - suspension *)
r3={1};
H3={{1},{0},{0},{0},{0},{0}};
q3={theta3};p3={w3};

JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2},{r3,H3,q3,p3}};
{V,X,H}=Joints[JointLst];

Body Data

(* Body 1 - Chassis *)
cm1={0,0,0};
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}}};
I1=DiagonalMatrix[{Ixx,Iyy,Izz}];

(* Body 2 - Wheel & Axle *)
cm2={0,d,0};
out2={{4,{0,d,R}},{8,{0,Sqrt[2]*a,0}}};
I2=DiagonalMatrix[{Wxx,Wyy,Wzz}];

(* Body 3 - Wheel & Axle *)
cm3={0,-d,0};
out3={{5,{0,-d,R}},{9,{0,-Sqrt[2]*a,0}}};
I3=DiagonalMatrix[{Wxx,Wyy,Wzz}];

BodyLst={{cm1,out1,m1,I1},{cm2,out2,mw,I2},{cm3,out3,mw,I3}};

System Structure
Notice that this system has a tree structure with two chains.

TreeLst={{{1,1},{2,2}},{{1,1},{3,3}}};

Potential Energy Constructions
The 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.
It 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.

(* Define Tire PE in terms of Space Coordinates *)

yy={{x2,y2,z2},{x3,y3,z3}};
Pot=(k/2)*(z2-z20)^2+(k/2)*(z3-z30)^2;

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.

(* Obtain Tire PE in terms of generalized Coordinates *)

TirePE=LeafPotential[BodyLst,TreeLst,X,H,Pot,yy];

TirePE

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.

SpringPot[x_]:=(kdel/2)*(x-ls)^2
q=Join[q1,q2,q3];
SpringPE1=SpringPotential[6,8,SpringPot,TreeLst,BodyLst,X,q];
SpringPE2=SpringPotential[7,9,SpringPot,TreeLst,BodyLst,X,q];

PE=TirePE+SpringPE1+SpringPE2;

SpringPE1

SpringPE2

Generalized Force
The 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.

Q={0,0,0,-c*w2,-c*w3};

Gravity
Since, we have chosen the z-axis to point down, we change the sign of the gravitational acceleration

g=-gc;

Obtain Equations of Motion
Notice that since the joint kinematic data V, X, and H has already been computed, we include this data in the argument list of CreateModel to avoid recomputing it.

{V,X,H,M,Cm,F,pp,qq}=CreateModel[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H];

M

Simplify[Cm.pp]

F

A Flexible Two Link Manipulator

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.

Modeling a Flexible Link

<< ProPac`

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.      
As 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
																				Cos[b*L] * Cosh[b*L] = -1
which has a countable set of solutions br, r = 1,2,3...; the first three roots are approximately:

bL={0.600*Pi,1.49*Pi,2.50*Pi};

They define the natural frequencies by the relation

w[i_]:=(bL[[i]])^2*(El*Ixx/(rho*L^4))^(1/2);

Correspondingly, the mode shapes are:

W[i_,z_]:=((Cosh[bL[[i]]*z/L]-Cos[bL[[i]]*z/L])/(Cosh[bL[[i]]]+Cos[bL[[i]]])

              - (Sinh[bL[[i]]*z/L]-Sin[bL[[i]]*z/L])/(Sinh[bL[[i]]]+Sin[bL[[i]]]));

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:

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}};

B=0*IdentityMatrix[7]; 
K=DiagonalMatrix[{0,0,0,0,0,0,(0.600*Pi)^2}]*(El*Ixx/(m*L^3));

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:

yend=W[1,L]*y1; phiend=D[W[1,z],z]/.z->L;

and the center of mass (which corresponds to the midpoint) is

ycom=W[1,L/2]*y1;

Consequently, we have the matrices Cout and Ccom:

Cout={{0,D[W[1,z],z]/.z->L},
      {0,0},{0,0},
      {0,0},{0,W[1,L]},{L,0}};
Ccom={{0,0},{0,W[1,L/2]},{L,0}};

The link body data is then

Link1={N[Ccom],{{2,N[Cout]}},m,{M,B,K},{y11},{v11}}/.L->L1

Link2={N[Ccom],{{3,N[Cout]}},m,{M,B,K},{y21},{v21}}/.L->L2

BodyList={Link1,Link2};

r1={1};
H1=Transpose[{{1,0,0,0,0,0}}];
q1={theta1};p1={w1};

r2={1};
H2=Transpose[{{1,0,0,0,0,0}}];
q2={theta2};p2={w2};

JointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};

TreeList={{{1,1},{2,2}}};
PE=0; Q={0,0,0,0};

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointList,BodyList,TreeList,g,PE,Q];

We can examine some of the results:

M

Simplify[Cp.p]

Fp

The absolute position and orientation of a frame at the end of the second link can be obtained with the function EndEffector.

EndPos=EndEffector[3,TreeList,BodyList,X];

Let's take a look at the position vector:

EndPos[[{1,2,3},4]]

Tractor Semi-Trailer

<< ProPac`

The goal is 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
                             F=-kappa*arctan[vy/vx]

Joint Definitions

Main body (tractor) joint

(* Joint 1  *)
r1={3}; q1={theta,x,y}; p1={wth,vx,vy};
H1={{0,0,0},{0,0,0},{1,0,0},{0,1,0},{0,0,1},{0,0,0}};

Tractor front end joints

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.

(* Castor/Camber *)
(* tr=0.02; tl=0.02; *)

(* Joint 2 right front spindle *)
r2={1}; q2={delta2}; p2={wdel2}; 
H2=Transpose[{{-sr,-tr,1,0,0,0}}];

(* Joint 3 left front spindle *)
r3={1}; q3={delta3}; p3={wdel3}; 
H3=Transpose[{{-sl,tl,1,0,0,0}}];

Tractor-Semi joint

(* Joint 4 5th wheel *)
r4={1}; q4={delta4}; p4={wdel4};
H4=Transpose[{{0,0,1,0,0,0}}];

JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2},{r3,H3,q3,p3},{r4,H4,q4,p4}};

Joint constructions

{T,{V,X,H}}=Timing[Joints[JointLst]];

Print[T];

Body data

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. 

	           wb = wheelbase (186")
	           R=tire radius (12")
	           2b=track (72-11/16")
	           pin= pin location from rear bogie (8")
	           d=tractor rear axle location from rear bogie (28")
	           tb=trailor base (388")
	           kappa=tire coefficient (approx 14,000 N?)

wb=186*.0254; R=12*.0254; b=(72+11/16)*.0254;
pin=8*.0254; d=28*.0254; tb=388*.0254;
Jtracxx=1.414*10^3; Jtracyy=5.684*10^4; Jtraczz=5.698*10^4;Jtracxz=1.96*10^3;
Jtrailxx=2.202*10^5; Jtrailyy=4.852*10^5; Jtrailzz=5.065*10^5;
mtrac=1.15*10^4; mtrail=4.376*10^4; mtire=40;
Ixx=mtire*(R^2)/2; Iyy=mtire*(3*R^2+(11*.0254)^2)/12; Izz=Iyy;

(* Body 1 tractor *)
cm1={2.4466,0,.9052};
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}}};
I1={{Jtracxx,0,Jtracxz},{0,Jtracyy,0},{Jtracxz,0,Jtraczz}};

(* Body 2 right front wheel *)
cm2={0,0,0}; out2={{9,{sr*R,tr*R,-R}}};
I2=DiagonalMatrix[{Ixx,Iyy,Izz}];

(* Body 3 left front wheel *)
cm3={0,0,0}; out3={{10,{sl*R,-tl*R,-R}}};
I3=DiagonalMatrix[{Ixx,Iyy,Izz}];

(* Body 4 trailor *)
cm4={-5.77,0,1.0592}; 
out4={{11,{-tb,-b,-R}},{12,{-tb,b,-R}}};
I4=DiagonalMatrix[{Jtrailxx,Jtrailyy,Jtrailzz}];

BodyLst={{cm1,out1,mtrac,I1},{cm2,out2,mtire,I2},{cm3,out3,mtire,I3},{cm4,out4,mtrail,I4}};
TreeLst={{{1,1},{2,2}},{{1,1},{3,3}},{{1,1},{4,4}}};

q={theta,x,y,delta2,delta3,delta4};
p={wth,vx,vy,wdel2,wdel3,wdel4};

Tire Forces

We will compute the tire generalized forces. This requires eight similar, and somewhat complicated, calculations. To reduce computation time, we use the function KinematicReplacements. The right front tire force is computed both with and without this enhancement. We find a factor of 10 improvement in computational speed.

(* Front right tire *)

ChnLst={{1,1},{2,2}};
Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};
VelNames={w9x,w9y,w9z,v9x,v9y,v9z};
TerminalNode=9;
{T,Q1}=Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]

Print[T]

{V,X,H,rules1,rules2}=KinematicReplacements[V,X,H,q];

rules1

(* Front right tire *)
ChnLst={{1,1},{2,2}};
Force={0,0,0,0,-kappa*ArcTan[v9y/v9x],0};
VelNames={w9x,w9y,w9z,v9x,v9y,v9z};
TerminalNode=9;
Q1=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

{T,Q1}=Timing[GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]]

Print[T];

(* Front left tire *)
ChnLst={{1,1},{3,3}};
Force={0,0,0,0,-kappa*ArcTan[v10y/v10x],0};
VelNames={w10x,w10y,w10z,v10x,v10y,v10z};
TerminalNode=10;
Q2=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Tractor rear rightÐforward *)
ChnLst={{1,1}};
Force={0,0,0,FDrive,-kappa*ArcTan[v5y/v5x],0};
VelNames={w5x,w5y,w5z,v5x,v5y,v5z};
TerminalNode=5;
Q3=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Tracter rear rightÐbackward *)
ChnLst={{1,1}};
Force={0,0,0,FDrive,-kappa*ArcTan[v7y/v7x],0};
VelNames={w7x,w7y,w7z,v7x,v7y,v7z};
TerminalNode=7;
Q4=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Tractor rear leftÐforward *)
ChnLst={{1,1}};
Force={0,0,0,FDrive,-kappa*ArcTan[v6y/v6x],0};
VelNames={w6x,w6y,w6z,v6x,v6y,v6z};
TerminalNode=6;
Q5=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Tractor rear left backward *)
ChnLst={{1,1}};
Force={0,0,0,FDrive,-kappa*ArcTan[v8y/v8x],0};
VelNames={w8x,w8y,w8z,v8x,v8y,v8z};
TerminalNode=8;
Q6=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Trailor right *)
ChnLst={{1,1},{4,4}};
Force={0,0,0,0,-kappa*ArcTan[v11y/v11x],0};
VelNames={w11x,w11y,w11z,v11x,v11y,v11z};
TerminalNode=11;
Q7=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

(* Trailor left *)
ChnLst={{1,1},{4,4}};
Force={0,0,0,0,-kappa*ArcTan[v12y/v12x],0};
VelNames={w12x,w12y,w12z,v12x,v12y,v12z};
TerminalNode=12;
Q8=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames];

Q=Q1+Q2+Q3+Q4+Q5+Q6+Q7+Q8;

Create model

{V,X,H,Q}=Chop[{V,X,H,Q}/.rules1,.001];
{V,X,H,M,Csys,Fsys,psys,qsys}=CreateModel[JointLst,BodyLst,TreeLst,g,0,Q,V,X,H];

{V,X,H,M,Csys,Fsys,psys,qsys}={V,X,H,M,Csys,Fsys,psys,qsys}/.rules2;

yout={X[[1]][[1,4]]/.0254,X[[1]][[2,4]]/.0254};

Inputs = {FDrive};
Outputs = yout;
MEXFilename = "truck.c";
paramvec={sr,sl,kappa,g};
PassedParams = {"X0","sr","sl","kappa","g"};
PassedParamsDimensions = {{2*Length[p],1},{1,1},{1,1},{1,1},{1,1}};

System Reduction & Creation of Mex File

For analysis purposes we create a simulation model with the following two features. First, we assume that delta2 and delta3 are inputs rather than coordinates determined by the dynamics. Thus, we eliminate two degrees of freedom. Second, we choose to ignore any steering imperfections and assume delta2 and delta3 are equal and call them both delta.

qred=Part[qsys,{1,2,3,6}];
pred=Part[psys,{1,2,3,6}];
Mred=Part[M,{1,2,3,6},{1,2,3,6}]/.Inner[Rule,{delta2,delta3},{0,0},List];
Cred=Part[Csys,{1,2,3,6},{1,2,3,6}]/.Inner[Rule,{delta2,delta3},{0,0},List];
Fred=Part[Fsys,{1,2,3,6}]/.Inner[Rule,{delta2,delta3},{delta,delta},List];
Vred=V[[{1,4}]];

Inputs = {FDrive,delta};

MEXFilename = "truck.c";
paramvec={sr,sl,kappa,g};
PassedParams = {"X0","sr","sl","kappa","g"};
PassedParamsDimensions = {{2*Length[pred],1},{1,1},{1,1},{1,1},{1,1}};

FpPrec = SetPrecision[Fred,5];
MPrec = SetPrecision[Mred,5];
CpPrec=SetPrecision[Cred,5];


CreateModelMEX[pred,qred,Inputs,Outputs,PassedParams,PassedParamsDimensions,
                              Vred,CpPrec,FpPrec,MPrec, MEXFilename,AssignMaxSize->300];

Inputs

Outputs

Constraints

Introductory Remarks

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. 

One 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.

Some chain and loop structures give rise to differential constraints - 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.'

Even 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.

<<ProPac`

A Simple Closed Chain

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.

r1={1};
H1=Transpose[{{1,0,0,0,0,0}}];
q1={theta1};p1={ω1};

r2={1};
H2=Transpose[{{1,0,0,0,0,0}}];
q2={theta2};p2={ω2};

JointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};

Mass1=m1; CenterOfMass1={0,0,L1/2}; OutboardNode1={2,{0,0,L1}};
InertiaMatrix1=DiagonalMatrix[{m1*L1^2/12,m1*L1^2/12,0}];

Mass2=m2; CenterOfMass2={0,0,L2/2}; OutboardNode2={3,{0,0,L2}};
InertiaMatrix2=DiagonalMatrix[{m2*L2^2/12,m2*L2^2/12,0}];

BodyList={{CenterOfMass1,{OutboardNode1},Mass1,InertiaMatrix1},
          {CenterOfMass2,{OutboardNode2},Mass2,InertiaMatrix2}};

TreeList={{{1,1},{2,2}}};
PE=0; Q={0,0};

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointList,BodyList,TreeList,g,PE,Q];

ChnBodyList={{CenterOfMass1,{0,0,L1},Mass1,InertiaMatrix1},
             {CenterOfMass2,{0,0,L2},Mass2,InertiaMatrix2}};

EndPos=EndEffector[ChnBodyList,X];

G=EndPos[[{2},4]]

The relation G=0 is not solvable for either theta1 or theta2 for all values of L1 and L2 - although, as we will describe below, it can be solved for particular values of these parameters. Thus, we will not try to eliminate any configuration coordinate. The constrained dynamics are obtained with:

{Mm,Cm,Fm,Vm,phat,qhat}=AlgebraicConstraints[M,Cp,Fp,V,G,p,q]

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:

{Mm,Cm,Fm,Vm}=Simplify[{Mm,Cm,Fm,Vm}/.{L1->L,L2->L,m1->m,m2->m}]

It is convenient to assemble the corresponding differential equations.

Eqns=MakeODEs[phat,qhat,Vm,Mm,Cm,Fm,t]

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:

SpEqns=Simplify[Eqns/.theta2[t]->-2*theta1[t]]

Simple trigonometric identities can be used to verify that these equations are equivalent to those given by Ginsberg for this example.

Rule1 = 
      D[w1[t], t] -> 
        D[D[theta1[t], t]/(-L Cos[theta1[t]]), t]

Simplify[(SpEqns[[3]] /. 
            w1[t] -> D[theta1[t], t]/(-L Cos[theta1[t]])) /. 
        Rule1]

Another Closed Chain

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.

r1={1,1};
H1=Transpose[{{0,0,1,0,0,0},{0,0,0,1,0,0}}];
q1={theta1,x};p1={w1,vx};

r2={1};
H2=Transpose[{{0,0,1,0,0,0}}];
q2={theta2};p2={w2};

JointList={{r1,H1,q1,p1},{r2,H2,q2,p2}};

Mass1=m1; CenterOfMass1={L1/2,0,0}; OutboardNode1={2,{L1,0,0}};
InertiaMatrix1=DiagonalMatrix[{0,m1*L1^2/2,m1*L1^2/2}];

Mass2=m2; CenterOfMass2={L2/2,0,0}; OutboardNode2={3,{L2,0,0}};
InertiaMatrix2=DiagonalMatrix[{0,m2*L2^2/2,m2*L2^2/2}];

BodyList={{CenterOfMass1,{OutboardNode1},Mass1,InertiaMatrix1},
          {CenterOfMass2,{OutboardNode2},Mass2,InertiaMatrix2}};

TreeList={{{1,1},{2,2}}};
PE=0; Q={0,0,0};

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointList,BodyList,TreeList,g,PE,Q];

ChnBodyList={{CenterOfMass1,{L1,0,0},Mass1,InertiaMatrix1},
             {CenterOfMass2,{L2,0,0},Mass2,InertiaMatrix2}};

EndPos=EndEffector[ChnBodyList,X];

G=EndPos[[{1},4]]

Since G=0 always has a solution for x, we will eliminate x from the constrained equations.

{Mm,Cm,Fm,Vm,phat,qhat}=AlgebraicConstraints[M,Cp,Fp,V,G,p,q,{2}]

Simplify[Cm.phat]

The Rolling Disk: a Classic Example of Differential Constraints

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:

r={6};
H=IdentityMatrix[6]

q={psi,theta,phi,x,y,z};
p={wx,wy,wz,vx,vy,vz};
JointList={{r,H,q,p}};

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-y-x (or 3-2-1) convention. 
The body data is:

(* reassign som variables *)
g="g"; m="m"; J="J"; Iy="Iy"; R="R";

Mass=m; CenterOfMass={0,0,0}; OutboardNode={2,{0,-R*Sin[psi],-R*Cos[psi]}};
InertiaMatrix=DiagonalMatrix[{J,Iy,Iy}];
BodyList={{CenterOfMass,{OutboardNode},Mass,InertiaMatrix}};

and the remaining data:

TreeList={{{1,1}}};
PE=0; Q={0,0,0,0,0,0};

The unconstrained disk model is obtained with

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointList,BodyList,TreeList,g,PE,Q];

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 or NodeVelocity.

{ChainList}=TreeList;
VCont=NodeVelocity[ChainList,BodyList,X,H,p]

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:

{Mm,Cm,Fm,Vm,T,phat}=DifferentialConstraints[M,Cp,Fp,V,VCont[[Range[4,6]]],p,q,{4,5,6}];

The dynamics are reduced to three dimensions and the original six quasiÐvelocities are reduced to three, in fact, we have:

phat

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.

{Sols}=Solve[{X[[1]][[3,4]]==R*(Cos[theta]-1)},{z}]

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:

Mmo=Simplify[Mm/.Sols];

Vmo=Simplify[Vm[[ {1,2,3,4,5} ]]/.Sols];

Now, we assemble the governing equations.

Eqns=MakeODEs[phat,q[[{1,2,3,4,5}]],Vmo,Mmo,Cm,Fm,t]

Since x,y,z locate the center of gravity it is of interest to compute the contact point.

ChnBodyList={{CenterOfMass,{0,-R*Sin[psi],-R*Cos[psi]},Mass,InertiaMatrix}};

{ContactPoint}=Simplify[Transpose[EndEffector[ChnBodyList,X][[{1,2,3},{4}]]]/.Sols]

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:

a1=wyy[t]==Cos[psi[t]]*wy[t]-Sin[psi[t]]*wz[t]
a2=wzz[t]==Sin[psi[t]]*wy[t]+Cos[psi[t]]*wz[t]

The inverse transformation rules are obtained using Mathematica's Solve function

Trans=Simplify[Flatten[Solve[{a1,a2},{wy[t],wz[t]}]]]

The transformed equations are obtained with the function StateTransformation:

StateVars={psi,theta,phi,x,y,wx,wy,wz}

TrEqns=StateTransformation[Eqns,StateVars,Trans,{wyy,wzz},t]

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 angular coordinates:

Rules1=Flatten[Solve[TrEqns[[{1,2,8}]],{wx[t],wyy[t],wzz[t]}]]

Rules2={wx'[t]->D[wx[t]/.Rules1,t],
        wyy'[t]->D[wyy[t]/.Rules1,t],
        wzz'[t]->D[wzz[t]/.Rules1,t]}

Rules3={theta[t]->th[t]-Pi/2,
        theta'[t]->th'[t],
        theta''[t]->th''[t]}

Rules4={phi[t]->-pi/2+ph[t],
        phi'[t]->ph'[t],
        phi''[t]->ph''[t]}

LagEqns=Simplify[TrEqns[[{5,6,7}]]/.Rules1/.Rules2/.Rules3/.Rules4]

These equations are easily confirmed to be equivalent to those given by Meirovitch.

A Rigid Drive Train

The system is a chain consisting of 3 bodies (coupler, forward shaft and aft shaft), 3 joints (one revolute andtwo universal) and and one constraints (cylindrical).

Body Data

Body 1: Coupler

com1={0,0,0}; mass1=12925.9; out1={{2,{0,0,0}}};
Inertia1=DiagonalMatrix[{3.6112 10^5,3.8792 10^5,3.6794 10^5}];

Body1={com1,out1,mass1,Inertia1};

Body 2: Forward Shaft

com2={0,175/2,0}; mass2=24232.2; out2={{3,{0,175,0}}};
Inertia2=DiagonalMatrix[{(mass2*(3*25^2+175^2))/12,(mass2*25^2)/2,(mass2*(3*25^2+175^2))/12}];

Body2={com2,out2,mass2,Inertia2};

Body 3: Aft Shaft

com3={0,10,0}; mass3=19618.8; out3={{6,{0,10,0}},{4,{0,20,0}}};
Inertia3=DiagonalMatrix[{2.2558 10^6,2.8837 10^5,2.2416 10^6}];

Body3={com3,out3,mass3,Inertia3};

BodyLst=Chop[{Body1,Body2,Body3},10^-5];

Joint Data

Joint 1: Revolute

r1={1};H1={{0},{1},{0},{0},{0},{0}};
q1={a1y};p1={w1y};
Joint1={r1,H1,q1,p1};

Joints 2 and 3: Universal

r2={1,1};H2={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};
q2={a2x,a2z};p2={w2x,w2z};
Joint2={r2,H2,q2,p2};

Universal joints have singularities which restritc them to 'local' two degree of freedom rotations, if the joint operates within the allowable limits the relative motion is equivalent to a spherical joint. Treating universal joints a spherical generally produces simpler dynamical equations. For variety, we do that with the last three joints.

r3={1,1};H3={{1,0},{0,0},{0,1},{0,0},{0,0},{0,0}};
q3={a3x,a3z};p3={w3x,w3z};
Joint3={r3,H3,q3,p3};


JointLst={Joint1,Joint2,Joint3};

Tree Definition

The tree consists of one chain, so we have:

TreeLst={{{1,1},{2,2},{3,3}}};

The Unconstrained Tree Model

PE=0;Q={0,0,0,0,0};

{V,X,H}=Joints[JointLst];

{V,X,H,M,Cp,Fp,p,q}=CreateModel[JointLst,BodyLst,TreeLst,0,PE,Q,V,X,H];

Adding the Constraints

There are two algebraic constraint sets. One at node 3 of the fore shaft constrains the node x-z coordinates. Similarly, the second applies at node 4 of the aft shaft and fixes its x and z coordinates. To set up the constraints, we compute the relevent node locations (3 and 4) in the space frame using the EndEffector function.

AftShaftPos=Chop[EndEffector[4,TreeLst,BodyLst,X],10^-5];
Gangles={AftShaftPos[[1,2]],AftShaftPos[[2,1]],AftShaftPos[[2,3]],
    AftShaftPos[[2,2]]-1,AftShaftPos[[3,2]]};
ForeShaftPos=Chop[EndEffector[3,TreeLst,BodyLst,X],10^-5];

G={ForeShaftPos[[1,4]]/175-alpha,ForeShaftPos[[3,4]]/175+alpha,
    AftShaftPos[[1,4]]/175-alpha,AftShaftPos[[3,4]]/175+alpha};
Gs=SeriesExpansion[G,{a2x,a2z,a3x,a3z},1];

Ms=Chop[N[SeriesExpansion[M/10^8,{a2x,a2z,a3x,a3z},1]],10^-4];
Cps=Chop[N[SeriesExpansion[Cp/10^8,{a2x,a2z,a3x,a3z},1]],10^-4];
Fps=Fp/10^8;

Vs=V;

{Mm,Cm,Fm,Vm,phat,qhat}=AlgebraicConstraints[Ms,Cps,Fps,Vs,Gs,p,q,{2,3,4,5},{2,3,4,5},{2,3,4,5}];

Mm

Cm

Fm

Bibliography

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.

V. I. Arnold, Mathematical Methods of Classical Mechanics, Springer-Verlag: New York, 1989 (2nd ed.)
N. G. Chetaev, Theoretical Mechanics, Springer-Verlag: New York, 1989.
S. H. Crandall, D. C. Karnopp, E. F. Kurtz, D. C. Pridmore-Brown, Dynamics of Mechanical and Electromechanial Systems, McGraw-Hill: New York, 1968.
F. Gantmacher, Lectures in Analytical Mechanics, MIR: Moscow, 1975.
J. H. Ginsberg, Advanced Engineering Dynamics, Harper & Row: New York, 1988.
H. Goldstein, Classical Mechanics, Addison-Wesley: Reading, 1981 (2nd ed.)
D. T. Greenwood, Classical Mechanics, Prentice-Hall: Englewood Cliffs, 1977.
T. R. Kane and D. A. Levinson, Dynamics: Theory and Application, McGraw-Hill: New York, 1985.
C. Lanczos, The Variational Principles of Mechanics, University of Toronto Press: Toronto, 1970 (4th ed.)
L.Meirovitch, Methods of Analytical Dynamics, McGraw-Hill: New York, 1970.
Ju. I. Neimark and N. A. Fufaev, Dynamics of Nonholonomic Systems, American Mathematical Society: Providence, 1972.
R. M. Rosenberg, Analytical Dynamics of Discrete Systems, Plenum: New York, 1977.