305 lines (240 with data), 9.6 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 = {
  
  #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