500 lines (407 with data), 16.1 kB

/* ----------------------------------------------------------------
   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 = {
  
  // 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;
    
  
  // Place the right toe and heel on the ground
  AnyKinEqSimpleDriver RToeGroundConstraint ={
    AnyKinLinear ToePos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ball = Main.Model.BodyModel.Right.Leg.Seg.Foot.ToeJoint;
    };
   MeasureOrganizer = {1};  // Only the y coordinate
    DriverPos = {0.0};
    DriverVel = {0};
    Reaction.Type = {Off};   // Provide ground reaction forces
  
  };
  
  AnyKinEqSimpleDriver RHeelGroundConstraint ={
    AnyKinLinear HeelPos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ball = Main.Model.BodyModel.Right.Leg.Seg.Foot.HeelJoint;
    };
    MeasureOrganizer = {1};  // Only the y coordinate
    DriverPos = {0.0};
    DriverVel = {0};
    Reaction.Type = {Off};   // Provide ground reaction forces
  };
  
  // Place the left toe and heel on the ground
AnyKinEqSimpleDriver LToeGroundConstraint ={
    AnyKinLinear ToePos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ball = Main.Model.BodyModel.Left.Leg.Seg.Foot.ToeJoint;
    };
    MeasureOrganizer = {1};  // Only the y coordinate
    DriverPos = {0.0};
    DriverVel = {0};
    Reaction.Type = {Off};  // Provide ground reaction

  };
  
  AnyKinEqSimpleDriver LHeelGroundConstraint ={
    AnyKinLinear HeelPos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ball = Main.Model.BodyModel.Left.Leg.Seg.Foot.HeelJoint;
    };
    MeasureOrganizer = {1};  // Only the y coordinate
    DriverPos = {0.0};
  DriverVel = {0};
    Reaction.Type = {Off};  // Provide ground reaction
  };  
  
  // Position the Ankles right above the z axis
  AnyKinEqSimpleDriver RAnkleX = {
    AnyKinLinear AnklePos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ankle = ..LegR.Seg.Talus.AnkleJoint;
    };
    MeasureOrganizer = {0};  // Only the x coordinate
    DriverPos = {0.0};
    DriverVel = {0.0};
    Reaction.Type = {Off};
  };
  AnyKinEqSimpleDriver LAnkleX ={
    AnyKinLinear AnklePos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &Ankle = ..LegL.Seg.Talus.AnkleJoint;
    };
    MeasureOrganizer = {0};  // Only the x coordinate
    DriverPos = {0.0};
    DriverVel = {0.0};
    Reaction.Type = {Off};
  };
  //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,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={
    AnyKinMeasureOrg  &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={
  AnyKinMeasureOrg  &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};
  };
    
  
 
  // ************************************
  // Drivers for the right leg
  // ************************************
  
   //Hip driver 
  AnyKinEqSimpleDriver HipDriverRight={
    AnyKinMeasure& ref1 =...BodyModel.Interface.Right.HipFlexion;
    AnyKinMeasure& ref2 =...BodyModel.Interface.Right.HipExternalRotation;
    AnyKinMeasure& ref3 =...BodyModel.Interface.Right.HipAbduction;
    DriverPos= pi/180*{
      .JntPos.Right.HipFlexion,
      .JntPos.Right.HipExternalRotation,
      .JntPos.Right.HipAbduction
    };    
    DriverVel = pi/180*{
      .JntVel.Right.HipFlexion,
      .JntVel.Right.HipExternalRotation,
      .JntVel.Right.HipAbduction
    };    
    Reaction.Type={Off,Off,Off};
  
  };
   
  //Knee driver
  AnyKinEqSimpleDriver KneeDriverRight={
    AnyKinMeasureOrg  &Knee = ...BodyModel.Interface.Right.KneeFlexion;
    DriverPos=pi/180*{.JntPos.Right.KneeFlexion};  
    DriverVel = pi/180*{.JntVel.Right.KneeFlexion};  
    Reaction.Type={Off};
  };
  
  //Ankle driver
  AnyKinEqSimpleDriver AnkleDriverRight={
    AnyKinMeasure& ref1=...BodyModel.Interface.Right.AnklePlantarFlexion;
    AnyKinMeasure& ref2=...BodyModel.Interface.Right.SubTalarEversion;
    DriverPos = pi/180*{.JntPos.Right.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Right.SubTalarEversion};  
    Reaction.Type={Off};
    MeasureOrganizer={1};
  };
  

  // ************************************
  // Drivers for the Left leg
  // ************************************
  
  //Left hip driver
  AnyKinEqSimpleDriver HipDriverLeft={
    AnyKinMeasure& ref1 =...BodyModel.Interface.Left.HipFlexion;
    AnyKinMeasure& ref2 =...BodyModel.Interface.Left.HipExternalRotation;
    AnyKinMeasure& ref3 =...BodyModel.Interface.Left.HipAbduction;
    DriverPos=pi/180*{
      .JntPos.Left.HipFlexion,
      .JntPos.Left.HipExternalRotation,
      .JntPos.Left.HipAbduction
    };    
    DriverVel = pi/180*{
      .JntVel.Left.HipFlexion,
      .JntVel.Left.HipExternalRotation,
      .JntVel.Left.HipAbduction
    };    
    Reaction.Type={Off,Off,Off};
  };
  
  //Knee driver
  AnyKinEqSimpleDriver KneeDriverLeft={
    AnyKinMeasure& Knee =...BodyModel.Interface.Left.KneeFlexion;
    DriverPos=pi/180*{      .JntPos.Left.KneeFlexion   };  
    DriverVel = pi/180*{      .JntVel.Left.KneeFlexion   };  
    Reaction.Type={Off};
  };
  

  
 
//Ankle driver
  AnyKinEqSimpleDriver AnkleDriverLeft={
    AnyKinMeasure& ref1=...BodyModel.Interface.Left.AnklePlantarFlexion;
    AnyKinMeasure& ref2=...BodyModel.Interface.Left.SubTalarEversion;
    DriverPos = pi/180*{.JntPos.Left.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Left.SubTalarEversion};  
    Reaction.Type={Off};
    MeasureOrganizer={1};
  };
  
  // Create reaction forces below the feet. Thi provides reactions
  // for the feet regardless of their positions.

  AnyReacForce RightFootReaction = {
    
    
    AnyKinRotational rot = {
      AnySeg &ref1=..LegR.Seg.Foot;
      AnyFixedRefFrame &ref2 = ....EnvironmentModel.GlobalRef;
      Type = RotVector;
    };  
    
    AnyKinLinear RightFootLinMeasure = {
      AnySeg &ref1=..LegR.Seg.Foot;
      AnyFixedRefFrame &ref2 = ....EnvironmentModel.GlobalRef;  
    };
    
  };
  
  AnyReacForce LeftFootReaction = {
    
    AnyKinLinear LeftFootLinMeasure = {
      AnySeg &ref1=..LegL.Seg.Foot;
      AnyFixedRefFrame &ref2 = ....EnvironmentModel.GlobalRef;
    };
    AnyKinRotational LeftFootRotMeasure = {
      AnySeg &ref1=..LegL.Seg.Foot;
      AnyFixedRefFrame &ref2 = ....EnvironmentModel.GlobalRef;
      Type = RotVector;
    };  
    
    
  };
  
};//Drivers

// ---------------------------------
// 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;
    };
    
    AnyForce3D Hip = {
      AnyRefNode &ApplPoint = .rhm.Leg.Seg.Thigh.HipJoint;
      F = .locmql.Hip;
    };
    
    AnyForce3D Knee = {
      AnyRefNode &ApplPoint = .rhm.Leg.Seg.Thigh.KneeJoint;
      F = .locmql.Knee;
    };
    
    AnyForce3D Ankle = {
      AnyRefNode &ApplPoint = .rhm.Leg.Seg.Talus.AnkleJoint;
      F = .locmql.Ankle;
    };    
  };  // 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;
    };
    
    AnyForce3D Hip = {
      AnyRefNode &ApplPoint = .lhm.Leg.Seg.Thigh.HipJoint;
      F = .locmql.Hip;
    };
    
    AnyForce3D Knee = {
      AnyRefNode &ApplPoint = .lhm.Leg.Seg.Thigh.KneeJoint;
      F = .locmql.Knee;
    };
    
    AnyForce3D Ankle = {
      AnyRefNode &ApplPoint = .lhm.Leg.Seg.Talus.AnkleJoint;
      F = .locmql.Ankle;
    };    
  };  // Left
};