(*                TSiDynamics 



           Techno-Sciences, Inc.
           10001 Derekwood Lane, Suite 204
           Lanham, MD 20706
           (301) 577-6000
           
           
Copyright © 1993, 1997, 1998 Techno-Sciences Incorporated
All Rights Reserved          
           
*)
(* 
Summary: Formulate Poincare's Equations for tree structures 
with rigid & flexible bodies and construct simulation code.
Generate models for tree structures with supplementalalgebraic
and differential constraints.
*)
(* ***********************************************************************)
spell1 = (Head[General::spell1] === $Off);
spell = (Head[General::spell] === $Off);
Off[General::spell1];
Off[General::spell]; 


BeginPackage["ProPac`Dynamics`",{"ProPac`NDTools`","ProPac`GeoTools`"}]

(* ****************************************************** *)

(* Function Usage Statements *)

(****************************************************************)

(* Package Help *)

(****************************************************************)

Dynamics::usage = 
"Functions included in the Dynamics package are:\n
\n
Kinematics:\n
RotationMatrixEuler, ConfigurationMatrixEuler,\n
AToATilda,ATildaToA,SimpleJointKinematics,\n
CompoundJointKinematics,CompoundJointMap,Joints,\n
JointRotation,JointTranslation,CompoundJointConfiguration,\n
SimpleJointConfiguration,RotationMatrixToEuler,\n
EndEffector,RelativeConfiguration,\n
NodeVelocity,KinematicReplacements,\n
\n
Generalized Forces & Potential Functions:\n
GeneralizedForce,DamperForce,SpringForce,GravPotential,\n
SpringPotential,DamperPotential,LeafPotential,\n
RayleighDissipationForce,BacklashPotential,\n
BacklashForce,JointFrictionPotential,\n
FlxDissPot,FlxStrnPot,\n
\n
Inertial Parameters:
RgdBdyInrShift,ChainInertia,TreeInertia,\n
\n
Model Assembly:\n
CMatrix,CreateModelSpecial,CreateModel,\n
PoincareCoefficients,PoincareFunctionCombined,PoincareFunction,\n
AlgebraicConstraints,DifferentialConstraints,\n
MakeODEs,StateTransformation, MakeLagrangeEquations,\n
CreateModelMEX,FindMostFrequentTerms,\n
\n
Miscellaneous:\n
SeriesExpansion,Truncate,MatlabForm\n
"


(* Kinematics *)

RotationMatrixEuler::usage =
"RotationMatrixEuler[chsi] gives the matrix for rotation defined by the\n 
specified 3 2 1 Euler angles chsi={phi,theta,psi}. Note that\n 
chsi is ordered in 1 2 3 or x-y-z convention. That is the rotations\n
are as follows: psi about z, then theta about new y, and then phi\n
about new x."

RotationMatrixToEuler::usage =
"RotationMatrixToEuler[L] determines the 3 2 1 Euler angles equivalent to\n
the rotation matrix L under the assumption that each angle satisfies\n
-Pi/2 < angle < Pi/2. "

ConfigurationMatrixEuler::usage = 
"ConfigurationMatrixEuler[chsi,r] gives the 4x4 matrix element of the Euclidean\n
defined by the Euler angles chsi and translation r."

AToATilda::usage =
"AToATilda[a] with a={a1,a2,a3} gives vector as antisymmetric matrix"

ATildaToA::usage =
"ATildaToA[A] with A a 3x3 antisymmetric matrix constructs the vector\n
{A32,A13,A21}"

SimpleJointKinematics::usage =
"SimpleJointKinematics[H,eps] gives rxr kinematic matrix for joint with 6xr joint map\n 
matrix H. eps is joint parameter vector of dim r."

CompoundJointKinematics::usage =
"CompoundJointKinematics[r,H,eps] returns list of matrices which are the diagonal elements\n 
of the block diagonal matrix gamma for a compound joint."

CompoundJointMap::usage =
"CompoundJointMap[r,H,eps]; r={r1,..rp} is a vector of dimension p where p is the\n 
number of joint frames; H is the composite joint map matrix in which r-ith\n 
columns are the ith frame joint map matrix; eps is the joint coordinate\n
vector of dimension r1+..+rp; Coordinates are labled corresponding to\n
frames; returns joint map matrix for compound joint."

Joints::usage =
"Joints[JointLst] returns {V,X,H} the kinematic matrix V(q), Euclidean joint\n 
configuration matrices X(q) and composite Joint map H(q) in terms of the\n 
coordinate vector joint q. JointLst is a list containing joint data. Each\n
joint is characterized as a compound joint with a vector r of integers, a\n
matrix H, and a vector of joint coordinate names q {r,H,q,p}. JointLst is a\n 
list of these quadruples.\n
\n
The list of Euclidean joint configuration matrices X is arranged so that\n 
X[[i]] is the ith joint coordinate transformation matrix from the joint inboard\n
frame coordinates to the joint outboard frame coordinates.
"  

JointRotation::usage =
"JointRotation[h,eps] gives rotation matrix for rotation about vector h of\n
angle eps*Length[h]."

JointTranslation::usage =
"JointTranslation[h,g,eps] gives translation vector for joint rotating about h and\n 
translating along g with motion parameter eps."                  

CompoundJointConfiguration::usage =
"CompoundJointConfiguration[r,H,eps] gives 4x4 matrix element of Eucldean group cooresponding\n 
to a compound joint; r={r1,..,rp} is a vector of dimension p where p is the\n
number of joint frames; H is the composite joint map matrix in which r-ith\n
columns are the ith frame joint map matrix; eps is the joint coordinate\n
vector of dimension r1+..+rp; Coordinates are labled corresponding to\n
frames; returns joint map matrix for compund joint." 

SimpleJointConfiguration::usage =
"SimpleJointConfiguration[H,eps] gives 4x4 matrix element of Eucldean group corresponding to\n
a simple joint 6xr map matrix H with motion parameter vector eps of dim r."

TerminalNodeChain::usage =
"TerminalNodeChain[NodeNumber,TreeList,BodyList] returns a chain list\n
terminating with body containing outboard node NodeNumber"

(* Energy Functions *)

ChainInertia::usage =
"ChainInertia[ChnLst,BodyLst,X,H,q] returns the inerta matrix of a chain\n
given a chain list, an ordered list of body data,BodyLst,an ordered list of\n 
joint configuration matrices, X, an ordered list of composite joint map\n
matrices H, and an ordered list of joint parameter names, q."   

EndEffector::usage =
"EndEffector[BodyLst,X] returns the Euclidean configuration matrix of a\n
frame in the last body of a chain, with origin at the outboard joint\n
location. BodyLst is list of body data in nonstandard chain form, X\n
is a corresponding list of joint Euler configuration matrices.\n
\n
EndEffector can also be used in the form\n
\n
EndEffector[ChnLst,TerminalNode,BodyLst,X]\n
\n
where BodyLst is the standard body data structure. ChnLst identifies\n
the system subchain that terminates with TerminalNode.\n
\n
EndEffector can also be used in the form\n
\n
EndEffector[TerminalNode,TreeList,BodyLst,X]\n
\n
in the event that the appropriate ChnLst has not been identified.
"
EndEffectorVelocity::usage ="Obsolete function replaced by\n
NodeVelocity.
"
RelativeConfiguration::usage =
"RelativeConfiguration[Node1,Node2,TreeList,BodyList,X,q] returns\n
the configuration matrix for a body fixed frame at node Node2 as\n
seen by an observer in a body fixed frame at node Node1.
"
NodeVelocity::usage =
"NodeVelocity[ChnLst,TerminalNode,BodyLst,X,H,pp] returns the velcity\n
at TerminalNode, where the body data, BodyLst, the joint data X and H,\n
and the quasivelocity names pp (includes joint and flex velocities)\n
corresponds to the chain defined by ChnLst. The velocity is a six element\n
vector defined in the body fixed frame.\n
\n
This function is used by GeneralizedForce.\n
\n
The following syntax may also be used\n
\n
NodeVelocity[ChnLst,TerminalNode,BodyLst,X,H,q,p]\n
\n
NodeVelocity[TerminalNode,TreeLst,BodyLst,X,H,q,p]\n
\n
See EndEffector and GeneralizedForce.
"

GeneralizedForce::usage =
"Q=GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames],\n
where\n
\n
  ChnLst and TerminalNode define the location of the point at which the\n
  external force is applied. ChnLst defines a chain and TerminalNode is\n
  the number of the node of interest in the last body of the chain.\n
\n
  BodyLst is the list of system body data.\n
\n
  X,H,q,p are the usual lists of joint data.\n
\n
  F is a list of 6 expressions which defines the external torques (first\n
  three) and forces (last three) in terms of body velocities (velocities\n
  of a body fixed frame at the terminal node).\n
\n
  VelNames is a list of (6) names of the velocities used in the expressions\n
  F. There must be six names - the first 3 corresponding to the angular\n
  velocity and the last 3 to the linear velocity.\n
\n
  Q is the generalized force vector.\n
\n
GeneralizedForce sets up the appropriate data structures to use\n
NodeVelocity\n
\n
An alternative calling syntax is\n
\n
Q=GeneralizedForce[TerminalNode,TreeLst,BodyLst,X,H,q,p,Force,VelNames]\n
\n
in which case the appropriate chain is identified from the system data"

DamperPotential::usage =
"DamperForce[Node1,Node2,DisPot,TreeList,BodyList,X,H,q,p] returns the\n
dissipation potential associated with a damper connecting nodes:\n
Node1 and Node2. DisPot is the damper dissipation function in terms of the\n
relative velocity across the damper (along the damper axis). DisPot\n
must be defined as a pure function."

DamperForce::usage =
"DamperForce[Node1,Node2,DisPot,TreeList,BodyList,X,H,q,p] returns the
generalized force produced by a damper connecting nodes: Node1 and Node2.
DisPot is the damper dissipation function in terms of the
relative velocity across the damper (along the damper axis). DisPot
must be defined as a pure function."

GravPotential::usage =
"GravPotential[BodyLst,TreeLst,X] returns the gravitational potential 
energy function/g of a tree composed of rigid and flexible bodies. 
BodyLst is list of body data, X is list of Euler configuration matrices for 
the joints, TreeLst is a list of chains which defines the tree."

LeafPotential::usage =
"LeafPotential[BodyLst,TreeLst,X,H,Pot,NameLst] returns the elastic\n
potential energy associated with leaf absolute position in terms of the\n
system generalized coordinates. Pot is a scalar potential energy function\n 
which is the sum of all leaf potential energy functions. Each leaf\n
potential energy function is specified as a function of the leaf outboard\n
joint (x,y,z)-coordinates in the space frame. NameLst is a list of three-\n
vectors, the names given to leaf coordinates. Thus, the purpose of\n 
LeafPotential is to replace NameLst by q in Pot."   

SpringPotential::usage =
"SpringPotential[Node1,Node2,Pot,TreeList,BodyList,X,q] returns the\n
potential energy expression in terms of the configuration coordinates q\n
of a spring connected between nodes: Node1 and Node2. Pot is the\n
spring potential energy as a function of its length. Pot must be\n
defined as a pure function."

SpringForce::usage =
"SpringForce[Node1,Node2,Pot,TreeList,BodyList,X,q] returns the\n
generalized force expression in terms of the configuration coordinates q\n
of a spring connected between nodes: Node1 and Node2. Pot is the\n
spring potential energy as a function of its length. Pot must be\n
defined as a pure function. Using spring force rather than SpringPotential\n
may yield simpler expressions when the Pot is not smooth.
"

FlxDissPot::usage = 
"FlxDissPot[BodyLst] returns the total dissipation function for a system 
containing flexible bodies."     

FlxStrnPot::usage = 
"FlxStrnPot[BodyLst] returns the total strain potential energy for a system 
containing flexible bodies."

RgdBdyInrShift::usage =
"RgdBdyInrShift[Inertiacm,cm,mass] returns rigid body inertia about body 
frame origin. Inertiacm is inertia matrix about center of mass, cm is 
location of center of mass in body frame, m is mass."

TreeInertia::usage =
"TreeInertia[BodyLst,TreeLst,X,H,q,p] returns the tree inertia matrix M(q). 
Each flex body is characterized by five items. The location of the center 
of mass in the body frame with origin at inboard joint, a list of pairs 
outboard joints (number) and their location and orientation (in body 
frame), the mass and the inertia matrix about the inboard joint, and a list 
of flex coordinate names and a list of velocity names. BodyLst is a list of 
these sextuples. TreeLst is a list of chains which  defines the tree 
structure."

KinematicReplacements::usage =
"KinematicReplacements[V,X,H] returns {Vnew,Xnew,Hnew,rules} where
repeated groups of expressions in V,X,H are replaced by temporary
variable to produce Vnew,Xnew,Hnew. The original forms are recovered
by applying the rule list 'rules'. For complex systems, computations 
with Vnew,Xnew,Hnew can save considerable time. 

Warning: The replacements must be made before using CreateModelSpecial or
CreateModel.
  
KinematicReplacements[V,X,H,q] returns {Vnew,Xnew,Hnew,rules1,rules2}.
In this usage, rules is divided into two sets. rules1 depend on the
coordinates q. rules2 do not. rules2 involves expressions that depend
only on system parameters. The application of rules1 must occur before
using CreateModelSpecial or CreateModel but rules2 can be applied at any time.
"    

(* Dynamics *)

CMatrix::usage =
"CMatrix[M,V,q,p] returns C matrix for Poincare's equations. M is inetia 
matrix, V is list of kinematic matrices, q is vector of coordinates and p 
is vector of quasi-velocities"     


CreateModelSpecial::usage = 
"CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,Q] 

returns

{V,X,H,M,F,p,q}

where

V is the velocity transformation matrix,
X is a list of joint configuration matrices
H is a list of joint map matrices 
M is the inertia matrix and 
F is the right hand side of Poincare's equations.

CreateModelSpecial may also be called with the argument lists:

CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q]
where DissPot is a dissipation potential function (Lur'e type).

or

CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H],
CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q,V,X,H] 

This avoids repeating the joint kinematics computations if that 
has already been completed."     

CreateModel::usage = 
"CreateModel[JointLst,BodyLst,TreeLst,g,PE,Q] 

returns

{V,X,H,M,C,F,p,q}

where

V is the velocity transformation matrix (as a list of matrices),
X is a list of joint configuration matrices
H is a list of joint map matrices 
M is the inertia matrix 
C is matrix premultiplying p in Poincare's equations
F is the remainder of right hand side of Poincare's equations.

CreateModel may also be called with the argument lists:

