198 lines (169 with data), 6.9 kB

/* ----------------------------------------------------------------
Drivers for the BergmannGH model:

- The pelvis is locked to the ground 
- The abduction of the arm is driven according to Bergmann's experimental protocol
- The remaining degrees of freedom are controlled by parameters mannequin.any 
----------------------------------------------------------------  */
AnyFolder Joints = {
  
  Main.Model.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.PalmJoint = {
    ARel = RotMat(pi/2,z);
  };
  
  AnyKinEq HandDumbbell = {
    AnyKinLinear lin = {
      AnyRefFrame &Hand = Main.Model.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.PalmJoint;
      AnyRefFrame &DumbBell = Main.Model.EnvironmentModel.Dumbbell;
    };
    AnyKinRotational rot = {
      AnyRefFrame &Hand = Main.Model.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.PalmJoint;
      AnyRefFrame &DumbBell = Main.Model.EnvironmentModel.Dumbbell;
      Type = RotAxesAngles;
    };
  };
  
};

AnyFolder Drivers = {
  
  // Aliases for convenient referencing
  AnyFolder &JntPos=...HumanModel.Mannequin.Posture;  
  AnyFolder &JntVel=...HumanModel.Mannequin.PostureVel;  
  
  AnyFolder &Thorax = ..BodyModel.Trunk.SegmentsThorax.ThoraxSeg;
  AnyFolder &ref=Main.Model.BodyModel.Trunk;
  
  
  // ************************************
  // Drivers for attaching the pelvis to the global reference system
  // ************************************  
  
  AnyKinEqSimpleDriver PelvisGroundDriver ={
    AnyKinLinear lin ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnyRefFrame &ref2 =....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.CompatibilityFrameAMMR24;
    };
    AnyKinRotational rot ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnyRefFrame &ref2 =....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.CompatibilityFrameAMMR24;
      Type=RotAxesAngles;
    };
    DriverPos={
      .JntPos.PelvisPosX,
      .JntPos.PelvisPosY,
      .JntPos.PelvisPosZ,
      pi/180*.JntPos.PelvisRotZ,
      pi/180*.JntPos.PelvisRotY,
      pi/180*.JntPos.PelvisRotX
    };
    DriverVel={
      .JntVel.PelvisPosX,
      .JntVel.PelvisPosY,
      .JntVel.PelvisPosZ,
      pi/180*.JntVel.PelvisRotX,
      pi/180*.JntVel.PelvisRotY,
      pi/180*.JntVel.PelvisRotZ
    };
    Reaction.Type={On,On,On,On,On,On};
  };
  
  
  // ************************************
  // Drivers for the spine
  // ************************************
  
  //Neck driver 
  AnyKinEqSimpleDriver NeckJntDriver = {
    AnyKinMeasure& ref0= ...BodyModel.Interface.Trunk.SkullThoraxLateralBending; 
    AnyKinMeasure& ref1= ...BodyModel.Interface.Trunk.SkullThoraxRotation; 
    AnyKinMeasure& ref2= ...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};
  };

  // Abduction movement similar to Bergmann's experiment.
  AnyKinEqFourierDriver AbductionDriverRight={
    AnyKinMeasure& ref1 =...BodyModel.Interface.Right.GlenohumeralAbduction;
    Type = Sin;
    Freq = 0.0625; // 0.1;
    A = { {0.0, 45*pi/180} };
    B = { {0.0, 0.0 } };
    Reaction.Type = {Off};    
  };
  
  // Keep the hand in the frontal plane
  AnyKinEq HandConstraint = {
    AnyKinLinear lin = {
      AnyRefFrame &GHnode = Main.Model.BodyModel.Right.ShoulderArm.Seg.Scapula.gh;
      AnyRefFrame &Hand = Main.Model.BodyModel.Right.ShoulderArm.Seg.Hand;
      Ref = 0;
    };
    Reaction.Type = {Off};
    MeasureOrganizer = {2};
  };
  
  //Elbow flexion driver  
  AnyKinEqSimpleDriver ElbowFlexionDriverRight={
    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 ElbowPronationDriverRight={
    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