330 lines (277 with data), 10.9 kB

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

// Spherical joints connecting the hands to the weight bar.
AnySphericalJoint rGloveWeight = {
  AnyRefFrame &Weight = Main.Model.EnvironmentModel.Weight.rhand;
  AnyRefFrame &Glove = Main.Model.BodyModel.Right.ShoulderArm.Seg.Glove;
};  

AnySphericalJoint lGloveWeight = {
  AnyRefFrame &Weight = Main.Model.EnvironmentModel.Weight.lhand;
  AnyRefFrame &Glove = Main.Model.BodyModel.Left.ShoulderArm.Seg.Glove;
};

// Additional support forces to the ground.
AnyReacForce ThoraxGround = {
  AnyKinLinear lin = {
    AnyRefFrame &Thorax = Main.Model.BodyModel.Trunk.SegmentsThorax.ThoraxSeg;
  };
};

AnyReacForce HeadGround = {
  AnyKinMeasureOrg Org = {
    AnyKinLinear lin = {
      AnyRefFrame &Head = Main.Model.BodyModel.Trunk.SegmentsCervicalSpine.SkullSeg;
      Ref = -1;
    };
    MeasureOrganizer = {1};
  };
};

AnyReacForce FeetGround = {
  AnyKinLinear rlin = {
    AnyRefFrame &rfoot = Main.Model.BodyModel.Right.Leg.Seg.Foot;
  };
  AnyKinLinear llin = {
    AnyRefFrame &rfoot = Main.Model.BodyModel.Left.Leg.Seg.Foot;
  };
};

AnyFolder Drivers = {
  
  // Aliases for convenient referencing
  AnyFolder &JntPos=..Mannequin.Posture;  
  AnyFolder &JntVel=..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 ={
      AnyRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnyRefFrame &ref2 =....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.AnatomicalFrameTrunk;
    };
    AnyKinRotational rot ={
      AnyRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnyRefFrame &ref2 =....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.AnatomicalFrameTrunk;
      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& 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. GH flexion and rotation are replaced by the connection
  // to the weight, and the same goes for elbow flexion.
  AnyKinEqSimpleDriver GHDriverRight={
    AnyKinMeasure& ref1 =...BodyModel.Interface.Right.GlenohumeralAbduction;
    
    DriverPos=pi/180*{
      .JntPos.Right.GlenohumeralAbduction  //GH joint
    };
    DriverVel = pi/180*{
      .JntVel.Right.GlenohumeralAbduction  //GH joint
    };
    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 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. GH flexion and rotation are replaced by the connection
  // to the weight, and the same goes for elbow flexion.
  AnyKinEqSimpleDriver GHDriverLeft={
    AnyKinMeasure& ref1 =...BodyModel.Interface.Left.GlenohumeralAbduction;
    DriverPos=pi/180*{
      .JntPos.Left.GlenohumeralAbduction  //GH joint
    };    
    DriverVel = pi/180*{
      .JntVel.Left.GlenohumeralAbduction  //GH joint
    };    
    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
  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={
    AnyKinMeasure& 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;
    #if BM_LEG_MODEL == _LEG_MODEL_LEG_
    AnyKinMeasure& ref2=...BodyModel.Interface.Right.AnkleEversion;
    #else 
    AnyKinMeasure& ref2=...BodyModel.Interface.Right.SubTalarEversion;
    #endif
    DriverPos = pi/180*{.JntPos.Right.AnklePlantarFlexion,.JntPos.Right.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Right.AnklePlantarFlexion,.JntVel.Right.SubTalarEversion};  
    
    Reaction.Type={Off,Off};
  };
  
  
  // ************************************
  // 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;
    #if BM_LEG_MODEL == _LEG_MODEL_LEG_
    AnyKinMeasure& ref2=...BodyModel.Interface.Left.AnkleEversion;
    #else 
    AnyKinMeasure& ref2=...BodyModel.Interface.Left.SubTalarEversion;
    #endif
    DriverPos = pi/180*{.JntPos.Left.AnklePlantarFlexion,.JntPos.Left.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Left.AnklePlantarFlexion,.JntVel.Left.SubTalarEversion};  
    Reaction.Type={Off,Off};
  };
};//Drivers