CreateModel[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q]
where DissPot is a dissipation potential function (Lur'e type).

CreateModel[JointLst,BodyLst,TreeLst,g,PE,Q,V,X,H]
or
CreateModel[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q,V,X,H] 

This avoids repeating the joint kinematics computations if that 
has already been completed."

PoincareCoefficients::usage =
"PoincareCoefficients[V,q,p] returns coefficient matrix used in forming Poincare's 
equations" 

PoincareFunctionCombined::usage =
"PoincareFunctionCombined[M,V,PE,Q,p,q,V,X,H] or 
PoincareFunctionCombined[M,V,PE,Q,p,q] returns Poincare Function given inertia matrix M, 
list of kinematic matrices V, potential energy function PE and generalized 
force vector Q."

PoincareFunction::usage =
"PoincareFunction[M,V,PE,Q,p,q,V,X,H] or
PoincareFunction[M,V,PE,Q,p,q] returns Poincare Function as a list of two 
items, the C matrix and a function containing every thing else. The inputs 
are inertia matrix M, list of kinematic matrices V, potential energy 
function PE and generalized force vector Q."

(* Constraints *)

DifferentialConstraints::usage = 
"DifferentialConstraints[M,Cmat,F,V,G,p,q] returns modified matrices
Mm,Cm,Fm,Vm and T, phat. M,C,F define the dynamics of the 
unconstrained system and V defines the kinematics of the
unconstrained system. Mm,Cm,Fm,Vm are the corresponding
quantities for a system with constraints of the form
G(q,p)=A(q)p=0. The columns of T span ker[A(q)] and phat are 
the names of the reduced quasi-velocities.
DifferentialConstraints[M,Cmat,F,V,G,p,q,Index] permits specification
of the quasi-velocity elements to be removed, i.e., in defining
the constrained dynamics the elements p(Index) are eliminated 
when possible."

AlgebraicConstraints::usage =
"AlgebraicConstraints[M,Cmat,F,V,G,p,q,Index] returns modified
system parameters Mm,Cm,Fm,Vm and reduced state variables 
phat, qhat that correspond to a system constrained by ideal
algebraic constraints G(q)=0. M,C,F define the dynamics of the 
unconstrained system and V defines the kinematics of the
unconstrained system. Mm,Cm,Fm,Vm are the corresponding
quantities for the system with constraints. Index is a list of
indices corresponding to the elements of q for which G(q)
is generically uniquely solvable. These variables will be eliminated 
from the reduced model. Index may be the empty list, or 
equivalently, this argument may be dropped: 
AlgebraicConstraints[M,Cmat,F,V,G,p,q]. 
A second index list may also be specified, in a call of the form: 
AlgebraicConstraints[M,Cmat,F,V,G,p,q,Index,DIndex]. The second index
specifies the quasi-velocities to be eliminated if possible. This
is useful when the specification of the coordinates to be 
removed (via Index) is not complete, but the modeler has some insight
into the appropriate velocities to be eliminated"   

StateTransformation::usage=
"StateTransformation[DiffEqns,StateVars,TransRules,NewVars,t] 
returns a system of transformed differential equations. 
DiffEqns is a list of first order differential equations that 
are to be transformed. StateVars is a list of the state variables.  
TransRules is a list of transformation rules, each in the form 
old_variable->F(new_variables). Only the variables to be 
eliminated need to be specified by a rule. t is the independent
variable.

StateTransformation returns a different result with the calling
syntax
{NewRHS,NewStateVars}=StateTransformation[RHS,StateVars,TransRules,NewVars].
Inthis case it returns a new right hand side, i.e.
given: x_dot=RHS(x) , obtain: xnew_dot=NewRHS(xnew)

Yet a different result returns when called as
StateTransformation[A,RHS,StateVars,TransRules,NewVars].
In this case it returns {NewA, NewRHS,NewStateVars}, i.e.
given: A(x)x_dot=RHS(x) obtain: NewA(xnew)xnew_dot=NewRHS(xnew)
" 

(* Code Constructions *)

MakeODEs::usage=
"MakeODEs[p,q,V,M,C,F,t] builds and returns a list of ordinary differential\n
equations that can be integrated in Mathematica.\n
\n
MakeODEs[q,F,t] builds and returns a list of ordinary differential\n
equations that can be integrated in Mathematica.\n
"

MakeLagrangeEquations::usage=
"LagrangeEquations[T,U,R,Q,t,q,p] assembles and returns Lagrange\n
Equations in a form that can be integrated in Mathematica. The\n 
arguments are\n
\n
t         independent variable (time)
q         list of generalized cordinate names\n
p         list of generalized velocity names (labels for q_dot)\n
T(q,p)    Kinetic energy function\n
U(q)      Potential energy function\n
R(q,p)    Raleigh dissipaton function\n
Q(q,p,t)  generalized forces\n
"

GenerateDissipationTerms::usage=
"Obsolete version of RayleighDissipationForce"

RayleighDissipationForce::usage = 
"Q = RayleighDissipationForce[qDot,R,V,p,q] generates the list Q that 
contains the energy dissipation terms resulting from the dissipation 
function defined in R.  The arguments are:
  qDot - a list containing names of the coordinate time derivatives 
  R - an expression for the dissipation function interms of q, qDot
  V - the kinematic matrix list
  p,q - lists of the state variables"

(*
MatlabForm::usage = 
"S = MatlabForm[Matrix,MatrixName] converts the matrix or vector expression given 
by the first argument Matrix into a Matlab readable and executable string with a
variable name given by the second argument MatrixName. For example,

x_matlabform = MatlabForm[{{1,2},{3,4}},x] 
returns a string of the form 

xmatlabform = x=[1,2;3,4];

This function is useful for transfering matrices from Mathematica to Matlab via an ASCII
file. The process is as follows: 

  (1) Using the above function, convert the pertinent matrices to 
      Matlab readable strings.
  (2) Write the strings to an ASCII file.
  (3) At the Matlab prompt, type the name of the file which will execute the matrix assignments."
*)

(* Analysis Tools *)

SeriesExpansion::usage =
"SeriesExpansion[A,x,n], Scalar,matrix or vector A(x), vector x and integer n.\n 
Produces a power series expansion about x=0 up to terms of order n.\n
"
Truncate::usage =
"Truncate[A,x,n], Scalar,matrix or vector A(x), vector x and integer n.\n 
Drops x terms of order higher n. Produces results equivalent to SeriesExpansion\n
but may be in a different (unexpanded) form. Generally, significantly\n
faster than SeriesExpansion.\n 
"


(* ****************************************************** *)

(*  Functions *)

Begin["`private`"]

(*****  Analysis Tools *****)

Clear[SeriesExpansion];
SeriesExpansion[A_,x_?VectorQ,n_Integer]:=Module[{
MySeries,YYY,YY,a},
MySeries[FF_,{XX__}]:=Normal[Series[FF,XX]];
YYY=MySeries[A,Map[{#,0,n}&,x]]/.Inner[Rule,x,a*x,List];
YY=Normal[Series[YYY,{a,0,n}]]/.{a->1};
Chop[YY]
]
SetAttributes[SeriesExpansion,ReadProtected];
SetAttributes[SeriesExpansion,Protected];
SetAttributes[SeriesExpansion,Locked];

Clear[Truncate];
Truncate[A_List,x_List,m_Integer]:=Map[Truncate[#,x,m]&,A]
Truncate[A_,x_List,m_Integer]:=
  Chop[Normal[(A/.Inner[Rule,x,a*x,List])+O[a]^(m+1)]/.a->1];
SetAttributes[Truncate,ReadProtected];
SetAttributes[Truncate,Protected];
SetAttributes[Truncate,Locked];

(***** Kinematics *****)

Clear[RotationMatrixEuler];
RotationMatrixEuler[chsi_] := 
        { { Cos[theta] Cos[psi],Cos[theta] Sin[psi],-Sin[theta] } , 
          { Sin[phi] Sin[theta] Cos[psi] - Cos[phi] Sin[psi],
            Sin[phi] Sin[theta] Sin[psi] + Cos[phi] Cos[psi],
            Sin[phi] Cos[theta] } ,
          { Cos[phi] Sin[theta] Cos[psi] + Sin[phi] Sin[psi],
            Cos[phi] Sin[theta] Sin[psi] - Sin[phi] Cos[psi],
            Cos[phi] Cos[theta] } }/.{phi->chsi[[1]],theta->chsi[[2]],
psi->chsi[[3]]};
SetAttributes[RotationMatrixEuler,ReadProtected];
SetAttributes[RotationMatrixEuler,Protected];
SetAttributes[RotationMatrixEuler,Locked];

Clear[RotationMatrixToEuler];
RotationMatrixToEuler[L_?MatrixQ]:={ArcTan[L[[1,1]],L[[1,2]]],
                           ArcSin[-L[[1,3]]],
                           ArcTan[L[[3,3]],L[[2,3]]]};
SetAttributes[RotationMatrixToEuler,ReadProtected];
SetAttributes[RotationMatrixToEuler,Protected];
SetAttributes[RotationMatrixToEuler,Locked];

Clear[ConfigurationMatrixEuler];
ConfigurationMatrixEuler[chsi_,r_] := Module[{L,X1,X},(
                          L=Simplify[RotationMatrixEuler[chsi]];
                          X1=Transpose[Join[L,{r}]];
                          X=Join[X1,{{0,0,0,1}}]
                          )];
SetAttributes[ConfigurationMatrixEuler,ReadProtected];
SetAttributes[ConfigurationMatrixEuler,Protected];
SetAttributes[ConfigurationMatrixEuler,Locked];

Clear[AToATilda];
AToATilda[a_] := { {0,-a[[3]],a[[2]]}, 
                {a[[3]],0,-a[[1]]}, 
                {-a[[2]],a[[1]],0} }
SetAttributes[AToATilda,ReadProtected];
SetAttributes[AToATilda,Protected];
SetAttributes[AToATilda,Locked];

Clear[ATildaToA];
ATildaToA[A_] := {A[[3,2]],A[[1,3]],A[[2,1]]}
SetAttributes[ATildaToA,ReadProtected];
SetAttributes[ATildaToA,Protected];
SetAttributes[ATildaToA,Locked];

Clear[JointRotation];
(*
JointRotation[h_,eps_] := SpecialMatrixExp[h,-eps];
*)
JointRotation[h_,eps_]:=Module[{a,b,c},
 If[h.h==0,Return[IdentityMatrix[3]]];
 Simplify[{{(a^2 + b^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps] + 
 c^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2), 
 (2*Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]*
 (a^2*c*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 b^2*c*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 c^3*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 a*b*(a^2 + b^2 + c^2)^(1/2)*
 Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]))/
 (a^2 + b^2 + c^2)^(3/2), 
 (a*c)/(a^2 + b^2 + c^2) - 
 (a*c*Cos[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2) - 
 (b*Sin[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2)^(1/2)}, 
 {(-2*Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]*
 (a^2*c*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 b^2*c*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 c^3*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] - 
 a*b*(a^2 + b^2 + c^2)^(1/2)*
 Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]))/
 (a^2 + b^2 + c^2)^(3/2), 
 (b^2 + a^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps] + 
 c^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2), 
 (b*c)/(a^2 + b^2 + c^2) - 
 (b*c*Cos[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2) + 
 (a*Sin[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2)^(1/2)}, 
 {(2*Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]*
 (a^2*b*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 b^3*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 b*c^2*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 a*c*(a^2 + b^2 + c^2)^(1/2)*
 Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]))/
 (a^2 + b^2 + c^2)^(3/2), 
 (-2*Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]*
 (a^3*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 a*b^2*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] + 
 a*c^2*Cos[((a^2 + b^2 + c^2)^(1/2)*eps)/2] - 
 b*c*(a^2 + b^2 + c^2)^(1/2)*
 Sin[((a^2 + b^2 + c^2)^(1/2)*eps)/2]))/
 (a^2 + b^2 + c^2)^(3/2), 
 (c^2 + a^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps] + 
 b^2*Cos[(a^2 + b^2 + c^2)^(1/2)*eps])/
 (a^2 + b^2 + c^2)}}/.Inner[Rule,{a,b,c},h,List]]
];
SetAttributes[JointRotation,ReadProtected];
SetAttributes[JointRotation,Protected];
SetAttributes[JointRotation,Locked];

Clear[JointTranslation];
JointTranslation[h_,g_,eps_] := Module[{V,sig},( V=JointRotation[h,-sig].g;Integrate[V,{sig,0,eps}])]
SetAttributes[JointTranslation,ReadProtected];
SetAttributes[JointTranslation,Protected];
SetAttributes[JointTranslation,Locked];

Clear[JointRotationJointTranslation];
JointRotationJointTranslation[h_,g_,eps_] := Module[{V,sig},( 
      V=JointRotation[h,-sig];
      {V/.{sig->-eps},Integrate[V.g,{sig,0,eps}]}
      )]
SetAttributes[JointRotationJointTranslation,ReadProtected];
SetAttributes[JointRotationJointTranslation,Protected];
SetAttributes[JointRotationJointTranslation,Locked];

Clear[SimpleJointConfiguration];
SimpleJointConfiguration[H_,eps_] := Module[{C,HH,SL,SR,L,R,i,h,g,q,r,s,t},(
                  {t}=Dimensions[eps];If[VectorQ[H],
                                         r=1,
                                         {q,r}=Dimensions[H]]; 
                                         If[r<=6 && t==r,t=r,Print["invalid joint parameters"]];
                  If[r==1,HH=Outer[Times,H,{1}],HH=H];
                  SL=IdentityMatrix[3];SR={0,0,0};
                  Do[
                  ( h=Flatten[HH[[{1,2,3},{i}]]];g=Flatten[HH[[{4,5,6},{i}]]];
                  {L,R}=JointRotationJointTranslation[h,g,eps[[i]]];
                  (*
                  L=JointRotation[h,eps[[i]]]; R=JointTranslation[h,g,eps[[i]]];
                  *)
                  SL=Transpose[L].SL; SR=Transpose[L].SR+R;
                  ),
                  {i,r}];
                  C=Transpose[Append[Transpose[SL],SR]];
                  Append[C,{0,0,0,1}] )]
SetAttributes[SimpleJointConfiguration,ReadProtected];
SetAttributes[SimpleJointConfiguration,Protected];
SetAttributes[SimpleJointConfiguration,Locked];

Clear[SimpleJointKinematics];
SimpleJointKinematics[H_,eps_] := Module[{D,HH,B,b,c,SL,SR,L,R,i,h,g,q,r,s,t},(
                  {t}=Dimensions[eps];If[VectorQ[H],
                                         r=1,
                                         {q,r}=Dimensions[H]]; 
                                         If[r<=6 && t==r,t=r,Print["invalid joint parameters"]];
                  If[r==1,HH=Outer[Times,H,{1}],HH=H];
                  b={}; c={};
                  SL=IdentityMatrix[3];SR={0,0,0};
                  Do[
                  ( h=Flatten[HH[[{1,2,3},{i}]]];g=Flatten[HH[[{4,5,6},{i}]]];
                  {L,R}=JointRotationJointTranslation[h,g,eps[[i]]];
                  (*
                  L=JointRotation[h,eps[[i]]]; R=JointTranslation[h,g,eps[[i]]];
                  *)
                  bb=Simplify[ATildaToA[SL.AToATilda[h].Transpose[SL]]];
                  cc=Simplify[SL.AToATilda[h].SR+SL.(g)];
                  b=Join[b,{bb}];
                  c=Join[c,{cc}];
                  SL=SL.L; SR=Transpose[L].SR+R;
                  ),
                  {i,r}];
                  B=Join[Transpose[b],Transpose[c]];
                  D=Simplify[Transpose[B].B];
                  Simplify[Simplify[Inverse[D]].Simplify[(Transpose[B].H)]]
                  )]
SetAttributes[SimpleJointKinematics,ReadProtected];
SetAttributes[SimpleJointKinematics,Protected];
SetAttributes[SimpleJointKinematics,Locked];

Clear[HCompnd];
HCompnd[r_,H_,eps_]:= Module[{i,j,XX,HH,h1,h2,rows,cols,rr,rsum,p,
                  para,veceps,JointM1,JointM2},(
                  {p}=Dimensions[r];
                  rows={1,2,3,4,5,6};veceps=Outer[Times,eps,{1}];
                  XX=IdentityMatrix[4];rsum=0;Xdot=ZeroMatrix[4];
                  JointM1={};JointM2={};
                  Do[rr=r[[i]];cols={};Do[dd=rsum+j;cols=Append[cols,dd],{j,1,rr}];
                     HH=H[[rows,cols]];para=Flatten[veceps[[cols,{1}]]];
                     Do[h1tilda=Transpose[XX[[{1,2,3},{1,2,3}]]].AToATilda[Flatten[HH[[{1,2,3},{j}]]]].XX[[{1,2,3},{1,2,3}]];
                     h1=ATildaToA[h1tilda];JointM1=Append[JointM1,h1];
                     h2=Transpose[XX[[{1,2,3},{1,2,3}]]].(AToATilda[Flatten[HH[[{1,2,3},{j}]]]].XX[[{1,2,3},{4}]]+HH[[{4,5,6},{j}]]);
                     JointM2=Append[JointM2,Flatten[h2]],
                     {j,1,rr}];
                  XX=SimpleJointConfiguration[HH,para].XX;
                  rsum=rr+rsum,
                  {i,1,p}];
                  Simplify[Join[Transpose[JointM1],Transpose[JointM2]]]
                  )]
