--- a +++ b/Application/Examples/ShoulderBag/Model/JointsAndDrivers.any @@ -0,0 +1,304 @@ +/* ---------------------------------------------------------------- +Drivers for a standing model: + +- The two feet are constrained forward/backward and vertically (x & y) +- The pelvis is constrained laterally (z) +- The CoM is constrained forward/backward and laterally above the origin. + +The remaining degrees of freedom (with the exception of a few shoulder +movements) are controlled by parameters in the mannequin.any file. + +The forces defined in the mannequin.any file are attached to the body +locations at the bottom of this file. + +John Rasmussen, March 5, 2004. +---------------------------------------------------------------- */ + +AnyFolder Drivers = { + + #if BM_LEG_RIGHT & (BM_LEG_MODEL == _LEG_MODEL_LEG_) + #include "RightLegDrivers.any" + #else + #endif + + + #if BM_LEG_LEFT & (BM_LEG_MODEL == _LEG_MODEL_LEG_) + #include "LeftLegDrivers.any" + #else + #endif + + + #if BM_LEG_RIGHT & BM_LEG_MODEL_IS_TLEM + #include "RightLegTDDrivers.any" + #else + #endif + + + #if BM_LEG_LEFT & BM_LEG_MODEL_IS_TLEM + #include "LeftLegTDDrivers.any" + #else + #endif + + + + // Aliases for convenient referencing + AnyFolder &JntPos=..Mannequin.Posture; + AnyFolder &JntVel=..Mannequin.PostureVel; + AnyFolder &Thorax = ..BodyModel.Trunk.SegmentsThorax.ThoraxSeg; + + AnyFolder &LegR = ..BodyModel.Right.Leg; + AnyFolder &LegL = ..BodyModel.Left.Leg; + + + //Constraint the collective CoM to be right above the GlobalRef + AnyKinEqSimpleDriver CoMDriver = { + AnyKinCoM CoM = { + AnyFolder &Body = Main.Model.BodyModel; + }; + MeasureOrganizer = {0,2}; // Only the x and z directions + DriverPos = {0.040147,0}; + DriverVel = {0,0}; + Reaction.Type = {Off,Off}; + }; + + AnyFolder &RefHM = Main.Model.BodyModel; + AnyFolder &ref=Main.Model.BodyModel.Trunk; + + //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*{.JntVel.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}; + }; + + + + // ************************************ + // 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}; + }; + + + //Glenohumeral joint + AnyKinEqSimpleDriver GHDriverRight={ + AnyKinMeasure& ref1 =...BodyModel.Interface.Right.GlenohumeralAbduction; + AnyKinMeasure& ref2 =...BodyModel.Interface.Right.GlenohumeralFlexion; + AnyKinMeasure& ref3 =...BodyModel.Interface.Right.GlenohumeralExternalRotation; + + DriverPos=pi/180*{ + .JntPos.Right.GlenohumeralAbduction, //GH joint + .JntPos.Right.GlenohumeralFlexion, //GH joint + .JntPos.Right.GlenohumeralExternalRotation //GH joint + }; + DriverVel = pi/180*{ + .JntVel.Right.GlenohumeralAbduction, //GH joint + .JntVel.Right.GlenohumeralFlexion, //GH joint + .JntVel.Right.GlenohumeralExternalRotation //GH joint + }; + Reaction.Type={Off,Off,Off}; + }; + + + //Elbow flexion driver + AnyKinEqSimpleDriver ElbowFEDriverRight={ + AnyKinMeasure& Elbow =...BodyModel.Interface.Right.ElbowFlexion; + DriverPos=pi/180*{.JntPos.Right.ElbowFlexion}; + DriverVel = pi/180*{.JntVel.Right.ElbowFlexion}; + Reaction.Type={Off}; + }; + + //Elbow pronation driver + AnyKinEqSimpleDriver ElbowPSDriverRight={ + AnyKinMeasure& Elbow =...BodyModel.Interface.Right.ElbowPronation; + DriverPos = pi/180*{.JntPos.Right.ElbowPronation }; + + DriverVel=pi/180*{.JntVel.Right.ElbowPronation }; + Reaction.Type={Off}; + }; + + //Wrist driver + 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}; + }; + + + + // ************************************ + // 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}; + }; + + //Glenohumeral joint driver + AnyKinEqSimpleDriver GHDriverLeft={ + AnyKinMeasure& ref1 =...BodyModel.Interface.Left.GlenohumeralAbduction; + AnyKinMeasure& ref2 =...BodyModel.Interface.Left.GlenohumeralFlexion; + AnyKinMeasure& ref3 =...BodyModel.Interface.Left.GlenohumeralExternalRotation; + DriverPos=pi/180*{ + .JntPos.Left.GlenohumeralAbduction, //GH joint + .JntPos.Left.GlenohumeralFlexion, //GH joint + .JntPos.Left.GlenohumeralExternalRotation //GH joint + }; + DriverVel = pi/180*{ + .JntVel.Left.GlenohumeralAbduction, //GH joint + .JntVel.Left.GlenohumeralFlexion, //GH joint + .JntVel.Left.GlenohumeralExternalRotation //GH joint + }; + Reaction.Type={Off,Off,Off}; + }; + + //Elbow flexion driver + AnyKinEqSimpleDriver ElbowFEDriverLeft={ + AnyKinMeasure& Elbow =...BodyModel.Interface.Left.ElbowFlexion; + DriverPos=pi/180*{.JntPos.Left.ElbowFlexion}; + DriverVel = pi/180*{.JntVel.Left.ElbowFlexion}; + Reaction.Type={Off}; + }; + + //Elbow pronation driver + AnyKinEqSimpleDriver ElbowPSDriverLeft={ + AnyKinMeasure& Elbow =...BodyModel.Interface.Left.ElbowPronation; + DriverPos= pi/180*{.JntPos.Left.ElbowPronation }; + DriverVel = pi/180*{.JntVel.Left.ElbowPronation }; + Reaction.Type={Off}; + }; + + + //Wrist driver + //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}; + }; + + + +// --------------------------------- +// Loads from the Mannequin.any file +// --------------------------------- +AnyFolder MannequinLoads = { + AnyFolder &hm = Main.Model.BodyModel; // For easy reference + AnyFolder &mql = Main.Model.Mannequin.Load; + + AnyForce3D TopVertebra = { + AnyRefNode &ApplPoint = .hm.Trunk.SegmentsCervicalSpine.C1Seg.C1C0JntNode; + F = .mql.TopVertebra; + }; + + AnyFolder Right = { + AnyFolder &rhm = .hm.Right; + AnyFolder &locmql = .mql.Right; + + AnyForce3D Shoulder = { + AnyRefNode &ApplPoint = .rhm.ShoulderArm.Seg.Humerus.gh; + F = .locmql.Shoulder; + }; + + AnyForce3D Elbow = { + AnyRefNode &ApplPoint = .rhm.ShoulderArm.Seg.Humerus.fe; + F = .locmql.Elbow; + }; + + AnyForce3D Hand = { + AnyRefNode &ApplPoint = .rhm.ShoulderArm.Seg.Hand.Ref.PalmJoint; + // F = .locmql.Hand*t; + F = .locmql.Hand; + }; + + }; // Right + + AnyFolder Left = { + AnyFolder &lhm = .hm.Left; + AnyFolder &locmql = .mql.Left; + + AnyForce3D Shoulder = { + AnyRefNode &ApplPoint = .lhm.ShoulderArm.Seg.Humerus.gh; + F = .locmql.Shoulder; + }; + + AnyForce3D Elbow = { + AnyRefNode &ApplPoint = .lhm.ShoulderArm.Seg.Humerus.fe; + F = .locmql.Elbow; + }; + + AnyForce3D Hand = { + AnyRefNode &ApplPoint = .lhm.ShoulderArm.Seg.Hand.Ref.PalmJoint; + // F = .locmql.Hand*t; + F = .locmql.Hand; + }; + }; // Left +}; + +};//Drivers +