(******************************************************************* This file was generated automatically by the Mathematica front end. It contains Initialization cells from a Notebook file, which typically will have the same name as this file except ending in ".nb" instead of ".m". This file is intended to be loaded into the Mathematica kernel using the package loading commands Get or Needs. Doing so is equivalent to using the Evaluate Initialization Cells menu command in the front end. DO NOT EDIT THIS FILE. This entire file is regenerated automatically each time the parent Notebook file is saved in the Mathematica front end. Any changes you make to this file will be overwritten. ***********************************************************************) Off[General::spell1] BeginPackage["DynamicsWorkbench`"]; AccCOM::usage = "AccCOM[body] returns a vector describing the acceleration of the center of mass of body with respect to the ground reference frame."; AccJnt::usage = "AccJnt[body] returns the velocity of the point on the inboard body which corresponds to the location of the joint connecting the two."; AccJnt2::usage = "AccJnt2[body] returns the acceleration of the point on body which instaneously corresponds to the location of the joint between the body and the inboard body. Used in slider joints."; AddFrame::usage = "AddFrame[frame,baseframe,jointtype,opts] adds a new reference frame to the frame tree. This frame is specified relative to a base frame which precedes it in the frame tree. Several Joint types are available."; AddBody::usage = "AddBody[body,inboard,jointtype,opts] adds a new body to the system. This body is connected to the inboard body through a joint specified by jointtype. Several Joint types are available (type '?Joints'). Options InbToJnt and BodyToJnt specify vectors from the center of mass of the inboard body to the joint, and from the new body's center of mass to the joint. These vectors may be in any reference frame(s). Options Mass and Inertia describe physical parameters. Other options specify the axes corresponding to the joint."; AngAcc::usage = "AngAcc[body] returns the angular acceleration of the body with respect to ground."; AngMom::usage = "AngMom[body,pnt] returns the angular momentum of the body about a given point, with respect to the ground reference frame. AngMom[body,pnt,ref] returns the angular momentum in a specified (inertial) frame. A list of bodies can also be provided. The given point pnt is treated as fixed in the reference frame. For angular momentum about a moving point, it is necessary to add the appropriate inertial term manually."; AngVel::usage = "AngVel[body] returns the angular velocity of the body with respect to ground. AngVel[body,frme] returns the angular velocity of the body with respect to frme"; AppFrc::usage = "AppFrc[body,force,point] applies vector force to the body at a vector point from the center of mass. Vectors may be specified in defined reference frame."; AppTrq::usage = "AppTrq[body,torque] applies a moment specified by the vector torque to body. The vector may be specified in any defined reference frame."; AtT0::usage = "AtT0[expr] returns expr set at time zero."; Axis::usage = "Axis->axis specifies a rotational axis, and may be in the form of an axis number (1 through 3), a vector (e.g., ground[1]), or a list of unit vector coefficients (direction cosines). For joints with multiple rotatory axes, they are numbered Axis1, Axis2, etc."; Ball::usage = "A Ball joint is a three degree-of-freedom rotatory joint. Its home position is coincident with the base reference frame (the frame of the inboard body). The generalized coordinates Qdof1 through Qdof4 describing the rotations are in the form of euler parameters {e1,e2,e3,e4}, where e4 is the cosine of the half-rotational angle. The generalized speeds Udof1 through Udof3 are the angular velocities about the body's reference frame axes."; Bodies::usage = "Bodies returns a list of the currently defined bodies."; BodyToJnt::usage = "BodyToJnt[body] is the vector from the body's center of mass to the joint connecting it to the inboard joint."; CastMtx::usage = "CastMtx[from,to] returns the transformation matrix needed to cast a vector from one reference frame to another."; CastV::usage = "CastV[vector,frame] will cast any vector into a specified reference frame."; CollectE::usage = "CollectE[eom] puts the equations of motion into a form simplified with respect to the udots, so that they do not appear multiple times. CollectE[eom, x] simplifies with respect to any expression x appearing in the equations."; ConvertList::usage = "ConvertList[vector, frame] is a utility function which converts either a vector, a list of unit vector coefficients (direction cosines), or the number between 1 and 3 designating an axis into the form of a list of the unit vector coefficients for the specified frame."; Cross::usage = "Cross[vector1,vector2] or (vector1 ~X~ vector2) performs the cross product of two vectors."; D::usage = "Partial derivatives of single or multiple vectors are found using D[ vector, scalar, in->frame, NonConstants->{functions} ], where in and NonConstants are options. Option in is used to specify the reference frame the partial derivative is desired in, and defaults to ground."; Dt::usage = "Total derivatives of a single or multiple vector are found using Dt[ vector, scalar, in->frame, Constants->{constants} ], where in and Constants are options. Option in is used to specify the reference frame the total derivative is desired in, and defaults to ground. If the angular velocities of all of the reference frames vector is defined in are themselves defined, the total time derivative is computed using those angular velocities."; Dist::usage = "Dist[point1,point2] returns the distance between two points, which need not be described in the same reference frames."; Dot::usage = "Dot[vector1,vector2] or vector1 . vector2 performs the dot product of two vectors (or a vector and a dyadic)."; Dyadic::usage = "A dyadic may be performed by using vector1 ** vector2."; EOM::usage = "EOM[] generates the equations of motion. EOM[Simplify->On] applies simplification at low levels to attempt to generate more compact equations, but at the expense of more execution time."; FindEquil::usage = "FindEquil[ eqn, {x,x0} ] finds the equilibrium point in the given equation, for variable x. The initial guess of x0 is used to start a Newton search. For a list of equations, use FindEquil[ eqns, {x,x0}, {y,y0},...] to find equilibrium points in several variables x, y,..."; Fixed::usage = "A Fixed joint is a zero degree-of-freedom joint. Its axes can be offset from those of the base reference frame using the Offset option. Offset may be a matrix of direction cosines or a list of vectors specifying the offset axes in terms of the base frame. Examples: Offset->{{0,1,0},{1,0,0},{0,0,1}}, Offset->{body[2],body[1],body[3]}. A Fixed joint has no generalized speeds or coordinates associated with it."; Force::usage = "Force[body] returns a list of the forces acting on the body."; Frames::usage = "Frames returns a list of the currently defined reference frames."; GActFrc::usage = "GActFrc[body,n] returns the generalized active force of body with respect to the nth generalized speed. GActFrc[body] returns the generalized active forces for body with respect to each of the generalized speeds. GActFrc[All] returns the sum of the generalized active forces for all bodies, with respect to each of the generalized speeds."; Gimbal::usage = "A Gimbal joint is a three degree-of-freedom rotatory joint. Its home position is coincident with the base reference frame (the frame of the inboard body). The generalized coordinates describing the rotations are the successive rotation angles about the three specified rotation axes, described by options Axis1, Axis2, and Axis3. Use Ball if euler parameters are preferred as generalized coordinates. Axes may be specified by coordinate number, or a list of direction cosines. Examples: Axis1->3, Axis2->1, Axis3->2; Axis1->{0,0,1}, Axis2->{0,.707,.707}, Axis3->{.707,0,.707}."; GInerFrc::usage = "GInerFrc[body,n] returns the generalized inertia force of body with respect to the nth generalized speed. GInerFrc[body] returns the generalized inertia forces for body with respect to each of the generalized speeds. GInerFrc[All] returns the sum of the generalized inertia forces for all bodies, with respect to each of the generalized speeds."; ground::usage = "ground is the default Newtonian reference frame, which all other frames are defined relative to."; Hinge::usage = "A Hinge joint is a single degree-of-freedom rotatory joint. The axis about which the joint rotates is specified by the option Axis, which may be the coordinate number, a list of direction cosines, or a vector in the base reference frame (the frame of the inboard body). Examples: Axis->3, Axis->{0,0,1}, Axis->body[1]. The generalized speed, Udof, is defined to be the time-derivative of generalized coordinate Qdof."; InbToJnt::usage = "InbToJnt[body] is the vector from the center of mass of the inboard body to the joint connecting it to the argument body."; Inboard::usage = "Inboard[body] returns the name of the body inboard to the argument."; Inertia::usage = "Inertia[body] is the inertia dyadic of the body."; JntToJnt::usage = "JntToJnt[body] is the vector from the joint of the inboard body to the joint of the outboard body. This vector is used internally with slider joints."; Joints::usage = "The following joint types are supported: Hinge, Ball, Slider, Fixed, Gimbal, SixDOF, UJoint. For more information, use ? followed by the joint type."; Kids::usage = "Kids[frame] returns a list of reference frames below the argument's entry in the frame tree."; Kinematics::usage = "Kinematics returns the set of kinematical differential equations."; LinMom::usage = "LinMom[body] is the linear momentum of the body. \ LinMom[{body1,body2,...}] is the linear momentum of the system \ consisting of the bodies listed."; Mass::usage = "Mass[body] is the mass of the body."; MassMatrix::usage = "MassMatrix[eom] returns the mass matrix and other terms from the specified equations of motion."; NewModel::usage = "NewModel[] initializes the Dynamics Workbench for a new model. The generalized coordinates and generalized speeds, and information regarding bodies and reference frames are all cleared. NewModel[Zees->On] indicates that the subsequent model is to be constructed with intermediate terms stored in the 'zees' variable"; Nonholonomic::usage = "Nonholonomic returns a list of the nonholonomic constraints. These include holonomic constraints rising from closed-loop mechanisms, which are treated as if they were nonholonomic and therefore expressed in differential form."; Offset::usage = "Offset->axeslist is an option to AddFrame which offsets the new reference frame with respect to the base frame. The default is no offset. Offset is specified as the coordinates of each of the new frame's axes, in terms of the base frame. These may specified as vectors, a list of three lists of direction cosines, or a list of three axis numbers."; Parents::usage = "Parents[frame] returns a list of reference frames above the argument's entry in the frame tree."; PosCOM::usage = "PosCOM[body] returns a vector describing the position of the center of mass of body with respect to the ground reference frame. PosCOM[{body1,body2,...}] returns the position of the center of mass of the system consisting of the bodies listed."; PosPnt::usage = "PosPnt[point,body] returns the vector describing the position of point (entered as a vector relative to the center of mass) fixed to body."; PrtVel::usage = "PrtVel[expr,n] or PrtVel[expr,u[n]] returns the partial velocity of expression with respective to the nth generalized speed."; PV::usage = "PV[{c,frame,n}..}] is a head used by the Dynamics Workbench to denote vectors. Each 'Packaged Vector' is placed within this head and consists of a series of lists denoting a constant multiplied by a unit vector n of the reference frame: c frame[n]. PV's are not normally visible to users."; q::usage = "q[n] refers to the nth generalized coordinate."; qdofs::usage = "qdofs is a list of the currently defined generalized coordinates."; qdots::usage="qdots[qs] returns a list of qdots, of the form q[1]', q[2]', etc. for the specified coordinates."; RelativeTo::usage = "RelativeTo->frame is an option for AddBody which specifies an alternate reference frame which the new body's frame is referenced to. The default for RelativeTo is the inboard body's frame."; ResltF::usage = "ResltF[body] returns the resultant of the forces acting on the body."; ResltT::usage = "ResltT[body] returns the resultant of the torques or moments acting on the body."; Rstar::usage = "Rstar[body] returns the inertia forces acting on body."; SixDOF::usage = "A SixDOF joint is a six degree-of- freedom joint which allows for full translational and rotational movement between bodies. Its location ahd home position are specified by the BodyToJoint option. The translational axes are specified by TAxis1, TAxis2, and TAxis3. The translations along those axes are specified by generalized coordinates Qdof1, Qdof2, and Qdof3, and by generalized speeds Udof1, Udof2, and Udof3. The rotational orientation is described by four euler parameters, specified by generalized coordinates Qdof4 through Qdof7. The angular velocities about the base frame's axes are specified by generalized speeds Udof1, Udof2, and Udof3."; Slider::usage = "A Slider joint is a single degree-of- freedom prismatic joint. Its location and home position are specified by the BodyToJoint option. The translational axis is specified by TAxis, which may be a coordinate number, a list of direction cosines, or a vector in the base reference frame. Examples: TAxis->3, TAxis->{0,0,1}, TAxis->body[1]. The generalized speed, Udof, is defined to be the time-derivative of generalized coordinate Qdof."; SolveEqn::usage = "SolveEqn[eqn, vars] solves a list of equations in eqn for the given variables, and puts them in order as an == expression. SolveEqn is useful if the -> expression returned by Solve is inconvenient."; States::usage = "States returns a list of all of the states currently used in the model."; t::usage = "t is time."; TAxis::usage = "TAxis->taxis specifies a translational axis, and may be in the form of an axis number (1 through 3), a vector (e.g., ground[1]), or a list of unit vector coefficients (direction cosines). For joints with multiple translational axes, they are numbered TAxis1, TAxis2, etc."; Tmtx::usage = "Tmtx[frame] is the transformation matrix which maps between frame and the inertial (ground) reference frame. Tmtx returns a list of coordinates of the three axes of frame, each in the form of a sublist comprising coefficients for axes of reference frame."; Torque::usage = "Torque[body] returns a list of the torques acting on the body."; Tstar::usage = "Tstar[body] returns the inertia torque acting on body."; u::usage = "u[n] refers to the nth generalized speed."; udofs::usage = "udofs is a list of the currently defined generalized speeds."; udots::usage="udots[us] returns a list of udots, of the form u[1]', u[2]', etc. for the specified speeds."; UJoint::usage = "A Ujoint is a two degree-of-freedom rotatory joint. Its home position is coincident with the base reference frame (the frame of the inboard body). The generalized coordinates describing the rotations are the successive rotation angles about the two specified rotation axes, Axis1 and Axis2. The generalized speeds, Udof1 and Udof2, are defined to be the time-derivatives of the respective generalized coordinates, Qdof1 and Qdof2."; VelCOM::usage = "VelCOM[body] returns a vector describing the velocity of the center of mass of body with respect to the ground reference frame. VelCOM[{body1, body2,...}] returns the velocity of the center of mass of the system consisting of the bodies listed."; VelJnt::usage = "VelJnt[body] returns the velocity of the point on the inboard body which corresponds to the location of the joint connecting the two."; VelJnt2::usage = "VelJnt2[body] returns the velocity of the point on body which instantaneously corresponds to the location of the joint between body and the inboard body. Used in slider joints."; XDot::usage = "XDot[eom, state] returns the state derivative evaluated using the provided equations eom, and the current state. The command States describes the ordering of generalized coordinates and speeds in the state vector."; Z::usage = "Z[n] refers to the nth zee variable, but does not return the value. Use zees[[n]] to retrieve the value of the intermediate expression. Use NewModel[Zees->On] to specify that zees should be formed."; Zees::usage="Zees->On is an option to NewModel that specifies that intermediate 'zee' variable should be generated in order to make expressions shorter. Z[n] refers to the nth zee variable, but does not return the value. Use zees[[n]] to retrieve the value of the intermediate expression. Use NewModel[Zees->On] to specify that zees should be formed."; formtrigreplacements::usage="formtrigreplacements[equations] returns a list of shorthand trigonometric replacements that could be used to condense the equations given. The output is of the form {Sin[q[1]]->\"s1\",Cos[q[2]]->\"c2\"}. Options include ArgReplacements->{{gamma,\"g\",alpha,\"alp\"}} for shortening other argument variables, and TrigReplacements->{{Sin,\"s\",{Cos,\"c\"}} for changing the default trigonometric function substitutions."; formqureplacements::usage= "formqureplacements returns a list of shorthand replacments for the q's, u's, and udot's, e.g. {q[1]\[Rule]\"q1\",u[1]->\"u1\",u[1]'->\"u1dot\"}."; SymmetricQ::usage= "SymmetricQ[M] returns True if M is a matrix."; ExportExpression::usage="ExportExpression[expression, \"name\"] formats expression as Matlab code, producing a string that begins with 'name = ' followed by the expression. ExportExpression can export a matrix, a symmeric matrix, a list, or a scalar. Formatting options include EndLine\[Rule]\"\\n\", Continuator\[Rule]\"...\", LineLength\[Rule]78, Deliminator\[Rule]\" \", which specify the end-of-line character, line continuator, maximum line length of the output, and the deliminators where line breaks can occur (default to space character). The options to formtrigreplacements can also be used."; ExportCode::usage="\!\(\* StyleBox[\"ExportCode\",\nFontWeight->\"Bold\"]\)\!\(\* StyleBox[\" \",\nFontWeight->\"Bold\"]\)writes a Matlab function to evaluate the state-derivative. Features include substitution of q's and u's for the state x, and substitution of shorthand trig functions such as s1 for sin(q(1)). ExportCode[{massmatrix,rhs}] uses the mass matrix and right-hand side given (usually found using MassMatrix[]). Alternatively, ExportCode[equations] will call MassMatrix[] automatically. Several optional arguments may be given. Forces\[Rule]{T1,T2} specifies forces that enter linearly into the equations; ExportCode will export a matrix of coefficients for these forces. Another option is Expressions\[Rule]{{KE,\"KE\"},{PE,\"PE\"}}, which outputs the specified additional expressions, named with the strings given. Each expression must be paired with a name within a list. The output of ExportCode can be sent to a file with OutputFile\[Rule]\"filename\", which is automatically produced in the current directory. Formatting options include EndLine\[Rule]\"\\n\", Continuator\[Rule]\"...\", LineLength\[Rule]78, Delimiter\[Rule]\" \", which specify the end-of-line character, line continuator, maximum line length of the output, and the delimiters where line breaks can occur (default to space character). The options to formtrigreplacements can also be used."; LinearTerms::usage="LinearTerms[expressions,terms] returns a matrix of linear coefficients of the variables listed in terms,that appear in the expressions (also a list) given. One option is Simplifier\[Rule]Simplify, which causes the specified function to be applied to the matrix. Also see UnLinearTerms.";\ UnLinearTerms::usage="UnLinearTerms[expressions,terms] returns all of the remaining terms that are not linear in the LinearTerms. One option is Simplifier->Simplify, which causes the specified function to be applied to the output."; fmtString::usage="fmtString[string] reformats a long String argument and tries to break it into a bunch of lines with a Matlab (or other) continuator,so that the output is fairly readable code. Options include EndLine\[Rule]\"\\n\" (end of line character), Continuator\[Rule]\"...\" (Matlab or other line continuator), LineLength\[Rule]78 (max line length), Delimiter\[Rule]\" \" (look for spaces to break lines on; lists of delimiters are also acceptable here)."; Clear[Axis1,Axis2,Axis3,TAxis1,TAxis2,TAxis3, Udof,Udof1,Udof2,Udof3,Udof4,Udof5,Udof6, Qdof,Qdof1,Qdof2,Qdof3,Qdof4,Qdof5,Qdof6,Qdof7 ]; AngVel[ frme1_, frme2_ ] := AngVel[ frme1 ] - AngVel[ frme2 ]; Common[a_List, b_List] := Module[ {n = Min[ Length[a], Length[b] ]}, Part[ a, Flatten[ Position[ MapThread[ SameQ, {Take[a,n], Take[b,n]} ], True ] ] ] ] Uncommon1[a_List, b_List] := Module[ {n = Min[ Length[a], Length[b] ]}, Delete[ a, Position[ MapThread[ SameQ, {Take[a,n], Take[b,n]} ], True ] ] ] Uncommon2[a_List, b_List] := Module[ {n = Min[ Length[a], Length[b] ]}, Delete[ b, Position[ MapThread[ SameQ, {Take[a,n], Take[b,n]} ], True ] ] ] Wrap[ {} ] = {{}}; Wrap[ a_List ] := a; Mod3[ n_Integer ] := Mod[ n-1, 3] + 1; Normed[ v_ ] := v / PowerExpand[ Sqrt[ v.v ] ]; NormV[ v_ ] := PowerExpand[ Sqrt[ v.v ] ]; NormV[ x:PV[__] ] := PowerExpand[ Sqrt[ Simplify[ Apply[ Plus, Map[ #^2 &, Map[ #[[1]]&, CastV[x, ground ] ] ] ] ] ] ]; Normed[ x:PV[__] ] := x / NormV[ x ]; CoordQ[ n_ ] := IntegerQ[n] && Abs[n] >= 1 && Abs[n] <=3; TripleQ[ v_ ] := VectorQ[v] && Length[v] == 3; PrincipalAxisQ[ x:{_,_,_} ] := Count[x,1] == 1 && Count[x,0] == 2; VecToList[ vec_, frame_ ] := {vec . frame[1], vec . frame[2], vec . frame[3]}; ListToVec[ lis_, frame_ ] := lis[[1]] frame[1] + lis[[2]] frame[2] + lis[[3]] frame[3]; ConvertVec[ coord_?CoordQ, frme_ ] := ListToVec[ Sign[coord]*RotateRight[{0,0,1},Abs[coord]], frme ]; ConvertVec[ axis_?TripleQ, frme_ ] := ListToVec[ Normed[axis], frme ]; ConvertVec[ x:PV[__], frme_ ] := x 1/Sqrt[(x.frme[1])^2+(x.frme[2])^2+(x.frme[3])^2]; ConvertVec[l:{(_?TripleQ)..}, frme_ ] := Map[ ConvertVec[#,frme]&, l ] ConvertList[ coord_?CoordQ, frme_ ] := Sign[coord]*RotateRight[{0,0,1},Abs[coord]]; ConvertList[ axis_?TripleQ, frme_ ] := Normed[axis]; ConvertList[ x:PV[__], frme_ ] := VecToList[ x 1/Sqrt[(x.frme[1])^2+(x.frme[2])^2+(x.frme[3])^2], frme]; ConvertOffset[M_?MatrixQ,frme_] := M; ConvertOffset[{x_?CoordQ,y_?CoordQ,z_?CoordQ},frme_]:={ConvertList[x,frme],ConvertList[y,frme],ConvertList[z,frme]}; ConvertOffset[{x:PV[__],y:PV[__],z:PV[__]},frme_] := {VecToList[x,frme],VecToList[y,frme],VecToList[z,frme]}; ConvertInertia[I_?VectorQ,frme_] := I[[1]] frme[1]**frme[1] + I[[2]] frme[2]**frme[2] + I[[3]] frme[3]**frme[3]; ConvertInertia[I_?MatrixQ,frme_] := Apply[ Plus, Flatten[ Outer[ NonCommutativeMultiply, Array[frme,3], Array[frme,3] ] ] * Flatten[ I ] ]; ConvertInertia[x:PV[__],frme_] := x; ConvertInertia[0|0.,frme_]:=PV[{PV[{0,frme,1}],frme,1}]; PickDof[dofs_List] := First[ Complement[ Range[ Length[dofs]+1 ], dofs ] ]; PickDof[dofs_List, num_?NumberQ] := Take[ Complement[ Range[ Length[dofs]+num ], dofs ], num ]; ReplaceDofs[ newdofs_, useddofs_ ] := replDofs[ newdofs, Flatten[ Position[ newdofs, Automatic ] ], PickDof[ Union[ Complement[ newdofs, {Automatic}],useddofs], Count[ newdofs, Automatic ] ] ]; replDofs[ newdofs_, {}, {}] := newdofs; replDofs[ newdofs_List, loc:{_}, val:{_}] := ReplacePart[ newdofs, val[[1]], loc]; replDofs[ newdofs_, loc_List, val_List ] := replDofs[ ReplacePart[ newdofs, val[[1]], loc[[1]] ], Drop[loc, 1], Drop[val, 1] ]; replacesin = { c_. Sin[x_] Cos[y_] + c_. Cos[x_] Sin[y_] -> c Sin[x+y], c_. Sin[x_] Cos[y_] + d_. Cos[x_] Sin[y_] /; d===-c-> c Sin[x-y]}; replacecos = { c_. Cos[x_] Cos[y_] + c_. Sin[x_] Sin[y_] -> c Cos[x-y], c_. Cos[x_] Cos[y_] + d_. Sin[x_] Sin[y_] /; d===-c-> c Cos[x+y]}; replace2SC = {a_. Cos[x_] + b_. Cos[x_] -> (a+b) Cos[x], a_. Sin[x_] + b_. Sin[x_] -> (a+b) Sin[x]}; replaceSC2 = {a_. Cos[x_]^2 + a_. Sin[x_]^2-> a}; replaceUs = {c_. u[n_] + d_. u[n_] -> (c+d) u[n]}; AtT0[ expr_ ] := expr /. t->0; AtT0[ expr__ ] := Flatten[{expr}] /. t->0; FindEquil[ eqn_, args:{_, _}.. ] := FindRoot[ Evaluate[eqn], args ] Mass[(bodies_)?ListQ] := Mass /@ bodies; VelCOM[(bodies_)?ListQ] := Simplify[Plus @@ (Mass /@ bodies*VelCOM /@ bodies)/ Plus @@ Mass /@ bodies] SymmetricQ[(m_List)?MatrixQ] := m === Transpose[m] (*PV/: Times[c_ /; FreeQ[c,PV],PV[{a_,frme_,n_}] ] := PV[{a c,frme,n}]; *) Timespkg[c_,{x_,y_,z_}] := {c x, y, z}; PV/: Times[c_ /; FreeQ[c,PV], PV[args__] ] := Map[ Timespkg[c,#]&, PV[args] ]; PV/: Plus[ PV[x:{_,_,_}..], PV[y:{_,_,_}..] ] := PV[x,y]; PV[{0,_,_}] := 0; PV[{0,_,_},rest__] := PV[rest]; PV[rest1__,{0,_,_},rest2__] := PV[rest1,rest2]; PV[r1___,{c1_ /; FreeQ[c1,PV],frme_,n_},r2___, {c2_ /; FreeQ[c2,PV],frme_,n_},r3___] := PV[r1,r2,r3,{c1+c2,frme,n}]; (*PV[r1:{_,_,_}...,{c1_ /; FreeQ[c1,PV],frme_,n_},r2:{_,_,_}..., {c2_ /; FreeQ[c2,PV],frme_,n_},rest___] := PV[{c1+c2,frme,n},r1,r2,rest] ;*) SetAttributes[PV, Orderless]; If[ TrueQ[ $VersionNumber >= 3.], Format[outV[{c_,v_,n_}]] := HoldForm[c Subscript[UnderBar[v],n] ]; Format[outV[{1,v_,n_}] ] := Subscript[UnderBar[v],n], (*else *) Format[outV[{c_,v_,n_}] ] := HoldForm[c Subscripted[v[n]] ]; Format[outV[{1,v_,n_}] ] := HoldForm[Subscripted[v[n]] ] ]; (*Format[PV[x:{0,_,_}]] := 0;*) Format[PV[x:{_,_,_}..] ] := Apply[ Plus, Map[ outV, {x}] ]; SetAttributes[CollectV,Orderless] CollectV[ PV[r1___,{c1_,frme_,n_},r2___, {c2_,frme_,n_},r3___] ]:= CollectV[ PV[r1,r2,r3,{c1+c2,frme,n}] ]; CollectV[ PV[x__] ] := PV[x]; CastMtx[ from_, to_ ] := Apply[ Dot, Map[ Tmtx, Wrap[Reverse[Uncommon1[ Parents[to], Parents[from] ]] ] ] ] ~ Dot ~ Transpose[ Apply[ Dot, Map[ Tmtx, Wrap[Reverse[Uncommon2[ Parents[to], Parents[from] ]] ] ] ] ] /. Join[replacesin,replacecos,replaceSC2,replace2SC] CastV[ PV[x__], trgtfrme_ /; MemberQ[Frames, trgtfrme] ]:= Simplify[ Apply[ Plus, Map[ Cast1V[#,trgtfrme]&, {x}, 1] ] /. replaceSC2 ]; CastV[ 0, trgtfrme_ ] := 0; Cast1V[ {c_,frme_,n_}, trgtfrme_ ] := Apply[PV, MapThread[List,{c*CastMtx[trgtfrme,frme][[n]], {trgtfrme,trgtfrme,trgtfrme},{1,2,3}}] ] ; CML[from_,to_]:=Module[{frame1,frame2},{frame1,frame2}=Sort[{from,to}]; If[OrderedQ[{from,to}],Identity,Transpose][ CML[frame1,frame2]=CastMtx[frame1,frame2]]] PV/:CenterDot[PV[x:{_,_,_}..],PV[y:{_,_,_}..]]:= Apply[Plus,Distribute[DotV[{x},{y}],List]]/.Join[replacesin,replacecos, replaceSC2,replace2SC]; PV/: CenterDot[ 0 | 0., PV[__] ] = 0; PV/: CenterDot[ PV[__], 0 | 0.] = 0; CenterDot[ 0 | 0., 0 | 0.] = 0; Unprotect[ Dot ]; Dot[ 0 | 0., 0 | 0.] = 0; Protect[ Dot ]; PV/: Dot[ PV[x:{_,_,_}..], PV[y:{_,_,_}..] ] := Apply[Plus, Distribute[ DotV[{x},{y}], List ] ] /. Join[replacesin,replacecos,replaceSC2,replace2SC]; PV/: Dot[ 0 | 0., PV[__] ] = 0; PV/: Dot[ PV[__], 0 | 0.] = 0; DotV[x:{_,_,_},{PV[y:{_,_,_}],frame2_,coord2_}] := PV[ {DotV[x,y], frame2, coord2} ]; (*DotV[{PV[y:{_,_,_}],frame2_,coord2_},x:{_,_,_}] := PV[ {DotV[x,y], frame2, coord2} ]; *) DotV[{PV[y:{c1_,frame1_,coord1_}],frame2_,coord2_},x:{_,_,_}]:= PV[{DotV[{c1,frame2,coord2},x],frame1,coord1}]; DotV[{c1_,frame1_?(MemberQ[Frames,#]&),coord1_}, {c2_,frame2_?(MemberQ[Frames,#]&),coord2_}] := c1 c2 CML[frame1,frame2][[coord2,coord1]] If[ TrueQ[ $VersionNumber >= 3. ], Unprotect[Cross] ]; Cross[ PV[x:{_,_,_}..], PV[y:{_,_,_}..] ] := Module[{ lftarg = Map[ CastV[ PV[x], #]&, Transpose[{y}][[2]] ], rgtarg = Map[ PV, {y} ]}, Apply[Plus, MapThread[ CrossV, {lftarg, rgtarg} ] ] ]; Cross[ expr_, 0 | 0.] := 0; Cross[ 0 | 0., expr_ ] := 0; Cross[ 0 | 0., 0 | 0.] := 0; CrossV[PV[{c1_,frme_,n1_}], PV[{c2_,frme_,n2_}]] := PV[{{ c1 c2, frme, Mod3[n1+2]}, {-c1 c2, frme, Mod3[n1+1]}, { 0, frme, n1 }}[[Mod3[n2-n1]]] ] /. Join[replacesin,replacecos,replaceSC2,replace2SC]; CrossV[PV[x:{_,frme_,_}..],PV[y:{_,frme_,_}] ] := Apply[ Plus, Map[ CrossV[ PV[#], PV[y] ]&, {x}] ]; CrossV[ 0 | 0., PV[__] ] := 0; CrossV[ PV[__], 0 | 0. ] := 0; X[x_,y_] := Cross[x,y]; PV/: PV[{x1_,x2_,x3_}] ** PV[{y1_,y2_,y3_}] := PV[{y1 PV[{x1,x2,x3}],y2,y3}] PV/: PV[x:{_,_,_}..]**PV[y:{_,_,_}..] := Apply[ Plus, Distribute[Map[ PV, {x} ] ** Map[ PV, {y} ], List ] ] PV/: PV[__] ** (0 | 0.) := 0; PV/: (0 | 0.) ** PV[__] := 0; u[n_] := ut[n][t]; If[ TrueQ[ $VersionNumber >= 3.], Format[ ut[n_][t] ] := Subscript[u,n]; Format[ ut[n_]'[t] ] := Subscript[u,n]', (*else *) Format[ ut[n_][t] ] := HoldForm[ Subscripted[u[n]] ]; Format[ ut[n_]'[t] ] := HoldForm[ Subscripted[u[n]]'] ] SetAttributes[ u, Listable ]; attr = If[ TrueQ[$VersionNumber >= 3.], NHoldAll, NProtectedAll]; SetAttributes[ u, attr]; q[n_] := qt[n][t]; If[ TrueQ[ $VersionNumber >= 3.], Format[ qt[n_][t] ] := Subscript[q,n]; Format[ qt[n_]'[t] ] := Subscript[q,n]'; Format[ qt[n_]''[t] ] := Subscript[q,n]'', (* else *) Format[ qt[n_][t] ] := HoldForm[ Subscripted[q[n]] ]; Format[ qt[n_]'[t] ] := HoldForm[ Subscripted[q[n]]']; Format[ qt[n_]''[t] ] := HoldForm[ Subscripted[q[n]]''] ]; SetAttributes[ q, Listable ]; SetAttributes[ q, attr ]; u[n_]' := ut[n]'[t]; q[n_]' := qt[n]'[t]; q[n_]'' := qt[n]''[t]; If[ TrueQ[ $VersionNumber >= 3.], Format[ ut[n_][t_] ] := Subscript[u,n][t]; Format[ qt[n_][t_] ] := Subscript[q,n][t]; Format[ ut[n_]'[t_] ] := Subscript[u,n]'[t]; Format[ qt[n_]'[t_] ] := Subscript[q,n]'[t]; Format[ qt[n_]''[t_] ] := Subscript[q,n]''[t], (* else *) Format[ ut[n_][t_] ] := HoldForm[Subscripted[u[n]][t]]; Format[ qt[n_][t_] ] := HoldForm[Subscripted[q[n]][t]]; Format[ ut[n_]'[t_] ] := HoldForm[Subscripted[u[n]]'[t]]; Format[ qt[n_]'[t_] ] := HoldForm[Subscripted[q[n]]'[t]]; Format[ qt[n_]''[t_] ] := HoldForm[Subscripted[q[n]]''[t]] ]; u[n_][t_] := ut[n][t] q[n_][t_] := qt[n][t] u[n_]'[t_] := ut[n]'[t] q[n_]'[t_] := qt[n]'[t] q[n_]''[t_] := qt[n]''[t] SetAttributes[ ut, Listable ] SetAttributes[ qt, Listable ] SetAttributes[ ut, attr ]; SetAttributes[ qt, attr ]; udots[udofs_List] := Map[ ut[#]'[t]&, udofs ] udots[udof_] := ut[udof]'[t] qdots[qdofs_List] := Map[ qt[#]'[t]&, qdofs ] qdots[qdof_] := qt[qdof]'[t] States := Join[ q[qdofs], u[udofs] ]; PosCOM[ body_ /; MemberQ[Bodies, body] ] := PosCOM[ Inboard[body] ] + InbToJnt[body]+JntToJnt[body]-BodyToJnt[body]; PosCOM[(bodies_)?ListQ] := Simplify[Plus @@ (Mass /@ bodies*PosCOM /@ bodies)/ Plus @@ Mass /@ bodies] PosPnt[ point_, body_ ] := PosCOM[ body ] + point; Dist[ pnt1_, pnt2_ ] := Simplify[ NormV[ Transpose[ Level[ CastV[ pnt2 - pnt1 , ground ], 1]][[1]] ] ] Options[DPV] = { in->ground }; PV/: D[ PV[{c_,frme_,n_}], x_, opts___Rule ] := Module[ {bfrme, ncons}, bfrme = in /. {opts} /. Options[DPV]; ncons = NonConstants /. {opts} /. NonConstants->{}; Dprtl[ PV[{c,frme,n}], x, bfrme, NonConstants->ncons ] ]; PV/: D[ PV[vecs:{_,_,_}..], x_, opts___Rule] := Apply[ Plus, Map[ D[ PV[#], x, opts]&, {vecs} ] ]; PV/: Dprtl[ PV[{c_,frme_,n_}], t, bfrme_, opts___Rule] := Module[ {pv}, pv = Apply[ List, CastV[ PV[{c,frme,n}], bfrme ] ] /. {q[k_] -> qh[k], u[k_] -> uh[k]}; Apply[ PV, Transpose[ MapAt[ D[#,x,opts]&, Transpose[pv], 1] ] ] /. {qh[k_]->q[k],uh[k_]->u[k]} ]; PV/: Dprtl[ PV[{c_,frme_,n_}], x_, bfrme_, opts___Rule] := Module[ {pv}, pv = Apply[ List, CastV[ PV[{c,frme,n}], bfrme ] ]; Apply[ PV, Transpose[ MapAt[ D[#,x,opts]&, Transpose[pv], 1] ] ] ]; PV/: Dt[ PV[{c_,frme_,n_}], x_, opts___Rule] := Module[ {bfrme, cons}, bfrme = in /. {opts} /. Options[DPV]; cons = Constants /. {opts} /. Constants->{}; Dttl[ PV[{c,frme,n}], x, bfrme, Constants->cons ] ]; PV/: Dt[ PV[v:{_,_,_}..], x_, opts___Rule] := Apply[ Plus, Map[ Dt[ PV[#], x, opts]&, {v} ] ]; PV/: Dttl[ PV[{c_,frme_,n_}], t, bfrme_, opts___Rule] := Cross[ AngVel[frme,bfrme], PV[{c,frme,n}] ] + PV[{Dt[c,t,opts],frme,n}] /; ValueQ[AngVel[frme]] && ValueQ[AngVel[bfrme]] PV/: Dttl[ PV[{c_,frme_,n_}], x_, bfrme_, opts___Rule] := Module[ {pv}, pv = Apply[ List, CastV[ PV[{c,frme,n}], bfrme ] ]; Apply[ PV, Transpose[ MapAt[ Dt[#,x,opts]&, Transpose[pv], 1] ] ] ]; AngMom[ body_, pnt_ ] := Inertia[body].AngVel[body] + Cross[ Mass[body] (PosCOM[body] - pnt), VelCOM[body] ]; AngMom[ bodies_List, pnt_ ] := Apply[ Plus, Map[ AngMom[#, pnt]&, bodies ] ]; AngMom[ body_, pnt_, ref_ ] := Inertia[body].AngVel[body,ref] + Cross[ Mass[body] (PosCOM[body] - pnt), VelCOM[body,ref] ]; AngMom[ bodies_List, pnt_, ref_ ] := Apply[ Plus, Map[ AngMom[#, pnt, ref]&, bodies ] ]; LinMom[body_] := Mass[body]*VelCOM[body]; LinMom[(bodies_)?ListQ] := Simplify[Plus @@ LinMom /@ bodies]; PV/: D[ PV[ {c_, frme_, n_ }], u[d_] ] := PV[{ D[c,u[d]], frme, n}]; PV/: D[ PV[x:{_,_,_}..], u[d_] ] := Apply[Plus, Map[ D[PV[#], u[d]]&, {x}] ]; PrtVel[ expr_, n_?NumberQ ] := D[ expr, u[n] ]; PrtVel[ expr_, u[n_?NumberQ] ] := D[ expr, u[n] ]; ResltF[ body_ ] := Apply[ Plus, Map[ #[[1]]&, Force[body] ] ]; ResltT[ body_ ] := Apply[ Plus, Torque[body] ] + Apply[ Plus, Map[ Cross[ #[[2]], #[[1]] ] &, Force[body] ] ]; GActFrcC[ All ] := (* Simplify[ *) Apply[ Plus, Map[ GActFrc, Bodies ] ] (*]*); GActFrcC[ All, n_ ] := Apply[ Plus, Map[ GActFrc[#,n]&, Bodies] ]; GActFrcC[ body_, udof_ /; MemberQ[udofs,udof] ] := PrtVel[ AngVel[body], udof] . ResltT[body] + PrtVel[ VelCOM[body], udof] . ResltF[body] /. Join[replacesin,replacecos,replace2SC,replaceSC2] //ZF; GActFrcC[ body_ ] := Map[ GActFrc[ body, # ]&, udofs ]; SetAttributes[ GActFrcC, Listable ] AppFrc[ body_, force_, point_ ] := ( AppendTo[ Force[body], {force, point} ]; Length[ Force[body] ] ) AppTrq[ body_, torque_ ] := ( AppendTo[ Torque[body], torque ]; Length[ Torque[body] ] ) GInerFrcC[ All ] := CollectU[ Apply[ Plus, Outer[ GInerFrc, Bodies, udofs ] ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; (* Apply[ Plus, Map[ gInerFrc, Bodies ] ]; *) GInerFrcC[ All, n_ ] := CollectU[ Apply[ Plus, Map[ GInerFrc[#,n]&, Bodies ] ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrcC[ body_, udof_ /; MemberQ[udofs,udof] ] := Collect[ PrtVel[ AngVel[body], udof] . Tstar[body] + PrtVel[ VelCOM[body], udof] . Rstar[body] //. Join[replacesin,replacecos,replace2SC,replaceSC2], Join[ Map[(u[#])'&, udofs], Map[u[#]&, udofs] ] ] //ZF GInerFrcC[ body_ ] := CollectU[ Map[ GInerFrc[ body, # ]&, udofs ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrc0[ All ] := Apply[ Plus, Outer[ GInerFrc, Bodies, udofs ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; (* Apply[ Plus, Map[ GInerFrc, Bodies ] ]; *) GInerFrc0[ All, n_ ] := Apply[ Plus, Map[ GInerFrc[#,n]&, Bodies ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrc0[ body_, udof_ /; MemberQ[udofs,udof] ] := Collect[ PrtVel[ AngVel[body], udof] . Tstar[body] + PrtVel[ VelCOM[body], udof] . Rstar[body] //. Join[replacesin,replacecos,replace2SC,replaceSC2], Join[ Map[(u[#])'&, udofs], Map[u[#]&, udofs] ] ] //ZF GInerFrc0[ body_ ] := Map[ GInerFrc[ body, # ]&, udofs ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrcS[ All ] := CollectU[ Apply[ Plus, Outer[ GInerFrc, Bodies, udofs ] ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrcS[ All, n_ ] := CollectU[ Apply[ Plus, Map[ GInerFrc[#,n]&, Bodies ] ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrcS[ body_ ] := CollectU[ Map[ GInerFrc[ body, # ]&, udofs ] ] //. Join[replacesin,replacecos,replace2SC,replaceSC2]; GInerFrcS[ body_, udof_ /; MemberQ[udofs,udof] ] := Simplify[Collect[ PrtVel[ AngVel[body], udof] . Tstar[body] + PrtVel[ VelCOM[body], udof] . Rstar[body] //. Join[replacesin,replacecos,replace2SC,replaceSC2], Join[ Map[(u[#])'&, udofs], Map[u[#]&, udofs] ] ] ] //ZF SetAttributes[ GInerFrcC, Listable ] SetAttributes[ GInerFrcS, Listable ] SetAttributes[ CleanGIF, Listable ] SetAttributes[ CleanGIFS, Listable ] TstarC[body_] := (-AngAcc[body].Inertia[body] - Cross[ AngVel[body], Inertia[body] . AngVel[body] ]) //ZF Rstar[body_] := -Mass[body] AccCOM[body] SetAttributes[ TstarC, Listable ] SetAttributes[ Rstar, Listable ] CollectU[ x_ ] := Collect[ x, Join[ Map[(u[#])'&, udofs], Map[u[#]&, udofs] ] ]; upow2 := Union[ Flatten[ Outer[ Times, Map[u[#]&, udofs], Map[u[#]&, udofs] ] ] ] CleanGIF[ x_ ] := Apply[ Plus, Map[ Coefficient[Expand[x], #] # &, Join[ Map[(u[#])'&, udofs], upow2 ] ] ] CleanGIFS[ x_ ] := Apply[ Plus, Map[ Simplify[ Coefficient[Expand[x], #] ] # &, Join[ Map[(u[#])'&, udofs], upow2 ] ] ] Z[n_]:=Zz[n][t]; Format[Zz[n_][t_]]:=Subscript[Z,n]; Zee[x_ ut[n1_]'[t_]]:=Module[{},If[ LeafCount[x]>4,Z[addtozee[x]] ut[n1]'[t], x ut[n1]'[t] ]]; Zee[x_ ut[n1_][t_] ut[n2_][t_]]:=Module[{},If[ LeafCount[x]>4,Z[addtozee[x]] ut[n1][t]ut[n2][t], x ut[n1][t] ut[n2][t]]]; Zee[x_ ut[n1_][t_] ut[n1_][t_]]:=Module[{},If[ LeafCount[x]>4,Z[addtozee[x]] ut[n1][t]ut[n1][t], x ut[n1][t] ut[n1][t]]]; Zee[x_ ut[n1_][t_]]:=Module[{},If[ LeafCount[x]>4,Z[addtozee[x]] ut[n1][t], x ut[n1][t] ]] /;FreeQ[x,u[_Integer]]; Zee[PV[List[x_,y_,z_]]]:=PV[{Zee[Collect[x,Join[u[udofs],udots[udofs]]]],y,z}] Zee[PV[x_]]:= Transpose[ Map[Zee[Collect[#,Join[u[udofs],udots[udofs]]]]&,Transpose[x][[1]]], Drop[Transpose[x],1]]; Zee[x:Plus[__]]:=Map[Zee[Collect[#,Join[u[udofs],udots[udofs]]]]&,x]; Zz[n_]'[t]:=Dt[zees[[n]],t] addtozee[x_]:=Module[{},AppendTo[zees,x];Length[zees]] /; !MemberQ[zees,x]; addtozee[x_]:=Flatten[Position[zees,x]][[1]]/;MemberQ[zees,x]; Options[EOM] = {Simplify -> Off}; EOM[opts___] := Module[{},If[(Simplify/.{opts}/.Options[EOM])===On, (* then Simplify it *) Thread[ GActFrc[All] + GInerFrcS[All] == Table[0, {Length[udofs]} ] ], (* else regular equations *) Thread[ GActFrc[All] + GInerFrc[All] == Table[0, {Length[udofs]} ] ] ] ] (* EOM0 := Thread[ GActFrc[All] + GInerFrc0[All] == Table[0, {Length[udofs]} ] ] *) CollectE[ eom_ ] := Collect[eom[[1]], udots[udofs] ] == 0 CollectE[ eom_, x_ ] := Collect[eom[[1]], x ] == 0 SetAttributes[ CollectE, Listable ] SolveEqn[ eqn_, vars_ ] := Sort[ Apply[ Equal, Solve[ eqn, vars], {2}][[1]] ] RHS[ eqn_ ] := Map[#[[2]]&,eqn] AsmStateEqn[ Kinematics_, eom_ ] := RHS[ Join[ SolveEqn[ Kinematics, qdots[qdofs] ], SolveEqn[ eom, udots[udofs] ] ]] MassMatrix[ eom_ ] := Module[ {eqns,mass}, eqns = Thread[ eom, Equal]; eqns = eqns[[1]]-eqns[[2]]; mass = Transpose[Map[ Coefficient[ Expand[eqns], ut[#]'[t] ] &, udofs ]]; {-mass, (eqns /. Thread[udots[udofs]->Table[0,{Length[udofs]}]])} ] XDot[eom_, states_List] := Module[ {staterule = Thread[ Evaluate[ Join[q[qdofs],u[udofs]] ] -> Flatten[states] ], xdots = Join[Thread[q[qdofs]'],Thread[u[udofs]'] ]}, xdots /. Solve[ Flatten[{eom /. parms, Kinematics}] /. staterule, xdots][[1]] ] lowercasefunctions = {Sin -> sin, Cos -> cos, Tan -> tan, Power -> power}; Options[formtrigreplacements]={TrigReplacements\[Rule]{{Sin,"s"},{Cos, "c"},{Tan,"t"},{Sec,"sec"},{Csc,"csc"}}, ArgReplacements\[Rule]None }; (* trigSubs = formtrigreplacements[equations] takes in equations of any form, and returns a list of replacements for trig function calls with shorthand \ variables. The shortened variables are given as strings. For example, an equation M*g*Sin[q[1]]*Cos[q[2]]*Sin[q[1]-q[2]]* Sin[unknown] returns {Sin[q[1]]->"s1",Cos[q[2]]->"c2", Sin[q[1]-q[2]]->"s1m2"}. There are two optional arguments that can be given. ArgReplacements\[Rule]{{gamma,"g"},{alpha, "a"}} specifies additional trig arguments that can be shortened, and TrigReplacements\[Rule]{{Sin,"s"},{Cos, "c"}} specifies all the trig functions that can be shortened. The second element of each entry is a string containing the \ abbreviation. *) formtrigreplacements[eom_,opts___]:= Module[{trigreps,altargs,alltrig,trigreplaced,argreplacements,argtostrings, plusminustostrings,successfulones,trigreplacements,trigfncalls,trigSubs, reorder}, trigreps= TrigReplacements/.{opts}/.Options[formtrigreplacements]/.None\[Rule]{}; altargs= ArgReplacements/.{opts}/.Options[formtrigreplacements]/.None\[Rule]{}; (* pull out all the unique trig function calls of the form Sin[_]| Cos[_] etc., yielding a list like {Sin[q[1]],Cos[q[2]-q[3]]} *) alltrig= Union[Cases[eom,Apply[Alternatives,Map[#[_]&,Transpose[trigreps][[1]]]], Infinity]]; (* form a list of the trig functions replaced by their shortcuts, e.g. {Cos[q[1]],Sin[anything]} becomes {"c","s"} *) trigreplaced=Map[Head,alltrig]/.Apply[Rule,trigreps,{1}]; (* make a list of replacements, starting with q[n_]\[RuleDelayed]ToString[n], followed by alternative arg replacements if any (whether in vector or \ matrix form) e.g. {q[n_]\[RuleDelayed]ToString[n],gamma->"g"} *) argreplacements= Prepend[ Apply[Rule, Replace[altargs/.None\[Rule]{},x:{_,_String}\[Rule]{x}],{1}], q[n_]\[RuleDelayed]ToString[n]]; (* convert all arguments into their replacements, e.g. q[1] becomes "1" *) argstostrings=Map[#[[1]]&,alltrig]/.argreplacements; (* for cases where two arguments are summed, convert all plus and minus signs into "p" and "m" *) plusminustostrings= argstostrings/.(x_String - y_String)\[RuleDelayed](x <> "m" <> y)/.(x_String + y_String) \[RuleDelayed]( x <> "p" <> y); (* positions of the arguments that were successfully replaced *) successfulones=Position[plusminustostrings,_String,{1}]; (* splice together the trig function replacement (e.g. "s" for Sin) with the argument replacements, yielding something like {"s1","c2","s12"} *) trigreplacements= MapThread[ StringJoin,{Extract[trigreplaced,successfulones], Extract[plusminustostrings,successfulones]}]; (* and pull out the matching trig functions, effectively discarding the ones that couldn't be replaced, e.g. {Sin[q[1],Cos[q[2]],Sin[q[1]-q[2]]} *) (* sort them so that shorter strings go first, and after that they are alphabetical *) reorder= Ordering[trigreplacements,All, StringLength[#1]< StringLength[#2]||(StringLength[#1]===StringLength[#2] && OrderedQ[{#1,#2}])&]; trigfncalls=Extract[alltrig,successfulones]; (* Use the two lists to form a set of rules, {Sin[q[1]]\[Rule]"s1", Cos[q[2]]->"c2"} *) trigSubs= MapThread[ Rule[#1,#2]&,{trigfncalls[[reorder]],trigreplacements[[reorder]]}]] \!\(\* RowBox[{ RowBox[{"formqureplacements", ":=", RowBox[{"Module", "[", RowBox[{\({qSubs, uSubs, upSubs, quSubs}\), ",", RowBox[{ RowBox[{"qSubs", "=", RowBox[{ RowBox[{"(", RowBox[{ RowBox[{\(q[#1]\), "\[Rule]", RowBox[{"ToString", "[", InterpretationBox["\"\\"", StringForm[ "q`1`", #], Editable->False], "]"}]}], "&"}], ")"}], "/@", "qdofs"}]}], ";", RowBox[{"uSubs", "=", RowBox[{ RowBox[{"(", RowBox[{ RowBox[{\(u[#1]\), "\[Rule]", RowBox[{"ToString", "[", InterpretationBox["\"\\"", StringForm[ "u`1`", #], Editable->False], "]"}]}], "&"}], ")"}], "/@", "udofs"}]}], ";", RowBox[{"upSubs", "=", RowBox[{ RowBox[{"(", RowBox[{ RowBox[{ SuperscriptBox[\(u[#1]\), "\[Prime]", MultilineFunction->None], "\[Rule]", RowBox[{"ToString", "[", InterpretationBox["\"\\"", StringForm[ "u`1`dot", #], Editable->False], "]"}]}], "&"}], ")"}], "/@", "udofs"}]}], ";", \(quSubs = Join[qSubs, uSubs, upSubs]\)}]}], "]"}]}], ";"}]\) pSub = {(x_)^2 -> HoldForm[x*x]}; Options[ExportExpression]={qusubs\[Rule]Automatic,TrigSubs\[Rule]Automatic, psub->{(x_)^2\[Rule]HoldForm[x*x]}}; ExportExpression[Mx_?SymmetricQ, name_String,opts___]:= Module[{elementsreplaced,quSubs,trigSubs,pSub}, quSubs= qusubs/.{opts}/.Options[ExportExpression]/.Automatic\[Rule] formqureplacements; trigSubs= TrigSubs/.{opts}/.Options[ ExportExpression]/.None\[Rule]{}/.Automatic\[Rule] formtrigreplacements[Mx,opts]; pSub=psub/.{opts}/.Options[ExportExpression]; (* produce table of values, such as {{MM(1,1)=M*g*s12,MM(1,2)=c12},{MM(2,1)=c12,MM(2,2)=c2}} for symmetric matrices, where duplicate definitions are avoided *) elementsreplaced=Table[ StringReplace[ (* strip quotes *) (* expression of the form name(i,j) = rhs *) ToString[ StringForm["`1`(`2`,`3`) = `4`; ",name,i,j, If[i>j,StringForm["`1`(`2`,`3`)",name,j,i], CForm[Mx[[i, j]]/.trigSubs/.quSubs/.pSub]/.\ lowercasefunctions]]],"\""->""], {i,1,Dimensions[Mx][[1]]},{j,1,Dimensions[Mx][[2]]}]; (* output each row as a separate formatted output, with line breaks in between *) StringJoin[ Map[StringInsert[fmtString[#,opts],"\n",-1]&, Map[StringJoin,elementsreplaced]]]]; ExportExpression[Mx_List?MatrixQ, name_String,opts___]:= Module[{elementsreplaced,quSubs,trigSubs,pSub}, quSubs= qusubs/.{opts}/.Options[ExportExpression]/.Automatic\[Rule] formqureplacements; trigSubs= TrigSubs/.{opts}/.Options[ ExportExpression]/.None\[Rule]{}/.Automatic\[Rule] formtrigreplacements[Mx,opts]; pSub=psub/.{opts}/.Options[ExportExpression]; (* produce table of values, such as {{MM(1,1)=M*g*s12,MM(1,2)=c12},{MM(2,1)=c12,MM(2,2)=c2}} *) elementsreplaced=Table[ StringReplace[ (* strip quotes *) (* expression of the form name(i,j) = rhs *) ToString[ StringForm["`1`(`2`,`3`) = `4`; ",name,i,j, CForm[Mx[[i, j]]/.trigSubs/.quSubs/.pSub]/.\ lowercasefunctions]],"\""->""], {i,1,Dimensions[Mx][[1]]},{j,1,Dimensions[Mx][[2]]}]; (* output each row as a separate formatted output, with line breaks in between *) StringJoin[ Map[StringInsert[fmtString[#,opts],"\n",-1]&, Map[StringJoin,elementsreplaced]]]]; ExportExpression[rhs_List?VectorQ, name_String,opts___]:= Module[{elementsreplaced,quSubs,trigSubs,pSub}, quSubs= qusubs/.{opts}/.Options[ ExportExpression]/.Automatic\[Rule]formqureplacements; trigSubs= TrigSubs/.{opts}/.Options[ ExportExpression]/.None\[Rule]{}/.Automatic\[Rule]\ formtrigreplacements[rhs,opts]; pSub=psub/.{opts}/.Options[ExportExpression]; (* produce list of values, such as {rhs(1)=M*g*s12,rhs(2)=c12} *) elementsreplaced=Table[ StringReplace[ (* strip quotes *) (* expression of the form name(i,j) = rhs *) ToString[ StringForm["`1`(`2`) = `3`; ",name,i, CForm[rhs[[i]]/.trigSubs/.quSubs/.pSub]/.lowercasefunctions]], "\""->""], {i,1,Length[rhs]}]; (* output each row as a separate formatted output, with line breaks in between *) StringJoin[ Map[StringInsert[fmtString[#,opts],"\n",-1]&,elementsreplaced]]]; ExportExpression[scalar_ /;Nor[VectorQ[scalar],MatrixQ[scalar] ],name_String , opts___]:=Module[{quSubs,trigSubs,pSub}, quSubs= qusubs/.{opts}/.Options[ ExportExpression]/.Automatic\[Rule]formqureplacements; trigSubs= TrigSubs/.{opts}/.Options[ ExportExpression]/.None\[Rule]{}/.Automatic\[Rule]\ formtrigreplacements[scalar,opts]; pSub=psub/.{opts}/.Options[ExportExpression]; fmtString[StringReplace[ToString[StringForm["`1` = `2`;",name, CForm[scalar/.trigSubs/.quSubs/.pSub]/.lowercasefunctions]], "\""->""],opts]]; Options[fmtString]={EndLine\[Rule]"\n",Continuator->"...",LineLength\[Rule]78, Delimiter->" "}; (* fmtString[ string] takes a long String argument and tries to break it into a bunch \ of lines with a Matlab (or other) continuator, so that the output is digestable code. Options include EndLine-> "\n" (end of line character), Continuator->"..." (Matlab or other line continuator), LineLength\[Rule]78 (max line length), Delimiter->" " (look for spaces, lists of delimiters are acceptable) *) fmtString[s_,opts___]:= Module[{maxlen,instring=s,outstring="",spaces,eol,continuator,delim}, eol=EndLine/.{opts}/.Options[fmtString]/.None->""; continuator=Continuator/.{opts}/.Options[fmtString]/.None->""; maxlen=LineLength/.{opts}/.Options[fmtString]; delim=Delimiter/.{opts}/.Options[fmtString]; While[StringLength[instring]>maxlen, spaces=StringPosition[ StringTake[instring, maxlen], delim]; If[ Length[spaces] < 1, spaces = StringPosition[ instring, delim]]; outstring = outstring <> StringTake[instring, Last[spaces][[1]]] <> continuator<> eol; instring = StringDrop[ instring, Last[spaces][[1]]]; ]; outstring = outstring <> instring ] Options[LinearTerms]={Simplifier\[Rule]Identity}; LinearTerms[ rhs_,terms_List,opts___ ] := Module[{linearstuff,simp},simp=Simplifier/.{opts}/.Options[LinearTerms]; linearstuff=Outer[Coefficient[#1,#2]&,rhs,terms]//simp]; Options[UnLinearTerms]={Simplifier\[Rule]Identity}; UnLinearTerms[rhs_,terms_,opts___]:= Module[{simp},simp=Simplifier/.{opts}/.Options[UnLinearTerms]; rhs/.Map[#\[Rule]0&,terms]//simp] LinearTerms[rhs_,term_,opts___]:=LinearTerms[rhs,{term},opts]; (* if only one term is used, turn it into a list *) Options[ExportCode]={OutputFile\[Rule]None,Forces\[Rule]None, Expressions\[Rule]None}; ExportCode[{mass_List?MatrixQ,rhs_List?VectorQ},opts___]:= Module[{ostrm,outputfile,quSubs,trigSubs,linearforceterms,forces, forcenames,myopts}, (* write a state-derivative function in Matlab, with mass matrix, right-hand side, and extras like forces and additional expressions *) outputfile=OutputFile/.{opts}/.Options[ExportCode]; forces=Forces/.{opts}/.Options[ExportCode]/.None\[Rule]{}; (* forces can be a list of forces, or a list of forces and force names, e.g. {{Rx,"Rx"},{Ry,"Ry"}} *) If[VectorQ[forces],forcenames=Map[SymbolName,forces], (* else *) If[MatrixQ[forces],{forces,forcenames}=Transpose[forces]]]; expressions= Replace[Expressions/.{opts}/.Options[ExportCode]/.None\[Rule]{}, x:{_,_String}\[Rule]{x}]; ostrm=If[outputfile=!=None, (* there is a file given *) Print["Opening file \"",outputfile,"\""]; Print["Current directory: ",Directory[]]; OpenWrite[outputfile], (* else *) $Output]; quSubs=formqureplacements; trigSubs=formtrigreplacements[{mass,rhs},opts]; myopts=Sequence[qusubs\[Rule]quSubs,TrigSubs\[Rule]trigSubs]; (* options generated automatically *) WriteString[ostrm,"function xdot = fstatederivative(t, x)\n\n"]; (* intended for ode45 call *) (* Print out some nice comment reminders *) WriteString[ostrm,"% Define constants\n\n"]; (* Print out the forces, if they exist *) WriteString[ostrm,"% Define forces: "]; If[Length[forces]>0,WriteString[ostrm,"[", StringDrop[StringJoin[Map[SymbolName[#]<>", "&,forces]],-2],"]'"]]; WriteString[ostrm,"\n\n% State assignments\n"]; WriteString[ostrm,(* export the state assignments, q1 = x(1), etc. *) StringJoin[Map[ToString[StringForm["q`1` = x(`1`); ",#]]&,qdofs]], "\n"];WriteString[ostrm, StringJoin[ Map[ToString[StringForm["u`1` = x(`2`); ", #,#+Length[qdofs]]]&, udofs]],"\n\n"]; (* export the trig assignments, s1 = sin(q1), etc. *) WriteString[ostrm,StringReplace[ (* strip out the quote marks *) StringJoin[ Map[ToString[ StringForm["`1` = `2`; ", #[[2]], CForm[#[[1]]/.quSubs/.{Sin\[Rule]"sin",Cos\[Rule]"cos", Tan\[Rule]"tan",Sec\[Rule]"sec", Csc\[Rule]"csc"}]]]&,trigSubs]],"\""->""],"\n\n"]; (* pre-allocate mass matrix, force matrix, and otherrhs *) WriteString[ostrm, ToString[StringForm["MM = zeros(`1`,`1`); ",Length[udofs]]]]; If[Length[forces]>0, (* a force matrix needed *) linearforceterms = LinearTerms[rhs,forces,opts]; WriteString[ostrm, ToString[ StringForm["FF = zeros(`1`,`2`); ",Length[udofs], Dimensions[linearforceterms][[2]] ]]]]; WriteString[ostrm, ToString[StringForm["rhs = zeros(`1`,1);\n\n",Length[udofs]] ]]; (* export something of the form{MM,"MM"} *) WriteString[ostrm,"% Mass Matrix\n", ExportExpression[mass,"MM",myopts,opts],"\n"]; If[Length[forces]>0, (* if there are listed forces, export the linear terms for them *) WriteString[ostrm,"% Force Matrix\n", ExportExpression[linearforceterms,"FF",myopts,opts],"\n"]; WriteString[ostrm,"% other righthand side terms\n", ExportExpression[UnLinearTerms[rhs,forces],"rhs",myopts,opts], "\n"]; WriteString[ostrm,"rhs = rhs + FF*forces;\n"], (* else just give the whole rhs *) WriteString[ostrm,"% righthand side terms\n", ExportExpression[rhs,"rhs",myopts,opts],"\n"]]; WriteString[ostrm,"udot = MM\\rhs;\n"]; WriteString[ostrm, ToString[StringForm["xdot = [x(1:`1`); udot];",Length[qdofs]]],"\n"]; (* write out extra expressions, if any *) WriteString[ostrm, Apply[Sequence, Flatten[Map[{"\n",ExportExpression[#[[1]],#[[2]],myopts,opts], "\n"}&,expressions]]]]; If[outputfile=!=None,Print["Closing file \"",outputfile,"\""]; Close[ostrm]]; ]; ExportCode[eom_,opts___]:=ExportCode[MassMatrix[eom],opts]; NewModel[opts___] := Module[{gif}, (*Begin["DynWkbnch`"];*) qdofs={}; udofs={}; zees={}; Clear[ Parents, Kids, Tmtx, Kinematics, Inboard, VelCOM, VelJnt, AngVel, AccCOM, AccJnt, AngAcc, Force, Torque, Mass, Inertia, BodyToJnt, InbToJnt, JntToJnt, GInerFrc, GActFrc, Tstar, CML]; Parents[ground] = {}; Kids[ground] = {}; Tmtx[{}] = IdentityMatrix[3]; Frames = {ground}; Bodies = {}; ground[n_?CoordQ] := PV[{1,ground,n}]; Kinematics = {}; Nonholonomic = {}; Mass[(bodies_)?ListQ] := Mass /@ bodies; PosCOM[ ground ] = 0; AngVel[ ground ] = PV[{0,ground,1}]; AngVel[ frme1_, frme2_ ] := AngVel[frme1] - AngVel[frme2] /; ValueQ[AngVel[frme2]]; VelCOM[ ground ] = 0; VelCOM[(bodies_)?ListQ] := Simplify[Plus @@ (Mass /@ bodies*VelCOM /@ bodies)/ Plus @@ Mass /@ bodies]; AngAcc[ ground ] = 0; AccCOM[ ground ] = 0; Inboard[ ground ] = {}; Dofs[ ground ] = {{},{}}; ZF = If[(Zees/.{opts})===On,Zee,Identity]; If[(DotProductList/.{opts})===On, (* want to memorize dot products *) DotProduct[frame1_,coord1_,frame2_,coord2_]=. ]; gif = If[(Simplify/.{opts})===On, GInerFrcS, GInerFrcC]; GInerFrc[x__] := GInerFrc[x] = gif[x]; GActFrc[x__] := GActFrc[x] = GActFrcC[x]; Tstar[x__] := Tstar[x] = TstarC[x]; CML[from_,to_]:=Module[{frame1,frame2},{frame1,frame2}=Sort[{from,to}]; If[OrderedQ[{from,to}],Identity,Transpose][ CML[frame1,frame2]=CastMtx[frame1,frame2]]] (*End[ ];*) ] Options[ AddFrame ] = {Axis->{0,0,1}, Qdof->Automatic, Axis1->{0,0,1},Axis2->{0,1,0},Axis3->{0,0,1},Qdof1->Automatic, Qdof2->Automatic, Qdof3->Automatic, Qdof4->Automatic, Offset->{{1,0,0},{0,1,0},{0,0,1}}, QOffset->0, QOffsets->{0,0} }; AddFrame[frme_,basefrm_,Hinge, opts___Rule ] := Module[ {newqdof, z1, z2, z3}, {newqdof} = ReplaceDofs[ {Qdof} /. {opts} /. Options[ AddFrame ], qdofs ]; qdofs = Union[ qdofs, {newqdof} ]; Frames = Union[ Append[Frames, frme] ]; q0 = QOffset /. {opts} /. Options[ AddFrame ]; {z1,z2,z3} = ConvertList[Axis /. {opts} /. Options[ AddFrame ], basefrm] ; (* Now set up the transformation matrix between frames *) Tmtx[frme] = { {z1^2 (1-Cos[q[newqdof]-q0]) + Cos[q[newqdof]-q0], z1 z2 (1-Cos[q[newqdof]-q0]) + z3 Sin[q[newqdof]-q0], z1 z3 (1-Cos[q[newqdof]-q0]) - z2 Sin[q[newqdof]-q0]}, {z1 z2 (1-Cos[q[newqdof]-q0]) - z3 Sin[q[newqdof]-q0], z2^2 (1-Cos[q[newqdof]-q0]) + Cos[q[newqdof]-q0], z2 z3 (1-Cos[q[newqdof]-q0]) + z1 Sin[q[newqdof]-q0]}, {z1 z3 (1-Cos[q[newqdof]-q0]) + z2 Sin[q[newqdof]-q0], z2 z3 (1-Cos[q[newqdof]-q0]) - z1 Sin[q[newqdof]-q0], z3^2 (1-Cos[q[newqdof]-q0]) + Cos[q[newqdof]-q0]}}; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; {newqdof} ]; AddFrame[frme_,basefrm_,Fixed, opts___Rule] := Module[ {mtx, z1, z2, z3}, Frames = Union[ Append[Frames, frme] ]; (* Now set up the transformation matrix between frames *) Tmtx[frme] = ConvertOffset[ Offset, basefrm ] /. {opts} /. Options[AddFrame]; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; {} (* No generalized coordinate, so output is null *) ]; AddFrame[frme_,basefrm_,Slider, opts___Rule] := Module[ {mtx, z1, z2, z3}, Frames = Union[ Append[Frames, frme] ]; (* Now set up the transformation matrix between frames *) Tmtx[frme] = ConvertOffset[ Offset, basefrm ] /. {opts} /. Options[AddFrame]; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; {} (* No generalized coordinate, so output is null *) ]; AddFrame[frme_,basefrm_,UJoint, opts___Rule ] := Module[ {z11, z12, z13, z21, z22, z23, newqdofs, q1, q2}, {z11,z12,z13} = ConvertList[Axis1, basefrm] /. {opts} /. Options[ AddFrame ]; {z21,z22,z23} = ConvertList[Axis2, basefrm] /. {opts} /. Options[ AddFrame ]; newqdofs = {q1, q2} = ReplaceDofs[ {Qdof1, Qdof2}/. {opts} /. Options[ AddFrame ], qdofs ] ; qdofs = Union[ qdofs, newqdofs ]; Frames = Union[ Append[Frames, frme] ]; {q10,q20} = QOffset /. {opts} /. Options[ AddFrame ]; (* Now set up the transformation matrix between frames *) Tmtx[frme] = { {z21^2 (1-Cos[q[q2-q20]]) + Cos[q[q2-q20]], z21 z22 (1-Cos[q[q2-q20]]) + z23 Sin[q[q2-q20]], z21 z23 (1-Cos[q[q2-q20]]) - z22 Sin[q[q2-q20]]}, {z21 z22 (1-Cos[q[q2-q20]]) - z23 Sin[q[q2-q20]], z22^2 (1-Cos[q[q2-q20]]) + Cos[q[q2-q20]], z22 z23 (1-Cos[q[q2-q20]]) + z21 Sin[q[q2-q20]]}, {z21 z23 (1-Cos[q[q2-q20]]) + z22 Sin[q[q2-q20]], z22 z23 (1-Cos[q[q2-q20]]) - z21 Sin[q[q2-q20]], z23^2 (1-Cos[q[q2-q20]]) + Cos[q[q2-q20]]}} .{ {z11^2 (1-Cos[q[q1-q10]]) + Cos[q[q1-q10]], z11 z12 (1-Cos[q[q1-q10]]) + z13 Sin[q[q1-q10]], z11 z13 (1-Cos[q[q1-q10]]) - z12 Sin[q[q1-q10]]}, {z11 z12 (1-Cos[q[q1-q10]]) - z13 Sin[q[q1-q10]], z12^2 (1-Cos[q[q1-q10]]) + Cos[q[q1-q10]], z12 z13 (1-Cos[q[q1-q10]]) + z11 Sin[q[q1-q10]]}, {z11 z13 (1-Cos[q[q1-q10]]) + z12 Sin[q[q1-q10]], z12 z13 (1-Cos[q[q1-q10]]) - z11 Sin[q[q1-q10]], z13^2 (1-Cos[q[q1-q10]]) + Cos[q[q1-q10]]}}; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; newqdofs ]; AddFrame[frme_,basefrm_,Ball | SixDOF, opts___Rule ] := Module[ {newqdofs, q1, q2, q3, q4}, newqdofs = {q1,q2,q3,q4} = ReplaceDofs[ {Qdof1,Qdof2,Qdof3,Qdof4} /. {opts} /. Options[ AddFrame ], qdofs ]; qdofs = Union[ qdofs, newqdofs ]; Frames = Union[ Append[Frames, frme] ]; (* Now set up the transformation matrix between frames *) Tmtx[frme] = { {1 - 2 q[q2]^2 - 2 q[q3]^2, 2 (q[q1] q[q2] + q[q3] q[q4]), 2 (q[q3] q[q1] - q[q2] q[q4])}, {2 (q[q1] q[q2] - q[q3] q[q4]), 1 - 2 q[q3]^2 - 2 q[q1]^2, 2 (q[q2] q[q3] + q[q1] q[q4])}, {2 (q[q3] q[q1] + q[q2] q[q4]), 2 (q[q2] q[q3] - q[q1] q[q4]), 1 - 2 q[q1]^2 - 2 q[q2]^2}}; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; newqdofs ]; AddFrame[frme_,basefrm_,Gimbal, opts___Rule ] := Module[ {z11, z12, z13, z21, z22, z23, z31, z32, z33, newqdofs, q1, q2, q3}, {z11,z12,z13} = ConvertList[Axis1, basefrm] /. {opts} /. Options[ AddFrame ]; {z21,z22,z23} = ConvertList[Axis2, basefrm] /. {opts} /. Options[ AddFrame ]; {z31,z32,z33} = ConvertList[Axis3, basefrm] /. {opts} /. Options[ AddFrame ]; newqdofs = {q1, q2, q3} = ReplaceDofs[ {Qdof1, Qdof2, Qdof3 } /. {opts} /. Options[ AddFrame ], qdofs ] ; qdofs = Union[ qdofs, newqdofs ]; Frames = Union[ Append[Frames, frme] ]; (* Now set up the transformation matrix between frames *) Tmtx[frme] = { {z31^2 (1-Cos[q[q3]]) + Cos[q[q3]], z31 z32 (1-Cos[q[q3]]) + z33 Sin[q[q3]], z31 z33 (1-Cos[q[q3]]) - z32 Sin[q[q3]]}, {z31 z32 (1-Cos[q[q3]]) - z33 Sin[q[q3]], z32^2 (1-Cos[q[q3]]) + Cos[q[q3]], z32 z33 (1-Cos[q[q3]]) + z31 Sin[q[q3]]}, {z31 z33 (1-Cos[q[q3]]) + z32 Sin[q[q3]], z32 z33 (1-Cos[q[q3]]) - z31 Sin[q[q3]], z33^2 (1-Cos[q[q3]]) + Cos[q[q3]]}} . { {z21^2 (1-Cos[q[q2]]) + Cos[q[q2]], z21 z22 (1-Cos[q[q2]]) + z23 Sin[q[q2]], z21 z23 (1-Cos[q[q2]]) - z22 Sin[q[q2]]}, {z21 z22 (1-Cos[q[q2]]) - z23 Sin[q[q2]], z22^2 (1-Cos[q[q2]]) + Cos[q[q2]], z22 z23 (1-Cos[q[q2]]) + z21 Sin[q[q2]]}, {z21 z23 (1-Cos[q[q2]]) + z22 Sin[q[q2]], z22 z23 (1-Cos[q[q2]]) - z21 Sin[q[q2]], z23^2 (1-Cos[q[q2]]) + Cos[q[q2]]}} . { {z11^2 (1-Cos[q[q1]]) + Cos[q[q1]], z11 z12 (1-Cos[q[q1]]) + z13 Sin[q[q1]], z11 z13 (1-Cos[q[q1]]) - z12 Sin[q[q1]]}, {z11 z12 (1-Cos[q[q1]]) - z13 Sin[q[q1]], z12^2 (1-Cos[q[q1]]) + Cos[q[q1]], z12 z13 (1-Cos[q[q1]]) + z11 Sin[q[q1]]}, {z11 z13 (1-Cos[q[q1]]) + z12 Sin[q[q1]], z12 z13 (1-Cos[q[q1]]) - z11 Sin[q[q1]], z13^2 (1-Cos[q[q1]]) + Cos[q[q1]]}}; Evaluate[frme][n_?CoordQ] := PV[{1,frme,n}]; Parents[frme] = Append[ Parents[basefrm], frme]; Kids[frme] = {}; Kids[basefrm] = Append[ Kids[basefrm], frme]; newqdofs ]; Options[ AddBody ] = {Axis->{0,0,1}, Qdof->Automatic, Udof->Automatic, Udof1->Automatic, Udof2->Automatic, Udof3->Automatic,Udof4->Automatic,Udof5->Automatic, Udof6->Automatic, Qdof1->Automatic, Qdof2->Automatic,Qdof3->Automatic, Qdof4->Automatic, Qdof5->Automatic, Qdof6->Automatic, Qdof7->Automatic, Axis1->{0,0,1}, Axis2->{0,1,0},Axis3->{0,0,1}, Taxis->{1,0,0}, TAxis1->{1,0,0},TAxis2->{0,1,0},TAxis3->{0,0,1}, BodyToJnt->0, InbToJnt->0, JntToJnt->0,Mass->0, Inertia->0, Frme->Automatic, Basefrm->Automatic, RelativeTo->Automatic}; AddBody[ body_, inboard_, Hinge, opts___Rule ] := Module[ {BtJ, ItJ, newqdof, newudof, frme, basefrm, axis}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; {newqdof} = AddFrame[ frme, basefrm, Hinge, opts ]; {newudof} = ReplaceDofs[{Udof} /. {opts} /. Options[ AddBody ], udofs ]; udofs = Union[ udofs, {newudof} ]; axis = ConvertVec[Axis, basefrm] /. {opts} /. Options[ AddFrame ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; {BtJ, ItJ, Mass[body], Inertia[body]} = {BodyToJnt, InbToJnt, Mass, ConvertInertia[Inertia,frme]} /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; AppendTo[ Kinematics, u[newudof] == qt[newqdof]'[t] ]; Kinematics = Union[Kinematics]; AngVel[ body ] = AngVel[ basefrm ] + u[newudof] * axis; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], -BtJ ] //ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ] //ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {{newqdof}, {newudof}} ]; AddBody[ body_, inboard_, Fixed, opts___Rule ] := Module[ {BtJ, ItJ, frme, basefrm, taxis}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; AddFrame[ frme, basefrm, Fixed, opts ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; AngVel[ body ] = AngVel[ inboard ]; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], -BtJ ] //ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ] //ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {{}, {}} ]; AddBody[ body_, inboard_, Slider, opts___Rule ] := Module[ {BtJ, ItJ, newqdof, newudof, frme, basefrm, taxis}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; {newqdof} = ReplaceDofs[{Qdof} /. {opts} /. Options[ AddBody ], qdofs]; qdofs = Union[ qdofs, {newqdof} ]; AddFrame[ frme, basefrm, Fixed, opts ]; {newudof} = ReplaceDofs[{Udof} /. {opts} /. Options[ AddBody ], udofs]; udofs = Union[ udofs, {newudof} ]; taxis = ConvertVec[TAxis, basefrm] /. {opts} /. Options[ AddFrame ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = q[newqdof] taxis; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; AppendTo[ Kinematics, u[newudof] == qt[newqdof]'[t] ]; Kinematics = Union[Kinematics]; AngVel[ body ] = AngVel[ inboard ]; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; (*VelJnt2[ body ] = VelJnt[ body ] + Cross[ AngVel[ body ], q[newqdof] taxis ] + u[newudof] taxis; VelCOM[ body ] = VelJnt2[ body ] + Cross[ AngVel[body], -BtJ ]//ZF;*) VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], q[newqdof] taxis - BtJ ] + u[newudof] taxis //ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; (*AccJnt2[ body ] = AccJnt[ body] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], q[newqdof] taxis ] ] + Cross[ AngAcc[inboard], q[newqdof] taxis ] + u[newudof]' taxis + 2 Cross[ AngVel[body], u[newudof] taxis ]; AccCOM[ body ] = AccJnt2[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ]//ZF;*) AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], q[newqdof] taxis - BtJ ] ] + Cross[ AngAcc[body], q[newqdof] taxis - BtJ ] + u[newudof]' taxis + 2 Cross[ AngVel[body], u[newudof] taxis ] //ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {{newqdof}, {newudof}} ]; AddBody[ body_, inboard_, UJoint, opts___Rule ] := Module[ {BtJ, ItJ, newqdofs, newudofs, q1, q2, u1, u2, frme, basefrm, axis1, axis2}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; newqdofs = {q1, q2} = AddFrame[ frme, basefrm, UJoint, opts ]; newudofs = {u1, u2} = ReplaceDofs[{Udof1, Udof2} /. {opts} /. Options[ AddBody ], udofs]; udofs = Union[ udofs, newudofs ]; axis1 = ConvertVec[Axis1, basefrm] /. {opts} /. Options[ AddFrame ]; axis2 = ConvertVec[Axis2, frme] /. {opts} /. Options[ AddFrame ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; AppendTo[ Kinematics, u[u1] == qt[q1]'[t] ]; AppendTo[ Kinematics, u[u2] == qt[q2]'[t] ]; Kinematics = Union[Kinematics]; AngVel[ body ] = AngVel[ inboard ] + u[u1] axis1 + u[u2] axis2; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], -BtJ ]//ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ]//ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {newqdofs, newudofs} ]; AddBody[ body_, inboard_, Gimbal, opts___Rule ] := Module[ {BtJ, ItJ, newqdofs, newudofs, q1, q2, q3, u1, u2, u3, frme, basefrm, axis1, axis2, axis3}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; newqdofs = {q1, q2, q3} = AddFrame[ frme, basefrm, Gimbal, opts ]; newudofs = {u1, u2, u3} = ReplaceDofs[{Udof1, Udof2, Udof3} /. {opts} /. Options[ AddBody ], udofs]; udofs = Union[ udofs, newudofs ]; axis1 = ConvertVec[Axis1, basefrm] /. {opts} /. Options[ AddFrame ]; axis2 = ConvertVec[Axis2, basefrm] /. {opts} /. Options[ AddFrame ]; axis3 = ConvertVec[Axis3, basefrm] /. {opts} /. Options[ AddFrame ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; AppendTo[ Kinematics, u[u1] == qt[q1]'[t] ]; AppendTo[ Kinematics, u[u2] == qt[q2]'[t] ]; AppendTo[ Kinematics, u[u3] == qt[q3]'[t] ]; Kinematics = Union[Kinematics]; AngVel[ body ] = AngVel[ inboard ] + u[u1] axis1 + u[u2] axis2 + u[u3] axis3; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], -BtJ ]//ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ]//ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {newqdofs, newudofs} ]; AddBody[ body_, inboard_, Ball, opts___Rule ] := Module[ {BtJ, ItJ, q1, q2, q3, q4, u1, u2, u3, frme, basefrm, newqdofs, newudofs,Euler}, frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; newqdofs = {q1,q2,q3,q4} = AddFrame[ frme, basefrm, Ball, opts ]; newudofs = {u1, u2, u3} = ReplaceDofs[ {Udof1, Udof2, Udof3} /. {opts} /. Options[ AddBody ], udofs]; udofs = Union[ udofs, newudofs ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; Euler = {{ q[q4], -q[q3], q[q2]}, { q[q3], q[q4], -q[q1]}, {-q[q2], q[q1], q[q4]}, {-q[q1], -q[q2], -q[q3]}}; AppendTo[ Kinematics,Thread[ {qt[q1]'[t], qt[q2]'[t], qt[q3]'[t], qt[q4]'[t]} == 1/2 Euler . {u[u1], u[u2], u[u3]},List] ]; Kinematics = Union[Flatten[Kinematics]]; AngVel[ body ] = AngVel[ inboard ] + PV[{u[u1],frme,1},{u[u2],frme,2}, {u[u3],frme,3}]; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelCOM[ body ] = VelJnt[ body ] + Cross[ AngVel[body], -BtJ ]//ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccCOM[ body ] = AccJnt[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ]//ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {newqdofs, newudofs} ]; AddBody[ body_, inboard_, SixDOF, opts___Rule ] := Module[ {BtJ, ItJ, q1, q2, q3, q4, q5, q6, q7, u1, u2, u3, u4, u5, u6, frme, basefrm, newqdofs, newudofs, taxis1, taxis2, taxis3, Euler}, newqdofs = {q1,q2,q3,q4,q5,q6,q7} = ReplaceDofs[ {Qdof1,Qdof2,Qdof3,Qdof4,Qdof5,Qdof6,Qdof7} /. {opts} /. Options[ AddBody ], qdofs]; If[ Length[#] > 0, Message[AddBody::repeatDOF, #] ] & [ Intersection[newqdofs, qdofs] ]; qdofs = Union[ qdofs, newqdofs ]; frme = Frme /. {opts} /. Options[ AddBody ]; If[ frme == Automatic, frme = body ]; basefrm = RelativeTo /. {opts} /. Options[ AddBody ]; If[ basefrm == Automatic, basefrm = inboard ]; AddFrame[ frme, basefrm, Ball, Qdof1->q4, Qdof2->q5, Qdof3->q6, Qdof4->q7, opts ]; {taxis1, taxis2, taxis3} = ConvertVec[#,basefrm]& /@ {TAxis1, TAxis2, TAxis3} /. {opts} /. Options[ AddBody ]; newudofs = {u1, u2, u3, u4, u5, u6} = ReplaceDofs[ {Udof1, Udof2, Udof3, Udof4, Udof5, Udof6} /. {opts} /. Options[ AddBody ], udofs]; udofs = Union[ udofs, newudofs ]; (* Handle the bodies *) AppendTo[ Bodies, body ]; Inboard[body] = inboard; BtJ = BodyToJnt /. {opts} /. Options[ AddBody ]; ItJ = InbToJnt /. {opts} /. Options[ AddBody ]; BodyToJnt[body] = BtJ; InbToJnt[body] = ItJ; JntToJnt[body] = 0; Mass[body] = Mass /. {opts} /. Options[ AddBody ]; Inertia[body] = ConvertInertia[Inertia,frme] /. {opts} /. Options[ AddBody ]; Euler = {{ q[q7], -q[q6], q[q5]}, { q[q6], q[q7], -q[q4]}, {-q[q5], q[q4], q[q7]}, {-q[q4], -q[q5], -q[q6]}}; AppendTo[ Kinematics, {qt[q4]'[t], qt[q5]'[t], qt[q6]'[t], qt[q7]'[t]} == 1/2 Euler . {u[u4], u[u5], u[u6]} ]; Kinematics = Union[Kinematics]; AngVel[ body ] = AngVel[ inboard ] + PV[{u[u4],frme,1},{u[u5],frme,2}, {u[u6],frme,3}]; VelJnt[ body ] = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ ]; VelJnt2[ body ] = VelJnt[ body ] + Cross[ AngVel[ inboard ], q[q1] taxis1 + q[q2] taxis2 + q[q3] taxis3 ] + u[u1] taxis1 + u[u2] taxis2 + u[u3] taxis3; VelCOM[ body ] = VelJnt2[ body ] + Cross[ AngVel[body], -BtJ ]//ZF; AngAcc[ body ] = Dt[ AngVel[body], t]; AccJnt[ body ] = AccCOM[ inboard ] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], ItJ ] ] + Cross[ AngAcc[inboard], ItJ ]; AccJnt2[ body ] = AccJnt[ body] + Cross[ AngVel[inboard], Cross[ AngVel[inboard], q[q1] taxis1 + q[q2] taxis2 + q[q3] taxis3 ] ] + Cross[ AngAcc[inboard], q[q1] taxis1 + q[q2] taxis2 + q[q3] taxis3 ] + u[u1]' taxis1 + u[u2]' taxis2 + u[u3]' taxis3 + 2 Cross[ AngVel[inboard], u[u1] taxis1 + u[u2] taxis2 + u[u3] taxis3 ]; AccCOM[ body ] = AccJnt2[ body ] + Cross[ AngVel[body], Cross[ AngVel[body], -BtJ ] ] + Cross[ AngAcc[body], -BtJ ]//ZF; Force[ body ] = {}; Torque[ body ] = {}; Dofs[ body ] = {newqdofs, newudofs} ]; CloseLoop[ body_, inboard_, Hinge, opts___Rule ] := Module[ {BtJ, ItJ, axis, velloopjnt1, velloopjnt2, joint, vector1, vector2, angvel, constraints}, axis = ConvertVec[Axis, body] /. {opts} /. Options[ AddFrame ]; {BtJ, ItJ} = {BodyToJnt, InbToJnt} /. {opts} /. Options[ AddBody ]; Loop[body] = {inboard, BtJ, ItJ}; velloopjnt1 = VelCOM[ body ] + Cross[ AngVel[body], BtJ]; velloopjnt2 = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ]; joint = velloopjnt1 - velloopjnt2; angvel = AngVel[body] - AngVel[inboard]; {vector1,vector2} = If[ PrincipalAxisQ[ ConvertList[axis,ground] ], ConvertVec[ NullSpace[ {ConvertList[axis,ground]} ], ground], ConvertVec[ NullSpace[ {ConvertList[axis,c]} ] ], ground]; constraints = Map[ # == 0 &, Select[ { joint . axis, joint . vector1, joint . vector2, angvel . vector1, angvel . vector2 }, # =!= 0 & ] ] /. Join[replacesin,replacecos,replace2SC,replaceSC2, replaceUs]; Nonholonomic = Join[ Nonholonomic, constraints ]; (* Return the number of constraints introduced *) Length[ constraints ] ]; CloseLoop[ body_, inboard_, Fixed, opts___Rule ] := Module[ {BtJ, ItJ, axis, velloopjnt1, velloopjnt2, joint, vector1, vector2, angvel, constraints}, axis = ConvertVec[Axis, body] /. {opts} /. Options[ AddFrame ]; {BtJ, ItJ} = {BodyToJnt, InbToJnt} /. {opts} /. Options[ AddBody ]; Loop[body] = {inboard, BtJ, ItJ}; velloopjnt1 = VelCOM[ body ] + Cross[ AngVel[body], BtJ]; velloopjnt2 = VelCOM[ inboard ] + Cross[ AngVel[inboard], ItJ]; joint = velloopjnt1 - velloopjnt2; angvel = AngVel[body] - AngVel[inboard]; {vector1,vector2} = If[ PrincipalAxisQ[ ConvertList[axis,inboard] ], NullSpace[ {ConvertList[axis,inboard]} ], NullSpace[ {ConvertList[axis,body]} ] ]; vector1 = ConvertVec[ vector1, body ]; vector2 = ConvertVec[ vector2, body ]; constraints = Map[ # == 0 &, Select[ { joint . vector1, joint . vector2, joint . vector3, angvel . vector1, angvel . vector2, angvel . vector3}, # =!= 0 & ] ]; Nonholonomic = Join[ Nonholonomic, constraints ]; (* Return the number of constraints introduced *) Length[ constraints ] ]; EndPackage[ ];