SetAttributes[HCompnd,ReadProtected];
SetAttributes[HCompnd,Protected];
SetAttributes[HCompnd,Locked];

Clear[CompoundJointConfiguration];
CompoundJointConfiguration[r_,H_,eps_]:=Module[{s,rsum,H1,epso,XX},
                    (
                    {s}=Dimensions[r];
                    rsum=0;XX=IdentityMatrix[4];
                    Do[
                      (
                      H1=H[[Range[1,6],Range[rsum+1,rsum+r[[j]]]]];epso=Take[eps,{rsum+1,rsum+r[[j]]}];
                      rsum=rsum+r[[j]]; 
                      XX=SimpleJointConfiguration[H1,epso].XX;
                      ),
                    {j,s}];
                    Simplify[XX]
                    )]
SetAttributes[CompoundJointConfiguration,ReadProtected];
SetAttributes[CompoundJointConfiguration,Protected];
SetAttributes[CompoundJointConfiguration,Locked];

Clear[CompoundJointKinematics];
CompoundJointKinematics[r_,H_,eps_]:= Module[{s,rtot,epso,V},(
             {s}=Dimensions[r];rtot=0;Do[rtot=rtot+r[[j]],{j,s}];
               rsum=0;V={};
               Do[
                 (
                 H1=H[[Range[1,6],Range[rsum+1,rsum+r[[j]]]]];
                 epso=Take[eps,{rsum+1,rsum+r[[j]]}];
                 rsum=rsum+r[[j]]; 
                 GamTemp=SimpleJointKinematics[H1,epso];V=DiagJoin[V,GamTemp];
                 ),
               {j,s}];
             V
             )]                       
SetAttributes[CompoundJointKinematics,ReadProtected];
SetAttributes[CompoundJointKinematics,Protected];
SetAttributes[CompoundJointKinematics,Locked];

Clear[Joints];
Joints[JointLst_]:= Module[{K,J,dof,r,s,rtot,H,eps,HC,H1,HH,
             epso,V,VTemp,X},(
             K=Length[JointLst];
             (* For each joint, construct joint map matrix HCompnd, kinematic
             matrix SimpleJointKinematics, and Euclidean location matrix CompoundJointConfiguration; Assemble 
             composite kinematic matrix V(q) and composite Joint map H(q) *) 
             dof=0;HH={};V={};X={};
             Do[
             (Print["Computing joint "<>ToString[i]<>" kinematics"];
             Joint=JointLst[[i]];r=Joint[[1]];H=Joint[[2]];eps=Joint[[3]];
             {s}=Dimensions[r];
             HC=HCompnd[r,H,eps];
             HH=Append[HH,HC];
               rsum=0;VTemp={};
               Do[
                 (
                 H1=H[[Range[1,6],Range[rsum+1,rsum+r[[j]]]]];epso=Take[eps,{rsum+1,rsum+r[[j]]}];
                 rsum=rsum+r[[j]]; 
                 GamTemp=SimpleJointKinematics[H1,epso];VTemp=DiagJoin[VTemp,GamTemp];
                 ),
               {j,s}];
             V=Append[V,VTemp];  
             X=Append[X,CompoundJointConfiguration[r,H,eps]];
             dof=dof+rtot;
             ),
             {i,K}];
             {V,X,HH}
             )]
SetAttributes[Joints,ReadProtected];
SetAttributes[Joints,Protected];
SetAttributes[Joints,Locked];

(* **** Energy Functions **** *)

Clear[ChainInertia];
ChainInertia[ChnLst_,BodyLst_,X_,H_,q_]:= Module[{K,J,angles,SpatInertia,
             ZeroIdent,Body,NextBody,co,cm,coLoc,BodyType,NextBodyType,JointNo,Inertia,Inertiacm,
             InertiaShift,TempPhi,mass,L,mtot,Phi,PhiLst,BigPhi,BigPhi1,
             CapPhi,CapPhiLast,CapPhi0,CapPhi1,HH,PhiHH,M,
             Space1,Space2,Flxco,FlxCoord,HList,NoFlx,NoNxtFlx,NoVel,
             XX,lambda},(
             K=Length[X];J=Length[BodyLst];If[K==J,J=K,Print["not a chain"]];
             (* For each body, construct velocity transfer matrix phi,
             and spatial inertia matrix about inboard joint *)
             PhiLst={};SpatInertia={};HList={};
             (* lambdaList={{}}; *)
             Do[(
             Body=BodyLst[[i]];
             (* Identify as Rigid or Flexible *)
             BodyType=Length[Body];
             If[BodyType==4,
               (
               (* For Rigid Body *)
               cm=Body[[1]];
               If[i<J,
                 JointNo=ChnLst[[i+1]][[1]];
                 {co}=Cases[Body[[2]],{JointNo,xx_}->xx];
                 NextBody=BodyLst[[i+1]];NextBodyType=Length[NextBody];
                 If[NextBodyType==4,NoNxtFlx=0,NoNxtFlx=Length[NextBody[[6]]]],
                 JointNo=Body[[2]][[1]][[1]];co=Body[[2]][[1]][[2]];
                 NextBodyType=4];
               HList=Append[HList,H[[i]]];
               mass=Body[[3]];Inertiacm=Body[[4]];
                   If[i==J,XX=IdentityMatrix[6],XX=X[[i+1]]];
               L=Transpose[XX[[{1,2,3},{1,2,3}]]];
               TempPhi=Join[L,L.AToATilda[-co]];
               ZeroIdent=Transpose[Join[DiagonalMatrix[{0,0,0}],L]];
               Phi=Transpose[Join[Transpose[TempPhi],ZeroIdent]];
               If[NextBodyType==4,BigPhi=Phi,
                 BigPhi=Join[Phi,Table[0,{NoNxtFlx},{6}]]];
              PhiLst=Append[PhiLst,BigPhi];
              If[Length[Inertiacm]==6,SpatInertia=Append[SpatInertia,Inertiacm],
               InertShift=mass*{{cm[[2]]^2+cm[[3]]^2,-cm[[1]]*cm[[2]],-cm[[1]]*cm[[3]]},
                              {-cm[[1]]*cm[[2]],cm[[1]]^2+cm[[3]]^2,-cm[[2]]*cm[[3]]},
                              {-cm[[1]]*cm[[3]],-cm[[2]]*cm[[3]],cm[[2]]^2+cm[[1]]^2}};
               Inertia=Inertiacm+InertShift;Space1=Transpose[Join[Inertia,-mass*AToATilda[cm]]];
               Space1=Transpose[Join[Inertia,-mass*AToATilda[cm]]];
               Space2=Transpose[Join[mass*AToATilda[cm],mass*IdentityMatrix[3]]];
               SpatInertia=Append[SpatInertia,Transpose[Join[Space1,Space2]]]
               ];
               ),
               (
               (* For Flex Body *)
               FlxCoord=Body[[5]];NoFlx=Length[FlxCoord];
               If[i<J,
                 JointNo=ChnLst[[i+1]][[1]];
                 {Flxco}=Cases[Body[[2]],{JointNo,xx_}->xx];
                 NextBody=BodyLst[[i+1]];NextBodyType=Length[NextBody];
                 If[NextBodyType==4,NoNxtFlx=0,NoNxtFlx=Length[NextBody[[6]]]],
                 JointNo=Body[[2]][[1]][[1]];Flxco=Body[[2]][[1]][[2]];
                 NextBodyType=4];
               NoVel=Length[Transpose[H[[i]]]];
               BigH1=Transpose[Join[Transpose[H[[i]]],Table[0,{NoFlx},{6}]]];
               BigH2=Transpose[Join[Table[0,{NoVel},{NoFlx}],IdentityMatrix[NoFlx]]];
               BigH=Join[BigH1,BigH2];HList=Append[HList,BigH];
               mass=Body[[3]];
               SpatInertia=Append[SpatInertia,Body[[4]][[1]]];
               coLoc=Flxco.Flatten[Join[{{1}},Transpose[{FlxCoord}]]];
               angles={coLoc[[1]],coLoc[[2]],coLoc[[3]]};
               Xco=ConfigurationMatrixEulerSm[angles,coLoc[[{4,5,6}]]];
               If[i==J,XX=Xco,XX=X[[i+1]].Xco];
               L=Transpose[XX[[{1,2,3},{1,2,3}]]];
               co=XX[[{1,2,3},4]];TempPhi=Join[L,L.AToATilda[-co]];
               ZeroIdent=Transpose[Join[DiagonalMatrix[{0,0,0}],L]];
               Phi=Transpose[Join[Transpose[TempPhi],ZeroIdent]];
               lambda=DiagJoin[L,L].Flxco[[Range[1,6],
                                           Range[2,(NoFlx+1)]]];
               BigPhi1=Transpose[Join[Transpose[Phi],Transpose[lambda]]];
               If[NextBodyType==4,BigPhi=BigPhi1,
                 BigPhi=Join[BigPhi1,Table[0,{NoNxtFlx},{6+NoFlx}]]];
               PhiLst=Append[PhiLst,BigPhi];
               )];
             ),
             {i,J}];
             (* Assemble composite chain inertia matrix *)
             (* First Construct CapPhi Matrix *)
             If[K==1,mtot=Length[HList[[1]]];
             CapPhi=IdentityMatrix[mtot],
             mtot=Length[HList[[1]]];
             CapPhi=IdentityMatrix[mtot];
             CapPhiLast=IdentityMatrix[mtot];
             Do[(
             m=Length[PhiLst[[i]]];
             CapPhi0=Join[Transpose[PhiLst[[i]].CapPhiLast],IdentityMatrix[m]];
             CapPhiLast=Transpose[CapPhi0];
             CapPhi1=Transpose[Join[Transpose[CapPhi],Table[0,{m},{mtot}]]];
             CapPhi=Join[CapPhi1,CapPhiLast];
             mtot=mtot+m;
             ),
             {i,K-1}]];
              (* Now Construct Inertia Matrix M(q) *)
             HH={};
                Do[(HH=DiagJoin[HH,HList[[i]]];
                ),{i,K}];
             PhiHH=CapPhi.HH;
             M=Transpose[PhiHH].DiagJoinLst[SpatInertia].PhiHH;
             M
             )]
SetAttributes[ChainInertia,ReadProtected];
SetAttributes[ChainInertia,Protected];
SetAttributes[ChainInertia,Locked];

Clear[TreeInertia];
TreeInertia[BodyLst_,TreeLst_,X_,H_,q_,p_]:=Module[{N,beta,dof,njoints,Vel,rtot,s,r,
             ChnLst,DoneChnLst,ChnLength,ChnBodyLst,ChnX,ChnH,Chnq,ChnV,ChnM,ChnKE,
             ExChnKE,BodyType,NoBodies,Mbeta,pp,M,
             ExcessKELst,NewExChnKE,TempExcessKELst,KELst,TreeKE
             },(
             (* For each chain of the tree, (i) compute chain KE,
             (ii) identify excess elements Û which themselves form
             a subchain originating at the tree root, (iii) compute
             the subchain KE and subtract *)
             dof=Length[q];beta=p;
             njoints=Length[H];rtot=0;qvec={};Vel={};
                Do[({s,r}=Dimensions[H[[j]]];
                qvec=Append[qvec,Take[q,{rtot+1,rtot+r}]];
                Vel=Append[Vel,Take[beta,{rtot+1,rtot+r}]];
                rtot=rtot+r;
                ),{j,njoints}];
             (* Initializations for main loop *)
             N=Length[TreeLst];DoneChnLst={};TreeKE=0;ExcessKELst={};
             Do[(
             ChnLst=TreeLst[[i]];ChnLength=Length[ChnLst];
             (* Compose ChnBodyLst, ChnX, ChnH and Chnq *)
                ChnBodyLst={}; ChnX={}; ChnH={}; Chnq={};ChnV={};
                Do[(
                ChnBodyLst=Append[ChnBodyLst,BodyLst[[ChnLst[[k]][[2]]]]];
                ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
                ChnH=Append[ChnH,H[[ChnLst[[k]][[1]]]]];
                Chnq=Join[Chnq,qvec[[ChnLst[[k]][[1]]]]];
                BodyType=Length[BodyLst[[ChnLst[[k]][[2]]]]];
                If[BodyType==4,
                  ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]],
                  ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]];
                  ChnV=Join[ChnV,BodyLst[[ChnLst[[k]][[2]]]][[6]]]
                ];
                (* add flex deformation velocities *)
                ),{k,ChnLength}];
             (* Get Chain Inertia and Form KE *)
             ChnM=ChainInertia[ChnLst,ChnBodyLst,ChnX,ChnH,Chnq];
             ChnKE=(ChnV.ChnM).ChnV/2;
             (* Remove excess bodies KE. First define the 
             excess chain *)
             ExChnLst={};
                Do[(ChnElement=ChnLst[[j]];
                XtraSet=Intersection[DoneChnLst,{ChnElement}];
                ExChnLst=Join[ExChnLst,XtraSet];
                ),{j,ChnLength}];
             (* Update DoneChnLst *)
             DoneChnLst=Union[DoneChnLst,ChnLst];
             If[ExChnLst=={},ExChnKE=0,
                (* Has the KE for this subchain been computed before? *)
                KELst=Cases[ExcessKELst,{ExChnLst,xx_}->xx];
                If[Length[KELst]>=1,
                  ExChnKE=KELst[[1]][[2]],  
                  (* Otherwise compute it *)
                  ChnLst=ExChnLst;ChnLength=Length[ChnLst];
                  (* Compose ChnBodyLst, ChnX, ChnH and Chnq *)
                  ChnBodyLst={}; ChnX={}; ChnH={}; Chnq={};ChnV={};
                  Do[(
                  ChnBodyLst=Append[ChnBodyLst,BodyLst[[ChnLst[[k]][[2]]]]];
                  ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
                  ChnH=Append[ChnH,H[[ChnLst[[k]][[1]]]]];
                  Chnq=Join[Chnq,qvec[[ChnLst[[k]][[1]]]]];
                  BodyType=Length[BodyLst[[ChnLst[[k]][[2]]]]];
                  If[BodyType==4,
                    ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]],
                    ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]];
                    ChnV=Join[ChnV,BodyLst[[ChnLst[[k]][[2]]]][[6]]]
                  ];
                  (* add flex deformation velocities *)
                  ),{k,ChnLength}];
                  (* Get Chain Inertia and Form KE *)
                  ChnM=ChainInertia[ChnLst,ChnBodyLst,ChnX,ChnH,Chnq];
                  ExChnKE=(ChnV.ChnM).ChnV/2;
                  (* Add result to list of excess chain kinetic energies *)
                  NewExChnKE={ExChnLst,ExChnKE};TempExcessKELst=ExcessKELst;
                  ExcessKELst=Union[TempExcessKELst,NewExChnKE]]
                ];
             TreeKE=TreeKE+Chop[ChnKE-ExChnKE];
             ),{i,N}];
             (* Define list of system quasi-velocities:
             these are joint quasi-velocities plus flex body deformation
             velocities *)
             pp=p;NoBodies=Length[BodyLst];
             Do[(
             BodyType=Length[BodyLst[[i]]];
             If[BodyType==4,
               pp=pp,
               pp=Join[pp,BodyLst[[i]][[6]]]
             ];
             ),{i,NoBodies}];
             Mbeta=Grad[TreeKE,pp];
             M=Jacob[Mbeta,pp];
             Chop[M]
             )]
SetAttributes[TreeInertia,ReadProtected];
SetAttributes[TreeInertia,Protected];
SetAttributes[TreeInertia,Locked];

Clear[EndEffector];
EndEffector[TerminalNode_Integer,TreeList_List,BodyList_,X_]:=
    Module[{ChnLst}, 
      ChnLst=TerminalNodeChain[TerminalNode,TreeList,BodyList];
      EndEffector[ChnLst,TerminalNode,BodyList,X]
    ];
