--- a +++ b/Application/Examples/BikeModel/Model/JointsAndDrivers.any @@ -0,0 +1,517 @@ +// Aliases for convenient referencing +AnyFolder &BikeParameters = Main.BikeParameters; +AnyFolder &JntPos=..Mannequin.Posture; +AnyFolder &JntVel=..Mannequin.PostureVel; + + /** Support both ankles in pronation/supination. This alleviates problems + due to insufficient muscles in the ankle region */ + AnyReacForce SubTalarEversion={ + AnyKinMeasure& Right = Main.HumanModel.BodyModel.Interface.Right.SubTalarEversion; + AnyKinMeasure& Left = Main.HumanModel.BodyModel.Interface.Left.SubTalarEversion; + }; + +#if BM_ARM_RIGHT != ON & BM_ARM_LEFT != ON + +/// Support of the upper body to compensate for lack of arms resting on the handlebar +AnyReacForce Thoraxsupport = { + AnyKinLinear lin = { + AnyRefFrame &Thorax = Main.HumanModel.BodyModel.Trunk.SegmentsThorax.ThoraxSeg; + }; + AnyKinRotational rot = { + AnyRefFrame &Thorax = Main.HumanModel.BodyModel.Trunk.SegmentsThorax.ThoraxSeg; + Type = RotAxesAngles; + }; +}; + +#endif + +Main.HumanModel.BodyModel.Trunk.SegmentsLumbar.PelvisSeg = +{ + AnyRefNode HipJoints_Midpoint = + { + sRel = 0.5*(.Left.HipJoint.sRel + .Right.HipJoint.sRel ); + }; +}; + +/** Keep the pelvis segment on the saddle. Reactions are off. They are handled by +a contact condition instead. */ +AnyKinEq SaddleLinear = { + AnyKinLinear SaddleJointLin = { + AnyRefNode &Sacrum= ....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.HipJoints_Midpoint; + AnyRefNode & Saddle = ....BikeModel.BikeFrame.SaddlePos; + }; + Reaction.Type={Off,Off,Off}; +}; + +/// Constrain the pelvis segment from rotation on the saddle. +AnyKinEqSimpleDriver SaddleRotation = { + AnyKinRotational SaddleJointRot = { + AnyRefNode & Sacrum= ....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.HipJoints_Midpoint; + AnyRefNode & Saddle = ....BikeModel.BikeFrame.SaddlePos; + Type=RotAxesAngles; + }; + DriverPos = {0.10*pi,0,0}; + DriverVel = {0.0,0,0}; + //Setting Reaction.Type=1 means that the joint will generate the moment it will + //need to balance this DOF, setting it to 0 which is default will mean that muscles + //forces will be used for balancing the joint + Reaction.Type = {Off,Off,Off}; +}; + +//------------------------------------------------------- +// Drivers - spine +//------------------------------------------------------- +// +//Neck driver +AnyKinEqSimpleDriver NeckJntDriver = { + AnyKinMeasure& lb = ...BodyModel.Interface.Trunk.SkullThoraxLateralBending; + AnyKinMeasure& ar = ...BodyModel.Interface.Trunk.SkullThoraxRotation; + AnyKinMeasure& fe = ...BodyModel.Interface.Trunk.SkullThoraxFlexion; + DriverPos = pi/180*{.JntPos.NeckLateralBending, .JntPos.NeckRotation, -1*.JntPos.NeckExtension}; + DriverVel = pi/180*{.JntPos.NeckLateralBending, .JntVel.NeckRotation, -1*.JntVel.NeckExtension}; + Reaction.Type = {Off, Off, Off}; +}; + +//Thorax pelvis rotation driver +AnyKinEqSimpleDriver PostureDriver ={ + AnyKinMeasure& Ref2 = ...BodyModel.Interface.Trunk.PelvisThoraxLateralBending; + AnyKinMeasure& Ref3 = ...BodyModel.Interface.Trunk.PelvisThoraxRotation; + AnyKinMeasure& Ref1 = ...BodyModel.Interface.Trunk.PelvisThoraxExtension; + + DriverPos = pi/180*{ + .JntPos.PelvisThoraxLateralBending, + .JntPos.PelvisThoraxRotation, + .JntPos.PelvisThoraxExtension + }; + DriverVel = pi/180*{ + .JntVel.PelvisThoraxLateralBending, + .JntVel.PelvisThoraxRotation, + .JntVel.PelvisThoraxExtension + }; + Reaction.Type = {Off,Off,Off}; +}; + +AnyFolder Right={ + // This is the joint connecting the foot to the pedal. It simply + // specifies that the linear distances between the two must be zero + + AnySphericalJoint PedalFootJoint = { + AnyRefNode &ref1= ....BikeModel.Crank.RPedal; + AnyRefNode &ref2 = ...ModelFolder.Right.Leg.Seg.Foot.PedalConnectNode; + #include "<ANYBODY_PATH_MODELUTILS>/JointReactions/DrawJointReactions.any" + AnyFolder &DrawRef=Main.DrawSettings; + }; + AnySeg& Ref_Foot = ..ModelFolder.Right.Leg.Seg.Foot; + Ref_Foot = + { + AnyRefNode PedalConnectNode = + { + sRel = .Metatarsal2Contact.sRel; + ARel = .AnatomicalFrame.ARel; + AnyDrawRefFrame drw = + { + ScaleXYZ = 0.1 * {1,1,1}; + RGB = {1,1,0}; + }; + }; + }; + AnyKinEq FootPedal = { + AnyKinRotational rot = { + AnyRefFrame &Pedal = .....BikeModel.Crank.RPedal; + //AnyRefFrame &Foot = ....ModelFolder.Right.Leg.Seg.Foot.GroundJoint; + AnyRefFrame &Foot = ....ModelFolder.Right.Leg.Seg.Foot.PedalConnectNode; + Type = RotAxesAngles; + + //Foot={AnyDrawRefFrame drw={};}; + }; + MeasureOrganizer = {2}; + }; + + + /** Datum segment for alignment of the knee. This segment passes through the hip +and the ankle and is otherwise directed forward. This creates a reasonable movement +of the knee when the pedal width is increased. */ + AnySeg Datum = { + Mass=0; + Jii = {0,0,0}; +// AnyDrawRefFrame drw = { +// ScaleXYZ = {1,1,1}/5; +// }; + r0 = {-1.062681e-015, -2.029448e-010, 1.08}; + AnyRefNode Ankle = { + sRel = {0,-1,0}; + }; + }; + + // Control rotations of the datum that are not governed by + // the contacts with the ankle joint + AnyKinEqSimpleDriver DatumRot = { + AnyKinRotational rot = { + AnyRefFrame &Datum = ..Datum; + Type = RotVector; + }; + MeasureOrganizer = {1,2}; + DriverPos = {-7,0}*pi/180; + DriverVel = {0,0}; + }; + + // Create a point near the greater trochanter for fixing + // the datum + Main.HumanModel.BodyModel.Right.Leg.Seg.Thigh = { + AnyRefNode DatumPoint = { + sRel = .Piriformis1Node.sRel; + //AnyDrawRefFrame drw = {ScaleXYZ = {1,1,0}/5;}; + }; + }; + + // Spherical joint between the datum and the hip joint + AnySphericalJoint DatumJoint1 = { + AnyRefFrame &Thigh = Main.Model.BodyModel.Right.Leg.Seg.Thigh.DatumPoint; + AnyRefFrame &Datum = .Datum; + }; + + // The datum must touch the ankle + AnyKinEq AnkleContact = { + AnyKinLinear lin = { + AnyRefFrame &Ankle = Main.Model.BodyModel.Right.Leg.Seg.Shank.AnkleJoint; + AnyRefFrame &Datum = ..Datum.Ankle; + Ref = 1; + }; + MeasureOrganizer = {2}; + }; + + // The knee must touch the datum + AnyKinEq KneeDatum = { + AnyKinLinear lin = { + AnyRefFrame &Datum = ..Datum; + AnyRefFrame &Knee = ....ModelFolder.Right.Leg.Seg.Thigh.KneeJoint; + Ref = 0; + }; + MeasureOrganizer = {2}; + Reaction.Type = {Off}; + }; + + // This measure is for controlling the rotation. + AnyKinRotational PedalJointRot = { + AnyRefFrame &Ground = ....BikeModel.BikeFrame; + AnyRefNode &Foot = ...ModelFolder.Right.Leg.Seg.Foot.PedalConnectNode; + Type=RotAxesAngles; + }; + + AnyKinMeasureOrg FootRMovement = { + AnyKinRotational &GroundFoot = .PedalJointRot; + MeasureOrganizer = {0}; // Standard rotational order = z,y,x = 0,1,2 + }; + + AnyKinEqFourierDriver PedalRRotationFourier ={ + AnyKinMeasure& Jnt = .FootRMovement; + Type = CosSin; + AnyVar a0 = -0.3797-0.5*pi; + AnyVar a1 = -0.0205; + AnyVar b1 = 0.3393; + AnyVar a2 = 0.004906; + AnyVar b2 = 0.03654; + AnyVar w = 7.681; + Freq = ..BikeParameters.Cadence/60; + A = { {a0+0.5*pi,a1,a2} }; + B = {{0,b1,b2} }; + Reaction.Type = {Off}; + }; +}; + +AnyFolder Left = { + + // This is the joint connecting the foot to the pedal. It simply + // specifies that the linear distances between the two mush be zero + /* AnyKinLinear PedalJointLin = { + AnyRefNode &Foot = ...ModelFolder.Left.Leg.Seg.Foot.GroundJoint; + AnyRefNode &PedalR= ....BikeModel.Crank.LPedal; + }; + + AnyKinEq PedalJointLinConstraints = { + AnyKinLinear &ref= .PedalJointLin; + }; + */ + + AnySphericalJoint PedalFootJoint = { + AnyRefNode &ref1= ....BikeModel.Crank.LPedal; + AnyRefNode &ref2 = ...ModelFolder.Left.Leg.Seg.Foot.PedalConnectNode; + // AnyDrawRefFrame Drw={}; //this lines adds a visible coordinate system in the ref2 node, and since the ref2 node points at the groundjoint at the foot it will display it there .. you could also have inserted it directly where the GroundJointNode was created + #include "<ANYBODY_PATH_MODELUTILS>/JointReactions/DrawJointReactions.any" + AnyFolder &DrawRef=Main.DrawSettings; + }; + AnySeg& Ref_Foot = ..ModelFolder.Left.Leg.Seg.Foot; + Ref_Foot = + { + AnyRefNode PedalConnectNode = + { + sRel = .Metatarsal2Contact.sRel; + ARel = .AnatomicalFrame.ARel; + AnyDrawRefFrame drw = + { + ScaleXYZ = 0.1 * {1,1,1}; + RGB = {1,0,0}; + }; + }; + }; + AnyKinEq FootPedal = { + AnyKinRotational rot = { + AnyRefFrame &Pedal = .....BikeModel.Crank.LPedal; + AnyRefFrame &Foot = ....ModelFolder.Left.Leg.Seg.Foot.PedalConnectNode; + Type = RotAxesAngles; + }; + MeasureOrganizer = {2}; + + }; + +// AnyReacForce FootPedalReac = { +// AnyKinMeasureOrg Org = { +// AnyKinRotational rot = { +// AnyRefFrame &Pedal = ......BikeModel.Crank.LPedal; +// AnyRefFrame &Foot = .....ModelFolder.Left.Leg.Seg.Foot.GroundJoint; +// Type = RotAxesAngles; +// }; +// MeasureOrganizer = {0}; +// }; +// }; + + /** Datum segment for alignment of the knee. This segment passes through the hip +and the ankle and is otherwise directed forward. This creates a reasonable movement +of the knee when the pedal width is increased. */ + AnySeg Datum = { + Mass=0; + Jii = {0,0,0}; +// AnyDrawRefFrame drw = { +// ScaleXYZ = {1,1,1}/5; +// }; + r0 = {-1.062681e-015, -2.029448e-010, 0.92}; + AnyRefNode Ankle = { + sRel = {0,-1,0}; + }; + }; + + // Control rotations of the datum that are not governed by + // the contacts with the ankle joint + AnyKinEqSimpleDriver DatumRot = { + AnyKinRotational rot = { + AnyRefFrame &Datum = ..Datum; + Type = RotVector; + }; + MeasureOrganizer = {1,2}; + DriverPos = {7,0}*pi/180; + DriverVel = {0,0}; + }; + + // Create a point near the greater trochanter for fixing + // the datum + Main.HumanModel.BodyModel.Left.Leg.Seg.Thigh = { + AnyRefNode DatumPoint = { + sRel = .Piriformis1Node.sRel; + //AnyDrawRefFrame drw = {ScaleXYZ = {1,1,1}/5;}; + }; + }; + + // Spherical joint between the datum and the hip joint + AnySphericalJoint DatumJoint1 = { + AnyRefFrame &Thigh = Main.Model.BodyModel.Left.Leg.Seg.Thigh.DatumPoint; + AnyRefFrame &Datum = .Datum; + }; + + // The datum must touch the ankle + AnyKinEq AnkleContact = { + AnyKinLinear lin = { + AnyRefFrame &Ankle = Main.Model.BodyModel.Left.Leg.Seg.Shank.AnkleJoint; + AnyRefFrame &Datum = ..Datum.Ankle; + Ref = 1; + }; + MeasureOrganizer = {2}; + }; + + // The knee must touch the datum + AnyKinEq KneeDatum = { + AnyKinLinear lin = { + AnyRefFrame &Datum = ..Datum; + AnyRefFrame &Knee = ....ModelFolder.Left.Leg.Seg.Thigh.KneeJoint; + Ref = 0; + }; + MeasureOrganizer = {2}; + Reaction.Type = {Off}; + }; + + // This measure is for controlling the rotation. + AnyKinRotational PedalJointRot = { + AnyRefFrame &Ground = ....BikeModel.BikeFrame; + AnyRefNode &Foot = ...ModelFolder.Left.Leg.Seg.Foot.PedalConnectNode; + Type=RotAxesAngles; + }; + + AnyKinMeasureOrg FootLMovement = { + AnyKinRotational &GroundFoot = .PedalJointRot; + MeasureOrganizer = {0}; // Standard rotational order = z,y,x = 0,1,2 + }; + + AnyKinEqFourierDriver PedalLRotationFourier ={ + AnyKinMeasure& Jnt = .FootLMovement; + Type = CosSin; + AnyVar a0 = -0.3797-0.5*pi; + AnyVar a1 = 0.0205; + AnyVar b1 = -0.3393; + AnyVar a2 = 0.004906; + AnyVar b2 = 0.03654; + AnyVar w = 7.681; + Freq = ..BikeParameters.Cadence/60; + A = { {a0+0.5*pi,a1,a2}}; + B = {{0,b1,b2}}; + Reaction.Type = {Off}; + }; +}; + + +#if BM_ARM_RIGHT == ON & BM_ARM_LEFT == ON + + Right ={ + + //------------------------------------------------------ + //This joint locks the hand to the bike + //------------------------------------------------------- + AnySphericalJoint HandToBike = { + AnyRefNode &ref1 = ....BikeModel.BikeFrame.RightHand; + AnyRefFrame &ref2= ...ModelFolder.Right.ShoulderArm.Seg.Glove; + #include "<ANYBODY_PATH_MODELUTILS>/JointReactions/DrawJointReactions.any" + AnyFolder &DrawRef=Main.DrawSettings; + + }; //Hand to ground + + AnyKinRotational HandToBikeRot = { + AnyRefNode &GroundNode = ....BikeModel.BikeFrame.RightHand; + AnyRefFrame &ref2= ...ModelFolder.Right.ShoulderArm.Seg.Glove; + Type=RotVector; + }; //Hand to ground + + + + AnyKinEqSimpleDriver HandToBikeDrv={ + AnyKinRotational &ref=.HandToBikeRot; +// DriverPos = {0.446805, 0.850377, 2.871550}; +// DriverVel={0,0,0}; +// Reaction.Type={On,On,On}; + DriverPos = {0.446805, 0.850377}; + DriverVel={0,0}; + Reaction.Type={On,On}; +MeasureOrganizer={0,1}; + }; + + + // ************************************ + // Drivers for the right arm + // ************************************ + + + //Sterno clavicular joint driver + AnyKinEqSimpleDriver SCDriverRight ={ + AnyKinMeasure& ref1 =....BodyModel.Interface.Right.SternoClavicularProtraction; + AnyKinMeasure& ref2 =....BodyModel.Interface.Right.SternoClavicularElevation; + DriverPos = pi/180*{ + ..JntPos.Right.SternoClavicularProtraction, + ..JntPos.Right.SternoClavicularElevation + }; + DriverVel = { + ..JntVel.Right.SternoClavicularProtraction, + ..JntVel.Right.SternoClavicularElevation + }; + Reaction.Type={Off,Off}; + }; + + AnyKinEqSimpleDriver WristDriverRight ={ + AnyKinMeasure& ref1 =....BodyModel.Interface.Right.WristFlexion; + AnyKinMeasure& ref2 =....BodyModel.Interface.Right.WristAbduction; + DriverPos = pi/180*{ + ..JntPos.Right.WristFlexion, + ..JntPos.Right.WristAbduction + }; + DriverVel = pi/180*{ + ..JntVel.Right.WristFlexion, + ..JntVel.Right.WristAbduction}; + Reaction.Type={Off,Off}; + };//Wrist driver + +};//Right + + + Left ={ + + + //------------------------------------------------------ + //This joint locks the hand to the bike + //------------------------------------------------------- + AnySphericalJoint HandToBike = { + AnyRefNode &ref1 = ....BikeModel.BikeFrame.LeftHand; + AnyRefFrame &ref2= ...ModelFolder.Left.ShoulderArm.Seg.Glove; + #include "<ANYBODY_PATH_MODELUTILS>/JointReactions/DrawJointReactions.any" + AnyFolder &DrawRef=Main.DrawSettings; + }; //Hand to bike + + AnyKinRotational HandToBikeRot = { + AnyRefNode &GroundNode = ....BikeModel.BikeFrame.LeftHand; + AnyRefFrame &ref2= ...ModelFolder.Left.ShoulderArm.Seg.Glove; + Type=RotVector; + }; //Hand to ground + + + + AnyKinEqSimpleDriver HandToBikeDrv={ + AnyKinRotational &ref=.HandToBikeRot; +// DriverPos = {-0.446805, -0.850377, 2.871550}; +// DriverVel={0,0,0}; +// Reaction.Type={On,On,On}; + DriverPos = {-0.446805, -0.850377}; + DriverVel={0,0}; + Reaction.Type={On,On}; + + MeasureOrganizer={0,1}; + }; + + + + // ************************************ + // Drivers for the left arm + // ************************************ + + //Sterno clavicular joint driver + AnyKinEqSimpleDriver SCDriverLeft ={ + AnyKinMeasure& ref1 =....BodyModel.Interface.Left.SternoClavicularProtraction; + AnyKinMeasure& ref2 =....BodyModel.Interface.Left.SternoClavicularElevation; + + DriverPos = pi/180*{ + ..JntPos.Left.SternoClavicularProtraction, + ..JntPos.Left.SternoClavicularElevation + }; + + DriverVel = pi/180*{ + ..JntVel.Left.SternoClavicularProtraction, + ..JntVel.Left.SternoClavicularElevation + }; + Reaction.Type={Off,Off}; + }; + + + //Wrist driver + AnyKinEqSimpleDriver WristDriverLeft ={ + AnyKinMeasure& ref1 =....BodyModel.Interface.Left.WristFlexion; + AnyKinMeasure& ref2 =....BodyModel.Interface.Left.WristAbduction; + DriverPos = pi/180*{ + ..JntPos.Left.WristFlexion, + ..JntPos.Left.WristAbduction}; + + DriverVel = pi/180*{ + ..JntVel.Left.WristFlexion, + ..JntVel.Left.WristAbduction}; + Reaction.Type={Off,Off}; + }; + + +};//left + +#endif + +