EndEffector[ChnLst_List,TerminalNode_Integer,BodyLst_,X_]:=
    Module[{ChnBdyLst,ChnX}, 
      {ChnBdyLst,ChnX}=DefineChainBodyX[ChnLst,TerminalNode,BodyLst,X];
      EndEffector[ChnBdyLst,ChnX]
    ];
EndEffector[BodyLst_,X_]:= Module[{K,J,XPartial,Temp1,XNext,oc,Xoc},(
                         K=Length[BodyLst];J=Length[X];If[K==J,J=K,Print["incompatible data"]];
             Xoc=IdentityMatrix[4];Temp1=Transpose[Join[IdentityMatrix[3],{{0,0,0}}]];
             Do[(
               Body=BodyLst[[i]];
               (* Identify as Rigid or Flexible *)
               BodyType=Length[Body];
               If[BodyType==4,
                 (
                 (* For Rigid Body *)
                 oc=Body[[2]];
                 XNext=X[[i]];XPartial=Xoc.XNext;
                 Xoc=XPartial.Transpose[Join[Temp1,{Join[oc,{1}]}]];
                 ),
                 (
                 (* For Flex Body *)
                 FlxCoord=Body[[5]];
                 Flxco=Body[[2]];
                 coLoc=Flxco.Flatten[Join[{{1}},Transpose[{FlxCoord}]]];
                 XNext=X[[i]];XPartial=Xoc.XNext;
                 angles={coLoc[[1]],coLoc[[2]],coLoc[[3]]};
                 Xoc=XPartial.ConfigurationMatrixEulerSm[angles,coLoc[[{4,5,6}]]];
                 )];
             ),{i,K}];
             Chop[Xoc]
             )]             
SetAttributes[EndEffector,ReadProtected];
SetAttributes[EndEffector,Protected];
SetAttributes[EndEffector,Locked];

Clear[RelativeConfiguration];
RelativeConfiguration[Node1_Integer,Node2_Integer,TreeList_,BodyList_,X_,q_]:=
     Module[{Chain1,Chain2},
     Chain1=TerminalNodeChain[Node1,TreeList,BodyList];
     Chain2=TerminalNodeChain[Node2,TreeList,BodyList];
     RelativeConfiguration[Chain1,Node1,Chain2,Node2,BodyList,X,q]
     ];
RelativeConfiguration[Chain1_List,Node1_Integer,Chain2_List,Node2_Integer,
    BodyList_,X_,q_]:=
     Module[{CommonElements,LastCommonElement,SubChain1,SubChain2,
    Config1,Config2,ShortChain,L1,P1,X1},
     CommonElements=Intersection[Chain1,Chain2];
     LastCommonElement=Last[CommonElements];
     ShortChain=Complement[Chain1,CommonElements];
     If[ShortChain!={},
       SubChain1=Join[{LastCommonElement},ShortChain],
       SubChain1={LastCommonElement}
     ];
     ShortChain=Complement[Chain2,CommonElements];
     If[ShortChain!={},
       SubChain2=Join[{LastCommonElement},ShortChain],
       SubChain2={LastCommonElement}
     ];
     Config1=EndEffector[SubChain1,Node1,BodyList,X];
     Config2=EndEffector[SubChain2,Node2,BodyList,X];
                L1=Config1[[{1,2,3},{1,2,3}]];
                P1=-Transpose[L1].Config1[[{1,2,3},4]];
                X1=Join[Transpose[Join[L1,{P1}]],{{0,0,0,1}}];
                Chop[Simplify[X1.Config2]]
     ];
SetAttributes[RelativeConfiguration,ReadProtected];
SetAttributes[RelativeConfiguration,Protected];
SetAttributes[RelativeConfiguration,Locked];

Clear[EndEffectorVelocity];
EndEffectorVelocity[TerminalNode_Integer,TreeLst_List,BodyLst_,X_,H_,q_,p_]:=
       NodeVelocity[TerminalNode,TreeLst,BodyLst,X,H,q,p]
EndEffectorVelocity[ChnLst_List,TerminalNode_Integer,BodyLst_List,X_,H_,q_,p_]:=
       NodeVelocity[ChnLst,TerminalNode,BodyLst,X,H,q,p]
EndEffectorVelocity[ChnLst_,BodyLst_,X_,H_,pp_]:=
       NodeVelocity[ChnLst,BodyLst,X,H,pp]
SetAttributes[EndEffectorVelocity,ReadProtected];
SetAttributes[EndEffectorVelocity,Protected];
SetAttributes[EndEffectorVelocity,Locked];

Clear[NodeVelocity];
NodeVelocity[TerminalNode_Integer,TreeLst_List,BodyLst_,X_,H_,q_,p_]:=
             Module[{ChnLst},
             ChnLst=TerminalNodeChain[TerminalNode,TreeLst,BodyLst];
             NodeVelocity[ChnLst,TerminalNode,BodyLst,X,H,q,p]
]
NodeVelocity[ChnLst_List,TerminalNode_Integer,BodyLst_List,X_,H_,q_,p_]:= 
Module[{ChnBodyLst,ChnX,ChnH,Chnq,ChnV},
        {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
                DefineChainData[ChnLst,TerminalNode,BodyLst,X,H,q,p];
        NodeVelocity[ChnLst,ChnBodyLst,ChnX,ChnH,ChnV]
]
NodeVelocity[ChnLst_,BodyLst_,X_,H_,pp_]:= Module[{K,J,PhiLst,
             ZeroIdent,Body,co,cm,BodyType,JointNo,TempPhi,L,Phi,BigPhi,
             BigPhi1,CapPhi,Flxco,FlxCoord,HList,NoNxtFlx,PhiLastHH,Velocity,
             CapPhiLast,XX,lambda,NextBody,NextBodyType},(
             K=Length[X];J=Length[BodyLst];If[K==J,J=K,Print["not a chain"]];
             (* For each body, construct velocity transfer matrix phi *)
             PhiLst={};HList={};
             (* lambdaList={{}}; *)
             Do[(
             Body=BodyLst[[i]];
             (* Identify as Rigid or Flexible *)
             BodyType=Length[Body];
             If[BodyType==4,
               (
               (* For Rigid Body *)
               cm=Body[[1]];
               If[i<J,
                 JointNo=ChnLst[[i+1]][[1]];
                 {co}=Cases[Body[[2]],{JointNo,xx_}->xx];
                 NextBody=BodyLst[[i+1]];NextBodyType=Length[NextBody];
                 If[NextBodyType==4,NoNxtFlx=0,NoNxtFlx=Length[NextBody[[6]]]],
                 JointNo=Body[[2]][[1]][[1]];co=Body[[2]][[1]][[2]];
                 NextBodyType=4];
               HList=Append[HList,H[[i]]];
               If[i==J,XX=IdentityMatrix[6],XX=X[[i+1]]];
               L=Transpose[XX[[{1,2,3},{1,2,3}]]];
               TempPhi=Join[L,L.AToATilda[-co]];
               ZeroIdent=Transpose[Join[DiagonalMatrix[{0,0,0}],L]];
               Phi=Transpose[Join[Transpose[TempPhi],ZeroIdent]];
               If[NextBodyType==4,BigPhi=Phi,
                 BigPhi=Join[Phi,Table[0,{NoNxtFlx},{6}]]];
               PhiLst=Append[PhiLst,BigPhi];
               ),
               (
               (* For Flex Body *)
               FlxCoord=Body[[5]];NoFlx=Length[FlxCoord];
               If[i<J,
                 JointNo=ChnLst[[i+1]][[1]];
                 {Flxco}=Cases[Body[[2]],{JointNo,xx_}->xx];
                 NextBody=BodyLst[[i+1]];NextBodyType=Length[NextBody];
                 If[NextBodyType==4,NoNxtFlx=0,NoNxtFlx=Length[NextBody[[6]]]],
                 JointNo=Body[[2]][[1]][[1]];Flxco=Body[[2]][[1]][[2]];
                 NextBodyType=4];
               NoVel=Length[Transpose[H[[i]]]];
               BigH1=Transpose[Join[Transpose[H[[i]]],Table[0,{NoFlx},{6}]]];
               BigH2=Transpose[Join[Table[0,{NoVel},{NoFlx}],IdentityMatrix[NoFlx]]];
               BigH=Join[BigH1,BigH2];HList=Append[HList,BigH];
               coLoc=Flxco.Flatten[Join[{{1}},Transpose[{FlxCoord}]]];
               angles={coLoc[[1]],coLoc[[2]],coLoc[[3]]};
               Xco=ConfigurationMatrixEulerSm[angles,coLoc[[{4,5,6}]]];
               If[i==J,XX=Xco,XX=X[[i+1]].Xco];
               L=Transpose[XX[[{1,2,3},{1,2,3}]]];
               co=XX[[{1,2,3},4]];TempPhi=Join[L,L.AToATilda[-co]];
               ZeroIdent=Transpose[Join[DiagonalMatrix[{0,0,0}],L]];
               Phi=Transpose[Join[Transpose[TempPhi],ZeroIdent]];
               lambda=Simplify[DiagJoin[L,L].Flxco[[Range[1,6],
                                           Range[2,(NoFlx+1)]]]];
               BigPhi1=Transpose[Join[Transpose[Phi],Transpose[lambda]]];
               If[NextBodyType==4,BigPhi=BigPhi1,
                 BigPhi=Join[BigPhi1,Table[0,{NoNxtFlx},{6+NoFlx}]]];
               PhiLst=Append[PhiLst,BigPhi];
               )];
             ),
             {i,J}];
             (* Assemble Last Rows of CapPhi Matrix *)
             If[K==1,mtot=Length[HList[[1]]];
             CapPhiLast=IdentityMatrix[mtot],
             mtot=Length[HList[[1]]];
             CapPhi=IdentityMatrix[mtot];
             CapPhiLast=IdentityMatrix[mtot];
             Do[(
             m=Length[PhiLst[[i]]];
             CapPhi0=Join[Transpose[Simplify[PhiLst[[i]].CapPhiLast]],IdentityMatrix[m]];
             CapPhiLast=Transpose[CapPhi0];
             mtot=mtot+m;
             ),
             {i,K-1}]];
             (* Transfer to Outboard Joint of Last Body *)
             CapPhiLast=Simplify[PhiLst[[K]].CapPhiLast];
             (* Now Construct Quasi-Velocity Coefficients *)
             HH={};
                Do[(HH=DiagJoin[HH,HList[[i]]];
                ),{i,K}];
             PhiLastHH=Simplify[CapPhiLast.HH];
             Velocity=Chop[PhiLastHH.pp]
             )]               
SetAttributes[NodeVelocity,ReadProtected];
SetAttributes[NodeVelocity,Protected];
SetAttributes[NodeVelocity,Locked];
                        
Clear[GeneralizedForce];
GeneralizedForce[TerminalNode_Integer,TreeLst_List,BodyLst_,X_,H_,q_,p_,Force_,VelNames_]:=
             Module[{ChnLst},
             ChnLst=TerminalNodeChain[TerminalNode,TreeLst,BodyLst];
             GeneralizedForce[ChnLst,TerminalNode,BodyLst,X,H,q,p,Force,VelNames]
             ]
GeneralizedForce[ChnLst_List,TerminalNode_Integer,BodyLst_,X_,H_,q_,p_,Force_,VelNames_]:=
             Module[{ChnBodyLst,ChnX,ChnH,Chnq,ChnV,NodalVelocity,
             QTemp,pp,VelCoef,Q},(
             {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
                DefineChainData[ChnLst,TerminalNode,BodyLst,X,H,q,p];
             NodalVelocity=NodeVelocity[ChnLst,ChnBodyLst,ChnX,ChnH,ChnV];
             Off[Simplify::time];
             QTemp=Simplify[Force/. Inner[Rule,Flatten[VelNames],
                                Flatten[NodalVelocity],List],Trig->True,TimeConstraint->10];
             On[Simplify::time];           
             pp=SystemQuasiVelocities[BodyLst,p];
             VelCoef=Jacob[NodalVelocity,pp];
             Q=QTemp.VelCoef
             )]
SetAttributes[GeneralizedForce,ReadProtected];
SetAttributes[GeneralizedForce,Protected];
SetAttributes[GeneralizedForce,Locked];

Clear[GravPotential];
GravPotential[BodyLst_List,TreeLst_List,X_List]:= Module[{K,J,N,Temp1,
             Temp2,ChnBodyLst,ChnX,ChnLst,JointNo,oc,
             XNext,XPartial,Xcm,GravPot,TreeGravPot,GrPE},(
             N=Length[TreeLst];TreeGravPot={};
             Do[(
               (* Define chain *)
               ChnLst=TreeLst[[j]];ChnLength=Length[ChnLst];
               (* Compose ChnBodyLst, ChnX *)
                ChnBodyLst={}; ChnX={};
                Do[(
                Body=BodyLst[[ChnLst[[k]][[2]]]];
                If[k<ChnLength,
                   JointNo=ChnLst[[k+1]][[1]];
                   {oc}=Cases[Body[[2]],{JointNo,xx_}->xx],
                   oc=Body[[2]][[1]][[2]]];
                Body[[2]]=oc;
                ChnBodyLst=Append[ChnBodyLst,Body];
                ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
              ),{k,ChnLength}];
               (* Form grav PE for each body in chain *)
               K=Length[ChnBodyLst];J=Length[ChnX];If[K==J,J=K,Print["incompatible data"]];
               XPartial=IdentityMatrix[4];Temp1=Transpose[Join[IdentityMatrix[3],{{0,0,0}}]];
               GravPot={};SubChnBdyLst={};SubChnLst={};
               SubChnX={};
               Do[(
                 BodyData=ChnBodyLst[[i]];mass=BodyData[[3]];
                 BodyType=Length[BodyData];
                 If[BodyType==4,
                   BodyData[[2]]=BodyData[[1]],
                   BodyData[[2]]=Join[Table[0,{3},
                                   {Length[BodyData[[1]][[1]]]}],BodyData[[1]]]];
                 SubChnLst=Append[SubChnLst,ChnLst[[i]]];
                 SubChnX=Append[SubChnX,ChnX[[i]]];
                 TempBdyLst=Append[SubChnBdyLst,BodyData];
                 Xcm=EndEffector[TempBdyLst,SubChnX];
                 SubChnBdyLst=Append[SubChnBdyLst,ChnBodyLst[[i]]];
                 GravPot=Append[GravPot,{ChnLst[[i,2]],Xcm[[3,4]]*mass}];
               ),{i,K}];
             Temp2=Union[TreeGravPot,GravPot];TreeGravPot=Temp2;
             ),{j,N}];
             N=Length[TreeGravPot];
             GrPE=Simplify[Sum[TreeGravPot[[i,2]],{i,N}]];
             GrPE
             )]
SetAttributes[GravPotential,ReadProtected];
SetAttributes[GravPotential,Protected];
SetAttributes[GravPotential,Locked];

Clear[SpringPotential];
SpringPotential[Node1_Integer,Node2_Integer,Pot_,TreeList_,BodyList_,X_,q_]:=
     Module[{Chain1,Chain2},
     Chain1=TerminalNodeChain[Node1,TreeList,BodyList];
     Chain2=TerminalNodeChain[Node2,TreeList,BodyList];
     SpringPotential[Chain1,Node1,Chain2,Node2,Pot,BodyList,X,q]
     ];
SpringPotential[Chain1_List,Node1_Integer,Chain2_List,Node2_Integer,Pot_,BodyList_,X_,q_]:=
     Module[{CommonElements,LastCommonElement,SubChain1,SubChain2,
     Position1,Position2,ShortChain},
     CommonElements=Intersection[Chain1,Chain2];
     LastCommonElement=Last[CommonElements];
     ShortChain=Complement[Chain1,CommonElements];
     If[ShortChain!={},
       SubChain1=Join[{LastCommonElement},ShortChain],
       SubChain1={LastCommonElement}
     ];
     ShortChain=Complement[Chain2,CommonElements];
     If[ShortChain!={},
       SubChain2=Join[{LastCommonElement},ShortChain],
       SubChain2={LastCommonElement}
     ];
     Position1=EndEffector[SubChain1,Node1,BodyList,X][[{1,2,3},4]];
     Position2=EndEffector[SubChain2,Node2,BodyList,X][[{1,2,3},4]];
     Chop[Pot[(Abs[Expand[Simplify[(Position1-Position2).(Position1-Position2)]]])^(1/2)]]
     ];
SetAttributes[SpringPotential,ReadProtected];
SetAttributes[SpringPotential,Protected];
SetAttributes[SpringPotential,Locked];

Clear[SpringForce];
SpringForce[Node1_Integer,Node2_Integer,Pot_,TreeList_,BodyList_,X_,q_]:=
     Module[{Chain1,Chain2},
     Chain1=TerminalNodeChain[Node1,TreeList,BodyList];
     Chain2=TerminalNodeChain[Node2,TreeList,BodyList];
     SpringForce[Chain1,Node1,Chain2,Node2,Pot,BodyList,X,q]
     ];
SpringForce[Chain1_List,Node1_Integer,Chain2_List,Node2_Integer,Pot_,BodyList_,X_,q_]:=
     Module[{CommonElements,LastCommonElement,SubChain1,SubChain2,
     Position1,Position2,ShortChain,SpringLength,xx,qq},
     CommonElements=Intersection[Chain1,Chain2];
     LastCommonElement=Last[CommonElements];
     ShortChain=Complement[Chain1,CommonElements];
     If[ShortChain!={},
       SubChain1=Join[{LastCommonElement},ShortChain],
       SubChain1={LastCommonElement}
     ];
     ShortChain=Complement[Chain2,CommonElements];
     If[ShortChain!={},
       SubChain2=Join[{LastCommonElement},ShortChain],
       SubChain2={LastCommonElement}
     ];
     Position1=EndEffector[SubChain1,Node1,BodyList,X][[{1,2,3},4]];
     Position2=EndEffector[SubChain2,Node2,BodyList,X][[{1,2,3},4]];
     SpringLength=Chop[Abs[Expand[Simplify[(Position1-Position2).(Position1-Position2)]]]]^(1/2);
     qq=SystemCoordinates[BodyList,q];
     Chop[Simplify[(-SimplifyDiracDelta1[D[Pot[xx],xx]]/.xx->SpringLength)*Grad[SpringLength,qq]]]
     ];
SetAttributes[SpringForce,ReadProtected];
SetAttributes[SpringForce,Protected];
SetAttributes[SpringForce,Locked];

Clear[DamperForce];
DamperForce[Node1_Integer,Node2_Integer,DisPot_,TreeList_,BodyList_,X_,H_,q_,p_]:=
     Module[{Chain1,Chain2},
     Chain1=TerminalNodeChain[Node1,TreeList,BodyList];
     Chain2=TerminalNodeChain[Node2,TreeList,BodyList];
     DamperForce[Chain1,Node1,Chain2,Node2,DisPot,BodyList,X,H,q,p]
     ];
DamperForce[Chain1_List,Node1_Integer,Chain2_List,Node2_Integer,DisPot_,BodyList_,X_,H_,q_,p_]:=
     Module[{CommonElements,LastCommonElement,SubChain1,SubChain2,
     ShortChain,Position1,Position2,ChnBodyLst,ChnX,ChnH,Chnq,ChnV,
     VelocityNode1,VelocityNode2,UnitVector,pp,TempPot,Config1,Config2,
     L1,L2,RelPos,RelVel,DisVel,xx},
     CommonElements=Intersection[Chain1,Chain2];
     LastCommonElement=Last[CommonElements];
     ShortChain=Complement[Chain1,CommonElements];
     If[ShortChain!={},
       SubChain1=Join[{LastCommonElement},ShortChain],
       SubChain1={LastCommonElement}
     ];
     ShortChain=Complement[Chain2,CommonElements];
     If[ShortChain!={},
       SubChain2=Join[{LastCommonElement},ShortChain],
       SubChain2={LastCommonElement}
     ];
     Config1=EndEffector[SubChain1,Node1,BodyList,X];
     Config2=EndEffector[SubChain2,Node2,BodyList,X];
     L1=Transpose[Simplify[Config1[[{1,2,3},{1,2,3}]]]];
     L2=Transpose[Simplify[Config2[[{1,2,3},{1,2,3}]]]];
     Position1=Config1[[{1,2,3},4]];
     Position2=Config2[[{1,2,3},4]];
     {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
            DefineChainData[SubChain1,Node1,BodyList,X,H,q,p];
     VelocityNode1=NodeVelocity[SubChain1,ChnBodyLst,ChnX,ChnH,ChnV][[{4,5,6}]];
     {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
            DefineChainData[SubChain2,Node2,BodyList,X,H,q,p];
     VelocityNode2=NodeVelocity[SubChain2,ChnBodyLst,ChnX,ChnH,ChnV][[{4,5,6}]];
     RelPos=Chop[Simplify[Chop[Expand[L1.(Position1-Position2)]]]];
     RelVel=Chop[Simplify[(VelocityNode1-L1.Transpose[L2].VelocityNode2)]];
     UnitVector=RelPos/Abs[Simplify[RelPos.RelPos]]^(1/2);
     pp=SystemQuasiVelocities[BodyList,p];
     (* TempPot=DisPot[RelVel.UnitVector]; *)
     DisVel=Simplify[RelVel.UnitVector];
     Chop[Simplify[(-SimplifyDiracDelta1[D[DisPot[xx],xx]]/.xx->DisVel)*Grad[DisVel,pp]]]
     (* Simplify[PowerExpand[-SimplifyDiracDelta1[Grad[TempPot,pp]]]] *)
     ];
SetAttributes[DamperForce,ReadProtected];
SetAttributes[DamperForce,Protected];
SetAttributes[DamperForce,Locked];

Clear[DamperPotential];
DamperPotential[Node1_Integer,Node2_Integer,DisPot_,TreeList_,BodyList_,X_,H_,q_,p_]:=
     Module[{Chain1,Chain2},
     Chain1=TerminalNodeChain[Node1,TreeList,BodyList];
     Chain2=TerminalNodeChain[Node2,TreeList,BodyList];
     DamperPotential[Chain1,Node1,Chain2,Node2,DisPot,BodyList,X,H,q,p]
     ];
DamperPotential[Chain1_List,Node1_Integer,Chain2_List,Node2_Integer,DisPot_,BodyList_,X_,H_,q_,p_]:=
     Module[{CommonElements,LastCommonElement,SubChain1,SubChain2,
     ShortChain,Position1,Position2,ChnBodyLst,ChnX,ChnH,Chnq,ChnV,
     VelocityNode1,VelocityNode2,UnitVector,pp,TempPot,Config1,Config2,
     L1,L2,RelPos,RelVel,DisVel,xx},
     CommonElements=Intersection[Chain1,Chain2];
     LastCommonElement=Last[CommonElements];
     ShortChain=Complement[Chain1,CommonElements];
     If[ShortChain!={},
       SubChain1=Join[{LastCommonElement},ShortChain],
       SubChain1={LastCommonElement}
     ];
     ShortChain=Complement[Chain2,CommonElements];
     If[ShortChain!={},
       SubChain2=Join[{LastCommonElement},ShortChain],
       SubChain2={LastCommonElement}
     ];
     Config1=EndEffector[SubChain1,Node1,BodyList,X];
     Config2=EndEffector[SubChain2,Node2,BodyList,X];
     L1=Transpose[Simplify[Config1[[{1,2,3},{1,2,3}]]]];
     L2=Transpose[Simplify[Config2[[{1,2,3},{1,2,3}]]]];
     Position1=Config1[[{1,2,3},4]];
     Position2=Config2[[{1,2,3},4]];
     {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
            DefineChainData[SubChain1,Node1,BodyList,X,H,q,p];
     VelocityNode1=NodeVelocity[SubChain1,ChnBodyLst,ChnX,ChnH,ChnV][[{4,5,6}]];
     {ChnBodyLst,ChnX,ChnH,Chnq,ChnV} =
            DefineChainData[SubChain2,Node2,BodyList,X,H,q,p];
     VelocityNode2=NodeVelocity[SubChain2,ChnBodyLst,ChnX,ChnH,ChnV][[{4,5,6}]];
     RelPos=Chop[Simplify[Chop[Expand[L1.(Position1-Position2)]]]];
     RelVel=Chop[Simplify[(VelocityNode1-L1.Transpose[L2].VelocityNode2)]];
     UnitVector=RelPos/Abs[Simplify[RelPos.RelPos]]^(1/2);
     pp=SystemQuasiVelocities[BodyList,p];
     (* TempPot=DisPot[RelVel.UnitVector]; *)
     DisVel=Simplify[RelVel.UnitVector];
     Chop[Simplify[DisPot[xx]/.xx->DisVel]]
     ];
SetAttributes[DamperPotential,ReadProtected];
SetAttributes[DamperPotential,Protected];
SetAttributes[DamperPotential,Locked];

Clear[LeafPotential];
LeafPotential[BodyLst_,TreeLst_,X_,H_,Pot_,NameLst_]:= Module[{K,J,N,Temp1,
             ChnBodyLst,ChnX,ChnLst,Chnq,Body,oc,TransPot=Pot,
             CoordLst},(
             N=Length[TreeLst];CoordLst={};
             Do[(
               (* Define chain *)
               ChnLst=TreeLst[[j]];ChnLength=Length[ChnLst];
               (* Compose ChnBodyLst, ChnX, Chnq *)
                ChnBodyLst={}; ChnX={}; Chnq={};
                Do[(
                Body=BodyLst[[ChnLst[[k]][[2]]]];
                If[k<ChnLength,
                   JointNo=ChnLst[[k+1]][[1]];
                   {oc}=Cases[Body[[2]],{JointNo,xx_}->xx],
                   oc=Body[[2]][[1]][[2]]];
                Body[[2]]=oc;
                ChnBodyLst=Append[ChnBodyLst,Body];
                ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
                ),{k,ChnLength}];
               (* Form EndEffector for each chain *)
               Temp1=EndEffector[ChnBodyLst,ChnX];
               (*
               CoordLst=Append[CoordLst,Temp1[[{1,2,3},4]]];
               *)
               TransPot=TransPot/. Inner[Rule,Flatten[NameLst[[j]]],
                                Flatten[Temp1[[{1,2,3},4]]],List];
             ),{j,N}];
             (* Create and apply a list of transformation rules *)
             (*
              TransPot=Simplify[Pot/. Inner[Rule,Flatten[NameLst],
                                Flatten[CoordLst],List]];
                                *)
             MemoryConstrained[Simplify[TransPot],5000000,TransPot]
             )];
SetAttributes[LeafPotential,ReadProtected];
SetAttributes[LeafPotential,Protected];
SetAttributes[LeafPotential,Locked];

Clear[FlxStrnPot];
FlxStrnPot[BodyLst_]:=Module[{NoBodies,StrainPot,BodyType,
             BodyStiffness,BodyStrainPot,BodyCoord},(
             NoBodies=Length[BodyLst];
             StrainPot=0;
             Do[(
               Body=BodyLst[[i]];BodyType=Length[Body];
               If[BodyType==4,BodyStrainPot=0,
                 BodyStiffness=Body[[4]][[3]];
                 SixZeros={{0},{0},{0},{0},{0},{0}};
                 BodyCoord=Flatten[Join[SixZeros,Transpose[{Body[[5]]}]]];
                 BodyStrainPot=(BodyCoord.BodyStiffness).BodyCoord/2];
               StrainPot=StrainPot+BodyStrainPot;
             ),{i,NoBodies}];
             Simplify[StrainPot]
             )];
SetAttributes[FlxStrnPot,ReadProtected];
SetAttributes[FlxStrnPot,Protected];
SetAttributes[FlxStrnPot,Locked];

Clear[FlxDissPot];
FlxDissPot[BodyLst_List]:=Module[{NoBodies,Body,BodyType,
             BodyDamping,SixZeros,BodyVel,BodyDissPot,DissPot},(
             NoBodies=Length[BodyLst];DissPot=0;
             Do[(
               Body=BodyLst[[i]];BodyType=Length[Body];
               If[BodyType==4,BodyDissPot=0,
                 BodyDamping=Body[[4]][[2]];
                 SixZeros={{0},{0},{0},{0},{0},{0}};
                 BodyVel=Flatten[Join[SixZeros,Transpose[{Body[[6]]}]]];
                 BodyDissPot=(BodyVel.BodyDamping).BodyVel/2];
               DissPot=DissPot+BodyDissPot;
             ),{i,NoBodies}];
             Simplify[DissPot]
             )];
SetAttributes[FlxDissPot,ReadProtected];
SetAttributes[FlxDissPot,Protected];
SetAttributes[FlxDissPot,Locked];

Clear[RgdBdyInrShift];
RgdBdyInrShift[Inertiacm_,cm_,mass_]:= Module[{InertShift,Inertia},(
             InertShift=mass*{{cm[[2]]^2+cm[[3]]^2,-cm[[1]]*cm[[2]],-cm[[1]]*cm[[3]]},
                              {-cm[[1]]*cm[[2]],cm[[1]]^2+cm[[3]]^2,-cm[[2]]*cm[[3]]},
                              {-cm[[1]]*cm[[3]],-cm[[2]]*cm[[3]],cm[[2]]^2+cm[[1]]^2}};
             Inertia=Inertiacm+InertShift
             )];
SetAttributes[RgdBdyInrShift,ReadProtected];
SetAttributes[RgdBdyInrShift,Protected];
SetAttributes[RgdBdyInrShift,Locked];


(* **** Dynamics **** *)

Clear[PoincareCoefficients];
PoincareCoefficients[V_List,q_,p_]:= Module[{i,j,k,m,n,index,K,UX,Vtemp,
       SumpX,qq,vv,bb,cc,Xtemp,XX},(
       (* construct sum(pX) *)
             K=Length[V];UX={};index=0;
             Do[({m,n}=Dimensions[V[[i]]];
                Vtemp=V[[i]];SumpX=Table[0,{m},{n}];
                (*
                VInv=Simplify[Inverse[Vtemp]];
                *)
                Do[(qq=q[[Range[index+1,index+m]]];
                vv=Transpose[Vtemp][[j]];
                   Xtemp={};
                   Do[(bb=Transpose[Vtemp][[k]];
                     cc=LieBracket[vv,bb,qq];
                     Xtemp=Join[Xtemp,{cc}];
                   ),
                   {k,m}];
                (*
                XX=Simplify[VInv.Transpose[Xtemp]];
                *)
                XX=Simplify[LinearSolve[Vtemp,Transpose[Xtemp],ZeroTest->(Simplify[#]==0 &)]];
                   
                SumpX=SumpX+p[[index+j]]*XX;
                ),
                {j,m}];
             index=index+m;
             UX=DiagJoin[UX,SumpX];
             ),
             {i,K}];
             Chop[UX]
             )];
SetAttributes[PoincareCoefficients,ReadProtected];
SetAttributes[PoincareCoefficients,Protected];
SetAttributes[PoincareCoefficients,Locked];

Clear[CMatrix];
CMatrix[M_,V_List,q_,p_]:= Module[{Vmat,Temp1,Temp2,Temp3,Cpq},(
             Vmat=DiagJoinLst[V];
             Temp1=Jacob[M.p,q].Vmat;
             Temp2=-Temp1+(1/2)*Transpose[Temp1];
             Temp3=Temp2+Transpose[PoincareCoefficients[V,q,p]].M;
             Cpq=Chop[-Temp3];
             Cpq
             )];
SetAttributes[CMatrix,ReadProtected];
SetAttributes[CMatrix,Protected];
SetAttributes[CMatrix,Locked];

Clear[PoincareFunctionCombined];
PoincareFunctionCombined[M_?MatrixQ,V_List,PE_,Q_?VectorQ,p_,q_]:= Module[
             {F},(
             F=CMatrix[M,V,q,p].p+
                  Simplify[ComputeTransVp[V,SimplifyDiracDelta1[Grad[PE,q]]]]-Q;
             Chop[F]
             )];
SetAttributes[PoincareFunctionCombined,ReadProtected];
SetAttributes[PoincareFunctionCombined,Protected];
SetAttributes[PoincareFunctionCombined,Locked];

Clear[PoincareFunction];
PoincareFunction[M_?MatrixQ,V_List,PE_,Q_?VectorQ,p_,q_]:= Module[
             {F,Cpq},(
             F=Chop[ComputeTransVp[V,SimplifyDiracDelta1[Grad[PE,q]]]-Q];
             Cpq=CMatrix[M,V,q,p];
             {Cpq,F}
             )];
SetAttributes[PoincareFunction,ReadProtected];
SetAttributes[PoincareFunction,Protected];
SetAttributes[PoincareFunction,Locked];

Clear[CreateModelSpecial];
CreateModelSpecial[JointLst_,BodyLst_,TreeLst_,g_,PE_,Q_]:=CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,0,Q]
CreateModelSpecial[JointLst_,BodyLst_,TreeLst_,g_,PE_,DissPot_,Q_]:= Module[
             {V,X,H,M,Vtot,pp,qq,F},(
             Print["Computing Joint Kinematics"];
             {V,X,H}=Joints[JointLst];
             {Vtot,X,H,M,F,pp,qq}=CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q,V,X,H];
             {Vtot,X,H,M,F,pp,qq}
             )];
CreateModelSpecial[JointLst_,BodyLst_,TreeLst_,g_,PE_,Q_,V_,X_,H_]:=CreateModelSpecial[JointLst,BodyLst,TreeLst,g,PE,0,Q,V,X,H]
CreateModelSpecial[JointLst_,BodyLst_,TreeLst_,g_,PE_,DissPot_,Q_,V_,X_,H_]:= Module[
             {NoJoints,NoBodies,q,p,q0,p0,M,Grv,FlxPE,FlxDP,
             BodyType,NoFlxVel,Vtot,PEtot,Qtot,pp,qq,F,Vmat},(
             (* Define Joint coordinate vector and quasi-velocity vector *)
             NoJoints=Length[JointLst];q={};p={};
             Do[(q0=JointLst[[k]][[3]];q=Join[q,q0];
                 p0=JointLst[[k]][[4]];p=Join[p,p0]
             ),{k,NoJoints}];
             Print["Computing Potential Functions"];
             If[g==0,Grv=0,
                     Grv=g*GravPotential[BodyLst,TreeLst,X],
                     Grv=g*GravPotential[BodyLst,TreeLst,X]];
             FlxPE=FlxStrnPot[BodyLst];
             FlxDP=FlxDissPot[BodyLst];
             (* Define system coordinate vector and velocities *)
             qq=q;pp=p;NoBodies=Length[BodyLst];
             Do[(
             BodyType=Length[BodyLst[[i]]];
             If[BodyType==4,
               pp=pp;qq=qq,
               pp=Join[pp,BodyLst[[i]][[6]]];
               qq=Join[qq,BodyLst[[i]][[5]]];
             ];
             ),{i,NoBodies}];
             PEtot=PE+Grv+FlxPE;
             Qtot=Q-Grad[FlxDP,pp]-SimplifyDiracDelta1[Grad[DissPot,pp]];
             NoFlxVel=Length[pp]-Length[p];
             If[NoFlxVel==0,Vtot=V,Vtot=Append[V,IdentityMatrix[NoFlxVel]]];
             Print["Computing Inertia Matrix"];
             M=TreeInertia[BodyLst,TreeLst,X,H,q,p];
             Print["Computing Poincare's Function"];
             F=PoincareFunctionCombined[M,Vtot,PEtot,Qtot,pp,qq];
             Vmat=DiagJoinLst[Vtot];
             {Vmat,X,H,M,F,pp,qq}
             )];
SetAttributes[CreateModelSpecial,ReadProtected];
SetAttributes[CreateModelSpecial,Protected];
SetAttributes[CreateModelSpecial,Locked];

Clear[CreateModel];
CreateModel[JointLst_,BodyLst_,TreeLst_,g_,PE_,Q_]:=CreateModel[JointLst,BodyLst,TreeLst,g,PE,0,Q]
CreateModel[JointLst_,BodyLst_,TreeLst_,g_,PE_,DissPot_,Q_]:= Module[
             {V,X,H,M,Vtot,pp,qq,F,Cmat},(
             Print["Computing Joint Kinematics"];
             {V,X,H}=Joints[JointLst];
             {Vtot,X,H,M,Cmat,F,pp,qq}=CreateModel[JointLst,BodyLst,TreeLst,g,PE,DissPot,Q,V,X,H];
             {Vtot,X,H,M,Cmat,F,pp,qq}
             )];
CreateModel[JointLst_,BodyLst_,TreeLst_,g_,PE_,Q_,V_,X_,H_]:=CreateModel[JointLst,BodyLst,TreeLst,g,PE,0,Q,V,X,H]             
CreateModel[JointLst_,BodyLst_,TreeLst_,g_,PE_,DissPot_,Q_,V_,X_,H_]:= Module[
             {NoJoints,NoBodies,q,p,q0,p0,M,Grv,FlxPE,FlxDP,
             BodyType,NoFlxVel,Vtot,PEtot,Qtot,pp,qq,F,Vmat,Cmat},(
             (* Define Joint coordinate vector and quasi-velocity vector *)
             NoJoints=Length[JointLst];q={};p={};
             Do[(q0=JointLst[[k]][[3]];q=Join[q,q0];
                 p0=JointLst[[k]][[4]];p=Join[p,p0]
             ),{k,NoJoints}];
             Print["Computing Potential Functions"];
             If[g==0,Grv=0,
                     Grv=g*GravPotential[BodyLst,TreeLst,X],
                     Grv=g*GravPotential[BodyLst,TreeLst,X]];
             FlxPE=FlxStrnPot[BodyLst];
             FlxDP=FlxDissPot[BodyLst];
             (* Define system coordinate vector and velocities *)
             qq=q;pp=p;NoBodies=Length[BodyLst];
             Do[(
             BodyType=Length[BodyLst[[i]]];
             If[BodyType==4,
               pp=pp;qq=qq,
               pp=Join[pp,BodyLst[[i]][[6]]];
               qq=Join[qq,BodyLst[[i]][[5]]];
             ];
             ),{i,NoBodies}];
             PEtot=PE+Grv+FlxPE;
             Qtot=Q-Grad[FlxDP,pp]-SimplifyDiracDelta1[Grad[DissPot,pp]];
             NoFlxVel=Length[pp]-Length[p];
             If[NoFlxVel==0,Vtot=V,Vtot=Append[V,IdentityMatrix[NoFlxVel]]];
             Print["Computing Inertia Matrix"];
             M=TreeInertia[BodyLst,TreeLst,X,H,q,p];
             Print["Computing Poincare Function"];
             {Cmat,F}=PoincareFunction[M,Vtot,PEtot,Qtot,pp,qq];
             {Vtot,X,H,M,Cmat,F,pp,qq}
             )];
SetAttributes[CreateModel,ReadProtected];
SetAttributes[CreateModel,Protected];
SetAttributes[CreateModel,Locked];

(* ******* Constraints ******* *)

Clear[DifferentialConstraints];
DifferentialConstraints[M_,Cmat_,F_,V_,G_,p_,q_]:=
    DifferentialConstraints[M,Cmat,F,V,G,p,q,{},{}];
DifferentialConstraints[M_,Cmat_,F_,V_,G_,p_,q_,Index_]:=
    DifferentialConstraints[M,Cmat,F,V,G,p,q,Index,{}];
DifferentialConstraints[M_,Cmat_,F_,V_,G_,p_,q_,Index_,Index2_]:=
    Module[{A,ker,n,m,AA,Mm,Cm,Fm,Vm,phat,A0,ker0,ker1,
            prule,T,IndexList,K,Temp1},
    (
    (* T(q) is a matrix whose columns span ker[A(q)]
            Mm=T'MT,Cm=T'CT+T'MJacob[Tp,q]VT,Fm=T'F,Vm=VT
    *)
         n=Length[M];
         A=Jacob[G,p];
         If[Index2!={},
                A0=A/.Inner[Rule,q[[Index2]],Table[0,{i,Length[Index2]}],List];
                ker0=Transpose[SpecialNullSpace[A0,ZeroTest->(Simplify[#]==0 &)]];
                ker1=LinearSolve[A0,(A0-A).ker0,ZeroTest->(Simplify[#]==0 &)];
                (*
                ker1=Transpose[RowReduce[Transpose[ker1],ZeroTest->(Simplify[#]==0 &)]];
                *)
                ker=ker0+ker1,
                ker=Transpose[SpecialNullSpace[A,ZeroTest->(Simplify[#]==0 &)]];
             ];
         m=Length[Transpose[ker]];
         T=ker;
         IndexList=Complement[Table[i,{i,Length[p]}],Index];
         K=T[[IndexList]];
         If[(Length[K]==m) && (Length[NullSpace[K,ZeroTest->(Simplify[#]==0 &)]]==0),
             T=Transpose[LinearSolve[Transpose[K],Transpose[T],ZeroTest->(Simplify[#]==0 &)]]];
             phat=Table[StringJoin["w",ToString[i]],{i,m}];
         AA=IdentityMatrix[m];
         Do[
           Do[
           If[T[[i]]==AA[[k]],phat[[k]]=p[[i]];Break[]];
           ,{i,n}];
         ,{k,m}];
         (*
         If[T[[ Range[n-m+1,n],Range[1,m] ]]==IdentityMatrix[m],
            phat=p[[ Range[n-m+1,n] ]],
            phat=Table[ToExpression[StringJoin["w",ToString[i]]],{i,m}]
         ];
         *)
         prule=Inner[Rule,p,T.phat,List];
         Vm=DiagJoinLst[V].T;Print["Vm"];
         Mm=Transpose[T].M.T;Print["Mm"];
         Temp1=Transpose[T].M.Jacob[T.phat,q].Vm;
         Cm=(Transpose[T].Cmat.T+Temp1)/.prule;Print["Cm"];
         Fm=(Transpose[T].F)/.prule;Print["Fm"];
         Return[{Mm,Cm,Fm,Vm,T,phat}]
    )];
SetAttributes[DifferentialConstraints,ReadProtected];
SetAttributes[DifferentialConstraints,Protected];
SetAttributes[DifferentialConstraints,Locked];

Clear[AlgebraicConstraints];
AlgebraicConstraints[M_,Cmat_,F_,V_,G_,p_,q_]:=
    AlgebraicConstraints[M,Cmat,F,V,G,p,q,{},{},{}];
AlgebraicConstraints[M_,Cmat_,F_,V_,G_,p_,q_,Index_]:=
    AlgebraicConstraints[M,Cmat,F,V,G,p,q,Index,Index,{}];
AlgebraicConstraints[M_,Cmat_,F_,V_,G_,p_,q_,Index_,Index2_]=
    AlgebraicConstraints[M,Cmat,F,V,G,p,q,Index,Index2,{}];
AlgebraicConstraints[M_,Cmat_,F_,V_,G_,p_,q_,Index_,Index2_,Index3_]:=
    Module[{GG,Mm,Cm,Fm,Vm,T,phat,Rule1,IndexList},
    (
      GG=Jacob[G,q].ComputeVp[V,p];
      {Mm,Cm,Fm,Vm,T,phat}=DifferentialConstraints[M,Cmat,F,V,GG,p,q,Index2,Index3];
      IndexList=Complement[Table[i,{i,Length[q]}],Index];
      Rule1={};
      If[Index!={},Print["Eliminating specified coordinates."];
         Rule1=Simplify[Flatten[Solve[G==0,q[[Index]]]]]];
      Off[Simplify::time];
      Off[Set::shape];
      {Mm,Cm,Fm,Vm}=TimeConstrained[Simplify[{Mm,Cm,Fm,Vm}/.Rule1,TimeConstraint->10],300];
      On[Simplify::time];
      On[Set::shape];
      {Mm,Cm,Fm,Vm[[IndexList]],phat,q[[IndexList]]}
    )];
SetAttributes[AlgebraicConstraints,ReadProtected];
SetAttributes[AlgebraicConstraints,Protected];
SetAttributes[AlgebraicConstraints,Locked];

Clear[StateTransformation];
StateTransformation[DiffEqns_,StateVars_,TransRules_,NewVars_,t_Symbol]:=
   Module[{OldVars,OldDerivs,DerRules,DerRules2,
   NewDerivs,Eqns1,StateDerivs,NewStateDers,Sols,Rule2Eqn},
     If[Length[TransRules]!=Length[NewVars],
        Print["Incompatible transformation rules and new variable definitions"];
        Return[]
       ];
     OldVars=Table[TransRules[[i]][[1]],{i,Length[TransRules]}];
     OldDerivs=
      Table[D[OldVars[[i]],t],{i,Length[OldVars]}];
     DerRules2=Table[D[OldVars[[i]]/.TransRules,t],{i,Length[OldVars]}];
     DerRules=Inner[Rule,OldDerivs,DerRules2,List];
     Eqns1=Simplify[Flatten[(DiffEqns/.TransRules)/.DerRules]];
     NewDerivs=Table[ToExpression[StringJoin[ToString[NewVars[[i]]],
                "'["<>ToString[t]<>"]"]],{i,Length[NewVars]}];
     StateDerivs=Table[ToExpression[StringJoin[ToString[StateVars[[i]]],
                "'["<>ToString[t]<>"]"]],{i,Length[StateVars]}];
     NewStateDers=StateDerivs/.Inner[Rule,OldDerivs,NewDerivs,List];
     Sols=Simplify[Solve[Eqns1,NewStateDers]];
     Rule2Eqn[x_]:=x[[1]]==x[[2]];
     Map[Rule2Eqn,Flatten[Sols]]
     ];

StateTransformation[A_?MatrixQ,RHS_?VectorQ,StateVars_,TransRules_,NewVars_]:=
Module[{FullTransExpr,NewStateVars, NewA,NewRHS,OldVars},
     If[Length[TransRules]!=Length[NewVars],
        Print[
        "Incompatible transformation rules and new variable definitions"];
        Return[]
       ];
                OldVars=Table[TransRules[[i]][[1]],{i,Length[TransRules]}];
                FullTransExpr=StateVars/.TransRules;
                NewStateVars=StateVars/.Inner[Rule,OldVars,NewVars,List];
                TransJac=Jacob[FullTransExpr,NewStateVars];
                NewA=(A.TransJac)/.TransRules;
                NewRHS=RHS/.TransRules;
                Simplify[{NewA,NewRHS,NewStateVars}]
    ];
StateTransformation[RHS_?VectorQ,StateVars_,TransRules_,NewVars_]:=
   Module[{FullTransExpr,NewStateVars, NewA,NewRHS,OldVars},
     If[Length[TransRules]!=Length[NewVars],
        Print[
        "Incompatible transformation rules and new variable definitions"];
        Return[]
       ];
                OldVars=Table[TransRules[[i]][[1]],{i,Length[TransRules]}];
                FullTransExpr=StateVars/.TransRules;
                NewStateVars=StateVars/.Inner[Rule,OldVars,NewVars,List];
                TransJac=Jacob[FullTransExpr,NewStateVars];
                NewRHS=MyInverse[TransJac].RHS/.TransRules;
                Simplify[{NewRHS,NewStateVars}]
    ];
SetAttributes[StateTransformation,ReadProtected];
SetAttributes[StateTransformation,Protected];
SetAttributes[StateTransformation,Locked];


(* **** Code Constructions **** *)

Clear[MakeODEs];
MakeODEs[p_,q_,V_,M_,Cmat_,F_,t_]:=Module[{qq,pp,VV,MM,CC,FF,
Kinematics,Dynamics,tt},
pp=Table[ToExpression[StringJoin[ToString[p[[i]]],"["<>ToString[t]<>"]"]],{i,Length[p]}];
qq=Table[ToExpression[StringJoin[ToString[q[[i]]],"["<>ToString[t]<>"]"]],{i,Length[q]}];
qrule=Inner[Rule,q,qq,List];
prule=Inner[Rule,p,pp,List];
VV=V/.qrule;MM=M/.qrule;
CC=Cmat/.qrule/.prule;FF=F/.qrule/.prule;
Kinematics=Inner[Equal,D[qq,t],ComputeVp[VV,pp],List];
Dynamics=Inner[Equal,MM.D[pp,t]+CC.pp+FF,Table[0,{i,Length[p]}],List];
Join[Kinematics,Dynamics]
];
MakeODEs[q_,F_,t_]:=Module[{qq,FF,Dynamics},
qq=Table[ToExpression[StringJoin[ToString[q[[i]]],"["<>ToString[t]<>"]"]],{i,
          Length[q]}];
qrule=Inner[Rule,q,qq,List];
FF=F/.qrule;
Dynamics=Inner[Equal,D[qq,t]-FF,Table[0,{i,Length[q]}],List];
Dynamics
];
SetAttributes[MakeODEs,ReadProtected];
SetAttributes[MakeODEs,Protected];
SetAttributes[MakeODEs,Locked];

Clear[MakeLagrangeEquations];
MakeLagrangeEquations[T_,U_,R_,Q_List,t_,q_List,p_List]:=
  Module[{vars,vels,QRules,PRules,L,RR},
		If[(Length[Q]=!=Length[q])||(Length[q]=!=Length[p]),
      Print["Incompatible dimensions."];Return[{}]];
		vars=ToExpression[Map[ToString[#]<>"[t]"&,q]];
		vels=D[vars,t];
		QRules=Inner[Rule,q,vars,List];
		PRules=Inner[Rule,p,vels,List];L=((T-U)/.QRules)/.PRules;
		RR=(R/.QRules)/.PRules;
		Inner[Equal,D[Jacob[L,vels],t]-Jacob[L,vars]+Jacob[RR,vels]-Q,0 vars,List]
		]
SetAttributes[MakeLagrangeEquations,ReadProtected];
SetAttributes[MakeLagrangeEquations,Protected];
SetAttributes[MakeLagrangeEquations,Locked];

Clear[FunctionReplace];
FunctionReplace[EquationList_]:= Module[{FCombined,FExp,FExpU,FExpLength,
                                     FunctionHeads,FunctionTerms ,
                                     NumberOfFunctionTerms,Replacements,
                                     ArcTanTerms,NumberOfArcTanTerms,ArcTanTermsNumberOfArgs,
                                     ArcTanRules,i,NewRule,
                                     FNew},(

       FCombined = Apply[Plus,Flatten[EquationList]];
       FExp = Level[FCombined,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       FunctionHeads = {_Sin,_ArcSin,_Cos,_ArcCos,_Tan,_ArcTan,_Global`ArcTan2,
                           _Csc,_Sec,_Cot,_Power,_Exp,_Log,_Sinh,_Cosh,
                           _Tanh,_Csch,_Sech,_Coth,_ArcSinh,_ArcCosh,_ArcTanh};
       (* First examine ArcTan functions and deteremine whether any are called with two arguments *)
       ArcTanTerms = Cases[FExpU,_ArcTan];
       NumberOfArcTanTerms = Length[ArcTanTerms];
       ArcTanTermsNumberOfArgs=Flatten[Map[Dimensions,Map[Level[#,{1}]&,ArcTanTerms]]];
       ArcTanRules = {};
       For[i=1,i<NumberOfArcTanTerms+1,i++,
          If[ArcTanTermsNumberOfArgs[[i]]==2, 
             NewRule = Rule[ArcTanTerms[[i]],ArcTanTerms[[i]]/.ArcTan->Global`ArcTan2];
             ArcTanRules = Join[ArcTanRules,{NewRule}];
          ];
       ];

       FNew = EquationList/.ArcTanRules;
       FExpU = FExpU/.ArcTanRules;
       FunctionTerms = Flatten[Map[Cases[FExpU,FunctionHeads[[#]]]&,
                                   Range[1,Length[FunctionHeads]]]];
       FunctionTerms=Sort[FunctionTerms,(Depth[#2]>Depth[#1])&];
       NumberOfFunctionTerms = Length[FunctionTerms];

       Replacements = Map[Rule[FunctionTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfFunctionTerms]];

       FNew  = FNew/. Replacements;
(* Apply replacement rules to the rhs of the replacement rules themselves *)
       For[i=2,i<NumberOfFunctionTerms+1,i++,
          FunctionTerms[[i]] = FunctionTerms[[i]]/.Drop[Replacements,{i,NumberOfFunctionTerms}];

];

       Replacements = Map[Rule[FunctionTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfFunctionTerms]];
 

{FNew,Replacements} )];
SetAttributes[FunctionReplace,ReadProtected];
SetAttributes[FunctionReplace,Protected];
SetAttributes[FunctionReplace,Locked];

Clear[ReplaceVariablesStr];
ReplaceVariablesStr[Expr_,InputList_,StateList_,PassedParamsList_]:=
       Module[{Inputs,States,NumberOfInputs,NumberOfStates,InputReplacements,
        StateReplacements,TotalReplacements,Freplace,Str,StrInputRules,
        StrPointerRules,StrStateRules,StrRules,ExprStr,NumberOfPassedParams,
        PassedParams,PassedParamsPr,StrPassedParamsRules},(
        Inputs = Flatten[InputList];
        States = Flatten[StateList];
        PassedParams = Flatten[PassedParamsList];
        NumberOfInputs = Length[Inputs];
        NumberOfStates = Length[States];
        NumberOfPassedParams = Length[PassedParams];
        (*
        InputReplacements = 
          Map[Rule[Inputs[[#]],Global`u[#-1]]&,Range[1,NumberOfInputs]];
          *)
        
        InputReplacements = 
          Map[Rule[Inputs[[#]],ToExpression[StringJoin["u[",
                                     ToString[#-1],
                                    "]"]]]&,Range[1,NumberOfInputs]];
        (*
        StateReplacements = 
          Map[Rule[States[[#]],Global`x[#-1]]&,Range[1,NumberOfStates]];
          *)
        StateReplacements = 
          Map[Rule[States[[#]],ToExpression[StringJoin["x[",
                                    ToString[#-1],
                                    "]"]]]&,Range[1,NumberOfStates]];
        TotalReplacements = Flatten[{InputReplacements,StateReplacements}];
        Freplace = Expr /. TotalReplacements;
        Str = ToString[ CForm[Freplace]];
        StrInputRules=
                Map[Rule[StringJoin["u(",
                                     ToString[#-1],
                                    ")"],
                        StringJoin["u[",
                                     ToString[#-1],
                                    "]"]]&,
                Range[1,NumberOfInputs]];
        StrPointerRules={" * "->"*"," + "->"+"," - "->"-","1.*"->""};
        StrStateRules=
                Map[Rule[StringJoin["x(",
                                    ToString[#-1],
                                    ")"],
                         StringJoin["x[",
                                    ToString[#-1],
                                    "]"]]&,
                Range[1,NumberOfStates]];
       PassedParamsPr = Map[StringJoin[ToString[PassedParams[[#]]],
                                             "pr"]&,
                              Range[1,NumberOfPassedParams]];
       StrPassedParamsRules = Map[Rule[
                              ToString[PassedParams[[#]]],
                              StringJoin[ToString[PassedParamsPr[[#]]],
                                         "[0]"]]&,
                              Range[1,NumberOfPassedParams]];
        StrRules = Join[StrInputRules,StrStateRules];
        ExprStr = StringReplace[Str,StrRules];
        ExprStr = StringReplace[ExprStr,StrPointerRules];
        ExprStr = StringReplace[ExprStr,StrPassedParamsRules];
        ExprStr
)];
SetAttributes[ReplaceVariablesStr,ReadProtected];
SetAttributes[ReplaceVariablesStr,Protected];
SetAttributes[ReplaceVariablesStr,Locked];

Clear[WriteCCode];
WriteCCode[CCodeList_,sfile_]:=
       Module[{NumberOfCCodeLines,n},(
        NumberOfCCodeLines = Length[CCodeList];
        For[n=1, n<NumberOfCCodeLines+1, n++,
           WriteString[sfile,StringJoin[CCodeList[[n]]," \n"]];
        ];
)];
SetAttributes[WriteCCode,ReadProtected];
SetAttributes[WriteCCode,Protected];
SetAttributes[WriteCCode,Locked];

Clear[SinCosPowerReplaceToo];
SinCosPowerReplaceToo[Fp_,Mp_,Cp_]:= Module[{FCombined,FExpU,FExpLength,
                                     SinCosPowerHeads,SinCosPowerTerms ,
                                     NumberOfSinCosPowerTerms,Replacements,
                                     FNew,MNew,CNew},(
       FCombined = Apply[Plus,Flatten[{Fp,Mp,Cp}]];    
       FExp = Level[FCombined,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       SinCosPowerHeads = {_Sin,_Cos,_Power,_Tan,_Cot,_Sec,_Csc};
       SinCosPowerTerms = Flatten[Map[Cases[FExpU,SinCosPowerHeads[[#]]]&,
                                   Range[1,Length[SinCosPowerHeads]]]];
       NumberOfSinCosPowerTerms = Length[SinCosPowerTerms];
       Replacements = Map[Rule[SinCosPowerTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfSinCosPowerTerms]];
       FNew  = Fp/. Replacements;
       MNew = Mp/. Replacements;
       CNew = Cp/. Replacements;
{FNew,MNew,CNew,Replacements} )];
SetAttributes[SinCosPowerReplaceToo,ReadProtected];
SetAttributes[SinCosPowerReplaceToo,Protected];
SetAttributes[SinCosPowerReplaceToo,Locked];

Clear[KinematicReplacements];
KinematicReplacements[Fp_,Mp_,Cp_,q_]:=Module[{Fnew,Mnew,Cnew,
       rules,RuleIndices,rules1,rules2,MyUnion,FindRules},
       {Fnew,Mnew,Cnew,rules}=KinematicReplacements[Fp,Mp,Cp];
       MyUnion[{}]:={};
       MyUnion[L_List]:=Union[L[[1]],MyUnion[L[[Range[2,Length[L]]]]]];
       FindRules[L_,a_]:=Module[{Pos},
         Pos=Position[L,a];
         MyUnion[Pos[[Range[1,Length[Pos]],{1}]]]
       ];
       RuleIndices=MyUnion[Map[FindRules[rules,#]&,q]];
       rules1=rules[[RuleIndices]];
       rules2=Complement[rules,rules1];
       rules1=rules1/.Inner[Rule,Map[#[[2]]&,rules2],Map[#[[1]]&,rules2],List];
       {Fnew,Mnew,Cnew,rules1,rules2}
];
KinematicReplacements[Fp_,Mp_,Cp_]:= Module[{FCombined,FExpU,FExpLength,
                                     SinCosPowerHeads,SinCosPowerTerms ,
                                     NumberOfSinCosPowerTerms,Replacements,
                                     rules,FNew,MNew,CNew},(
       FCombined = Apply[Plus,Flatten[{Fp,Mp,Cp}]];    
       FExp = Level[FCombined,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       SinCosPowerHeads = {_Sin,_Cos,_Power,_Tan,_Cot,_Sec,_Csc};
       SinCosPowerTerms = Flatten[Map[Cases[FExpU,SinCosPowerHeads[[#]]]&,
                                   Range[1,Length[SinCosPowerHeads]]]];
       NumberOfSinCosPowerTerms = Length[SinCosPowerTerms];
       Replacements = Map[Rule[SinCosPowerTerms[[#]],
                        ToExpression[StringJoin["t",ToString[#]]]]&,
                   Range[1,NumberOfSinCosPowerTerms]];
       rules = Map[Rule[ToExpression[StringJoin["t",ToString[#]]],
                        SinCosPowerTerms[[#]]]&,
                   Range[1,NumberOfSinCosPowerTerms]];
       FNew  = Fp/. Replacements;
       MNew = Mp/. Replacements;
       CNew = Cp/. Replacements;
{FNew,MNew,CNew,rules} )];
SetAttributes[KinematicReplacements,ReadProtected];
SetAttributes[KinematicReplacements,Protected];
SetAttributes[KinematicReplacements,Locked];

Clear[FindMostFrequentTerms];
FindMostFrequentTerms[F_]:=Module[{FExp,FExpU,FExpLength,
                                   AcceptableHeads,NumberOfAcceptableHeads,
                                   LowestLevelExps,InstancesOfOccurences,
                                   LengthLowestLevelExps,NumberOfOccurences,
                                   MaxElem,IndexOfMaxElem},(
       FExp = Level[F,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       AcceptableHeads = {_Cos,_Sin,_Power,_Symbol,_Tan,_Cot,_Sec,_Csc};
       NumberOfAcceptableHeads = Length[AcceptableHeads];
       LowestLevelExps = Flatten[Map[Cases[FExpU,AcceptableHeads[[#]]]&,
                                  Range[1,NumberOfAcceptableHeads]]];
       InstancesOfOccurences = Flatten[Map[Cases[FExp,AcceptableHeads[[#]]]&,
                                  Range[1,NumberOfAcceptableHeads]]];
       LengthLowestLevelExps = Length[LowestLevelExps];
       NumberOfOccurences = Map[Count[InstancesOfOccurences,
                                      LowestLevelExps[[#]]]&,
                                Range[1,LengthLowestLevelExps]];
       MaxElem = Max[NumberOfOccurences];
       IndexOfMaxElem = Flatten[Position[NumberOfOccurences,MaxElem]];
       MostFrequentTerms = LowestLevelExps[[IndexOfMaxElem]];
       
MostFrequentTerms)];
SetAttributes[FindMostFrequentTerms,ReadProtected];
SetAttributes[FindMostFrequentTerms,Protected];
SetAttributes[FindMostFrequentTerms,Locked];


Clear[ReduceFLOPSToo];
ReduceFLOPSToo[Expr_]:=Module[{FExp,FExpU,FExpLength,AcceptableHeads,
       ListOfExpressions,LengthListOfExpresions,ContinueCollection,
       CollectionList,InstancesOfOccurences,NumberOfOccurences,MaxElem,
       IndexOfMaxElem,MostFrequentExpression,ExprTmp},(
(* Compute a list of all expression with specified heads *)
       ExprTmp = Expr;
       FExp = Level[ExprTmp,Infinity];
       FExpU = Union[FExp];
       FExpLength = Length[FExp];
       AcceptableHeads = {_Cos,_Sin,_Power,_Symbol,_Tan,_Cot,_Sec,_Csc};
       ListOfExpressions = Flatten[Map[Cases[FExpU,#]&,AcceptableHeads]];
       LengthListOfExpresions = Length[ListOfExpressions];
       ContinueCollection = True;
       CollectionList = {};
       Exprnew= Collect[ExprTmp,CollectionList];
     While[ContinueCollection,(
(* Compute how many times each expression in ListOfExpressions appears in Expr *)
         InstancesOfOccurences = Flatten[Map[Cases[FExp,#]&,AcceptableHeads]];
         NumberOfOccurences = Map[Count[InstancesOfOccurences,#]&,
                                     ListOfExpressions];
(* Find expression(s) with the highest frequency of occurence *)
         MaxElem = Max[NumberOfOccurences];
         IndexOfMaxElem = Flatten[Position[NumberOfOccurences,MaxElem]];
       
  MostFrequentExpression =ListOfExpressions[[IndexOfMaxElem]];
(* Add it to collection list *)
         CollectionList =Flatten[{CollectionList,MostFrequentExpression}];
(* Remove it from list of expressions *)
         ListOfExpressions = Complement[ListOfExpressions,CollectionList];
        (* Print[ListOfExpressions];
         Print[CollectionList];*)
         If [Length[ListOfExpressions] != 0,(
           (*then*)
           (* Perform Collections *)
            Exprnew= Collect[ExprTmp,CollectionList];
            FExp = Level[Exprnew,Infinity];
            ContinueCollection = True),
        (*else*)   
            ContinueCollection = False;];
    )];
  Exprnew)];
SetAttributes[ReduceFLOPSToo,ReadProtected];
SetAttributes[ReduceFLOPSToo,Protected];
SetAttributes[ReduceFLOPSToo,Locked];


GenerateDissipationTerms[qDot_,R_,V_,p_,q_]:=RayleighDissipationForce[qDot,R,V,p,q]
Clear[RayleighDissipationForce];
RayleighDissipationForce[qDot_,R_,V_,p_,q_]:=Module[{NumElemsVList,
                                 IndexStart,Vp,n,NumElemsList2,Dummy,IndexEnd,
                             PartialRPartialqDot,QFunction,QFunction2},(

    (* Compute Vp = V(q)*p   *)
    NumElemsVList = Length[V]; 
    IndexStart = 1;Vp = {};
    For[n=1, n<NumElemsVList+1, n++,
        {NumElemsList2,Dummy} = Dimensions[V[[n]]];
        IndexEnd = IndexStart+NumElemsList2-1;
        Vp = Join[Vp,V[[n]].p[[Range[IndexStart,IndexEnd]]]];
        IndexStart = IndexEnd+1;
   ];
   PartialRPartialqDot = Map[D[R,qDot[[#]] ]&,Range[1,Length[qDot]]];
  (* Make above expression a function of the elements of qDot *)
  (*                       QFunction[qDot] = PartialRPartialqDot                  *)
  QFunction = Function[Evaluate[qDot],Evaluate[PartialRPartialqDot]];
  (*                                      t *)
  (* Generate product -V   * QFunction[Vp]   *)
  (*  *)
  QFunction2 = Apply[QFunction,Vp];
  IndexStart = 1;
  Q = {};
   For[n=1, n<NumElemsVList+1, n++,
        {NumElemsList2,Dummy} = Dimensions[V[[n]]];
        IndexEnd = IndexStart+NumElemsList2-1;
        Q= Join[Q,-Transpose[V[[n]] ].QFunction2[[Range[IndexStart,IndexEnd]]]];
        IndexStart = IndexEnd+1;
   ];
Q
)];
SetAttributes[RayleighDissipationForce,ReadProtected];
SetAttributes[RayleighDissipationForce,Protected];
SetAttributes[RayleighDissipationForce,Locked];

Clear[MatlabForm]
MatlabForm[Matrix_,Matrixname_]:=Module[{MatrixRules,VectorRules,M2},(
    MatrixRules={"{{"->"[","}}"->"]","}, {"->";", ", "->","};
    VectorRules={"{"->"[","}"->"]", ", "->","};
    If[MatrixQ[Matrix],
      {n,m}=Dimensions[Matrix];
      M2 = StringReplace[ToString[NumberForm[Matrix,16,ExponentFunction -> (If[-100 < # < 100, Null,#]&)]],MatrixRules],
      M2 = StringReplace[ToString[NumberForm[Matrix,16,ExponentFunction -> (If[-100 < # < 100, Null,#]&)]],VectorRules]
    ];
    StringJoin[Matrixname,"=",M2,";"]
)];
SetAttribute[MatlabForm,ReadProtected];
SetAttribute[MatlabForm,Protected];
SetAttribute[MatlabForm,Locked];

                         
(* ****************************************************** *)                         

(* Some Utility Functions *)

Clear[DiagJoin];
DiagJoin[A_,B_]:=Module[{ma,na,mb,nb,Zilch,H0,H1,X},(
                 If[A=={},X=B,(
                 {ma,na}=Dimensions[A];{mb,nb}=Dimensions[B];
                 Zilch=Table[0,{mb},{na}];H0=Join[A,Zilch];
                 Zilch=Table[0,{ma},{nb}];H1=Join[Zilch,B];
                 X=Transpose[Join[Transpose[H0],Transpose[H1]]])];
                 X
                 )];
Clear[DiagJoinLst];
DiagJoinLst[Lst_]:=Module[{N,X,A,B},(
                 N=Length[Lst];X=Lst[[1]];
                 Do[(
                 A=X;B=Lst[[i+1]];
                 X=DiagJoin[A,B];
                 ),
                 {i,N-1}];
                 X
                 )];
              
Clear[RotationMatrixEulerSm];
RotationMatrixEulerSm[chsi_] := 
        { { 1,psi,-theta } , 
          { -psi,1,phi } ,
          {theta,-phi,1 } }/.{phi->chsi[[1]],theta->chsi[[2]], psi->chsi[[3]]};

Clear[ConfigurationMatrixEulerSm];
ConfigurationMatrixEulerSm[chsi_,r_] := Module[{L,X1,X},(
                          L=Simplify[RotationMatrixEulerSm[chsi]];
                          X1=Transpose[Join[L,{r}]];
                          X=Join[X1,{{0,0,0,1}}]
                          )];

Clear[SpecialMatrixExp];
SpecialMatrixExp[h_,eps_]:=Module[{s,s1,s2,s3,
b,b1,b2,b3,rules},
s={s1,s2,s3};b={b1,b2,b3};
rules=Inner[Rule,s,h,List];
Do[If[(h[[i]]==0)||(h[[i]]==1),
     b[[i]]=h[[i]],
     b[[i]]=s[[i]],
     b[[i]]=s[[i]]],
{i,1,3}];
SpecialMatrix[b,eps]/.rules
];

Clear[SpecialMatrix];
SpecialMatrix[h_,eps_]:=Module[{rules,rule2,cc,invrule2,
  vals,vecs,Lambda,Temp},
  If[h.h==0,Return[IdentityMatrix[3]]];
  rules={1/Sqrt[-h.h]->-I*Sqrt[h.h],
           Sqrt[-h.h]->I*Sqrt[h.h]};
  rule2={Sqrt[h.h]->cc};
  invrule2={cc->Sqrt[h.h]};
  {vals,vecs}=Eigensystem[eps*AToATilda[Rationalize[h]]]/.rules;
  Lambda=ComplexExpand[{Exp[vals[[1]]],
                        Exp[vals[[2]]],Exp[vals[[3]]]}];
  Temp=Transpose[vecs].DiagonalMatrix[Lambda].Inverse[Transpose[vecs]];
  Simplify[Temp/.rule2]/.invrule2
];

Clear[ComputeVp];
ComputeVp[V_,p_]:=V.p /; MatrixQ[V];                          
ComputeVp[V_,p_]:=Module[{Finish,Start,Inds,VdimsCols,Vp},(
    VdimsCols = Transpose[Map[Dimensions,V]][[1]];
    LengthV = Length[VdimsCols];
    Finish = Map[Apply[Plus,Take[VdimsCols,#]]&,Range[1,LengthV]];
    Start = Finish-VdimsCols+1;
    Inds = Map[Range[Start[[#]],Finish[[#]]]&,Range[1,LengthV]];
    Vp = Flatten[Map[V[[#]].p[[Inds[[#]]]]&,Range[1,LengthV]]];
    Vp
    )];
 
Clear[ComputeTransVp];
ComputeTransVp[V_,p_]:=Transpose[V].p /; MatrixQ[V];                          
ComputeTransVp[V_,p_]:=Module[{Finish,Start,Inds,VdimsCols,Vp},(
    VdimsCols = Transpose[Map[Dimensions,V]][[1]];
    LengthV = Length[VdimsCols];
    Finish = Map[Apply[Plus,Take[VdimsCols,#]]&,Range[1,LengthV]];
    Start = Finish-VdimsCols+1;
    Inds = Map[Range[Start[[#]],Finish[[#]]]&,Range[1,LengthV]];
    Vp = Flatten[Map[Transpose[V[[#]]].p[[Inds[[#]]]]&,Range[1,LengthV]]];
    Vp
    )];
 
Clear[DefineChainBodyX];
DefineChainBodyX[ChnLst_,TerminalNode_,BodyLst_,X_]:=Module[
    {ModChnBodyLst,ChnBodyLst,ChnX,s,r,NextBody,
    BodyType,ChnLength,Body,LastBody,JointNo,co},(
    ChnLength=Length[ChnLst];
     (* Compose ChnBodyLst, ChnX *)
    ChnBodyLst={}; ChnX={};
    Do[(
       ChnBodyLst=Append[ChnBodyLst,BodyLst[[ChnLst[[k]][[2]]]]];
       ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
       ),{k,ChnLength}];
     (* Modify Last Body Data to Include only Terminal Node *)
     ModChnBodyLst={};
     If[ChnLength>1,
       Do[(
         Body=ChnBodyLst[[k]];
         JointNo=ChnLst[[k+1]][[1]];
         {co}=Cases[Body[[2]],{JointNo,xx_}->xx];
         NextBody={Body[[1]],co};
         Do[NextBody=Append[NextBody,Body[[k]]],{k,3,Length[Body]}];
         ModChnBodyLst=Append[ModChnBodyLst,NextBody];
         (*
         ModChnBodyLst=Append[ModChnBodyLst,{Body[[1]],co,Body[[3]],Body[[4]]}]
         *)
       ),{k,ChnLength-1}]
     ];
     Body=ChnBodyLst[[ChnLength]];
     JointNo=TerminalNode;
     If[Count[Body[[2]],{JointNo,xx_}]!=1,
          Print["terminal node not identified"],
          {co}=Cases[Body[[2]],{JointNo,xx_}->xx]];
     LastBody={Body[[1]],co};
     Do[LastBody=Append[LastBody,Body[[k]]],{k,3,Length[Body]}];
     (*
     LastBody={Body[[1]],co,Body[[3]],Body[[4]]};
     *)
     ModChnBodyLst=Append[ModChnBodyLst,LastBody];
     {ModChnBodyLst,ChnX}
     )];
  
Clear[DefineChainData];
DefineChainData[ChnLst_,TerminalNode_,BodyLst_,X_,H_,q_,p_]:=Module[
    {ModChnBodyLst,ChnBodyLst,ChnX,ChnH,Chnq,ChnV,
    dof,s,r,rtot,njoints,qvec,Vel,beta,BodyType,
    ChnLength,Body,LastBody,JointNo,co},(
     ChnLength=Length[ChnLst];
     (* Compose ChnBodyLst, ChnX, ChnH and Chnq, ChnV *)
     ChnBodyLst={}; ChnX={}; ChnH={}; Chnq={};ChnV={};
     dof=Length[q];beta=p;
     njoints=Length[H];rtot=0;qvec={};Vel={};
     Do[({s,r}=Dimensions[H[[j]]];
       qvec=Append[qvec,Take[q,{rtot+1,rtot+r}]];
       Vel=Append[Vel,Take[beta,{rtot+1,rtot+r}]];
       rtot=rtot+r;
     ),{j,njoints}];
     Do[(
       ChnBodyLst=Append[ChnBodyLst,BodyLst[[ChnLst[[k]][[2]]]]];
       ChnX=Append[ChnX,X[[ChnLst[[k]][[1]]]]];
       ChnH=Append[ChnH,H[[ChnLst[[k]][[1]]]]];
       BodyType=Length[BodyLst[[ChnLst[[k]][[2]]]]];
       If[BodyType==4,
         Chnq=Join[Chnq,qvec[[ChnLst[[k]][[1]]]]];
         ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]],
         Chnq=Join[Chnq,qvec[[ChnLst[[k]][[1]]]]];
         Chnq=Join[Chnq,BodyLst[[ChnLst[[k]][[2]]]][[5]]];
         ChnV=Join[ChnV,Vel[[ChnLst[[k]][[1]]]]];
         ChnV=Join[ChnV,BodyLst[[ChnLst[[k]][[2]]]][[6]]]
        ];
       (* add flex deformation coords & velocities *)
     ),{k,ChnLength}];
     (* Modify Last Body Data to Include only Terminal Node *)
     ModChnBodyLst={};
     If[ChnLength>1,
       Do[(
         ModChnBodyLst=Append[ModChnBodyLst,ChnBodyLst[[k]]]
       ),{k,ChnLength-1}]
     ];
     Body=ChnBodyLst[[ChnLength]];
     JointNo=TerminalNode;
     If[Count[Body[[2]],{JointNo,xx_}]!=1,
          Print["terminal node not identified"],
          {co}=Cases[Body[[2]],{JointNo,xx_}->xx]];
     LastBody={Body[[1]],{{TerminalNode,co}}};
     Do[LastBody=Append[LastBody,Body[[k]]],{k,3,Length[Body]}];
     (*
     LastBody={Body[[1]],{{TerminalNode,co}},Body[[3]],Body[[4]]};
     *)
     ModChnBodyLst=Append[ModChnBodyLst,LastBody];
   {ModChnBodyLst,ChnX,ChnH,Chnq,ChnV}
   )];

Clear[SystemQuasiVelocities];   
SystemQuasiVelocities[BodyLst_,p_]:=Module[{pp,NoBodies,
   BodyType},(   
   (* Define list of system quasi-velocities:
      these are joint quasi-velocities plus flex body deformation
      velocities *)
   pp=p;NoBodies=Length[BodyLst];
   Do[(
      BodyType=Length[BodyLst[[i]]];
      If[BodyType==4,
        pp=pp,
        pp=Join[pp,BodyLst[[i]][[6]]]
        ];
     ),{i,NoBodies}];
  pp
  )];
  
Clear[SystemCoordinates];   
SystemCoordinates[BodyLst_,q_]:=Module[{qq,NoBodies,
   BodyType},(   
   (* Define list of system coordinatess:
      these are joint coordinates plus flex body deformation
      coordinates *)
   qq=q;NoBodies=Length[BodyLst];
   Do[(
      BodyType=Length[BodyLst[[i]]];
      If[BodyType==4,
        qq=qq,
        qq=Join[qq,BodyLst[[i]][[5]]]
        ];
     ),{i,NoBodies}];
  qq
  )];
  
  Clear[TerminalNodeChain];
  TerminalNodeChain[NodeNumber_,TreeList_,BodyList_]:=
    Module[{Bodyouts,TempList,BodyNo,Chain,SubChain},
    TempList={};BodyNo=0;
    Do[(BodyOuts=BodyList[[i]][[2]];
        TempList=Cases[BodyOuts,{NodeNumber,xx_}->xx];
        If[TempList!={},BodyNo=i;Break[]];
    ),{i,Length[BodyList]}];
    If[Length[TemList]>1,
       Print["Body "<>ToString[i]<>" has more than one node "<>ToString[NodeNumber]];
    ];
    If[BodyNo==0,Print["Can't find node in any body"];Return[]];
    Do[(Chain=TreeList[[i]];
        SubChain={};
        If[MatchQ[Chain,{___,{x_Integer,BodyNo},___}],
           SubChain=Chain/.{y___,{x_Integer,BodyNo},___}->{y,{x,BodyNo}};
           Return[SubChain]
        ];
    ),{i,Length[TreeList]}];
    SubChain
    ];
    

Clear[MyInverse];

MyInverse[A_?MatrixQ]:=Module[{
mat,            (* matrix of symbolic terms of same size as A*)
a,              (* a[i,j] are elements of mat *)
Inv,            (* inverse[mat] *)
ElemsA,         (* List of elements of A *)
ZeroElemsA,     (* positions of the zero elements of A *)
NonZeroElemsA,  (* Complement of ZeroElemsA*)
ZeroRule,       (* Rule to substitute the zero elements first *)
NonZeroRule     (* Rule to substitute the rest of the elements *)
},
mat=Table[a[i,j], {i,Dimensions[A][[1]]},{j,Dimensions[A][[2]]}];
ElemsA=Flatten[A];
ZeroElemsA=Flatten[Position[ElemsA,0]];
NonZeroElemsA=Complement[Range[Length[ElemsA]],ZeroElemsA];
ZeroRule=Thread[Flatten[mat][[ZeroElemsA]]->ElemsA[[ZeroElemsA]]];
NonZeroRule=Thread[Flatten[mat][[NonZeroElemsA]]->ElemsA[[NonZeroElemsA]]];
Inv=Inverse[mat/.ZeroRule];
Inv/.NonZeroRule
];

Print["  *** Dynamics successfully loaded ***"];
End[]

EndPackage[ ]
If[!spell1, On[General::spell1]];
If[!spell, On[General::spell]];

