568 lines (469 with data), 16.8 kB

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

-The pelvis is locked to the ground 
-The remaining degrees of freedom are controlled by parameters in the mannequin.any file.
-This file is a good way to get an overview of the dof in the full body model
 all the dof in the body model is driven from this file.
-In total 

Soeren, March 1, 2006.
----------------------------------------------------------------  */
#ifndef GLOBAL_DRIVER_SWITCH
  #define GLOBAL_DRIVER_SWITCH 0
#endif

#if GLOBAL_DRIVER_SWITCH == 1
  
  // ************************************
  // Drivers for attaching the pelvis to the global reference system
  // ************************************  

  #if TRUNK == 1
  
  #if PELVIS_POS_X_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverPosX ={
    AnyKinLinear lin ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
    };
    MeasureOrganizer = {0};
    DriverPos={.JntPos.PelvisPosX};
    DriverVel={.JntVel.PelvisPosX};
    Reaction.Type={On};
  };
  #else
  #endif
  
  
  #if PELVIS_POS_Y_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverPosY ={
    AnyKinLinear lin ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
    };
    MeasureOrganizer = {1};
    DriverPos={.JntPos.PelvisPosY};
    DriverVel={.JntVel.PelvisPosY};
    Reaction.Type={On};
  };
  #else
  #endif

  
  
  #if PELVIS_POS_Z_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverPosZ ={
    AnyKinLinear lin ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
    };
    MeasureOrganizer = {2};
    DriverPos={.JntPos.PelvisPosZ};
    DriverVel={.JntVel.PelvisPosZ};
    Reaction.Type={On};
  };
  #else
  #endif

  
  #if PELVIS_ROT_Z_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverRotZ ={
    AnyKinRotational rot ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
      Type=RotAxesAngles;
    };
    MeasureOrganizer = {0};
    DriverPos={pi/180*.JntPos.PelvisRotZ};
    DriverVel={pi/180*.JntVel.PelvisRotZ};
    Reaction.Type={On};
  };
  #else
  #endif
  
  
  #if PELVIS_ROT_Y_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverRotY ={
    AnyKinRotational rot ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
      Type=RotAxesAngles;
    };
    MeasureOrganizer = {1};
    DriverPos={pi/180*.JntPos.PelvisRotY};
    DriverVel={pi/180*.JntVel.PelvisRotY};
    Reaction.Type={On};
  };
  #else
  #endif
  
  
  #if PELVIS_ROT_X_DRIVER == 1
  AnyKinEqSimpleDriver PelvisGroundDriverRotX ={
    AnyKinRotational rot ={
      AnyFixedRefFrame &ref1 =....EnvironmentModel.GlobalRef;
      AnySeg &ref2 =....HumanModel.Trunk.SegmentsLumbar.PelvisSeg;
      Type=RotAxesAngles;
    };
    MeasureOrganizer = {2};
    DriverPos={pi/180*.JntPos.PelvisRotX};
    DriverVel={pi/180*.JntVel.PelvisRotX};
    Reaction.Type={On};
  };
  #else
  #endif
  
  
  
  
  
  
  // ************************************
  // Drivers for the spine
  // ************************************
  
  //Neck driver 
  #if NECK_DRIVER == 1
  AnyKinEqSimpleDriver NeckJntDriver = {
    AnyRevoluteJoint &T12L1Joint = ....HumanModel.BodyModel.Interface.Trunk.NeckJoint;
    DriverPos = pi/180*{.JntPos.NeckExtension};
    DriverVel = pi/180*{.JntVel.NeckExtension};
    Reaction.Type = {Off};
  };
  #else
  #endif
  
  
  //Thorax pelvis rotation driver
  #if PELVIS_THORAX_LATERAL_BENDING_DRIVER == 1
  AnyKinEqSimpleDriver PostureDriverBending ={
    AnyKinMeasure& Ref2 = ...HumanModel.Interface.Trunk.PelvisThoraxLateralBending;        
    DriverPos = pi/180*{.JntPos.PelvisThoraxLateralBending};
    DriverVel = pi/180*{.JntVel.PelvisThoraxLateralBending};
    Reaction.Type = {Off};  
  };  
  #else
  #endif

  #if PELVIS_THORAX_ROTATION_DRIVER == 1
  AnyKinEqSimpleDriver PostureDriverRotation ={
    AnyKinMeasure& Ref3 = ...HumanModel.Interface.Trunk.PelvisThoraxRotation;   
    DriverPos = pi/180*{.JntPos.PelvisThoraxRotation};
    DriverVel = pi/180*{.JntVel.PelvisThoraxRotation};
    Reaction.Type = {Off};  
  };  
  #else
  #endif
  
  #if PELVIS_THORAX_EXTENSION_DRIVER == 1
  AnyKinEqSimpleDriver PostureDriverExtension ={
    AnyKinMeasure& Ref1 = ...HumanModel.Interface.Trunk.PelvisThoraxExtension;
    DriverPos = pi/180*{.JntPos.PelvisThoraxExtension};
    DriverVel = pi/180*{.JntVel.PelvisThoraxExtension};
    Reaction.Type = {Off};  
  };  
  #else
  #endif
  
  #else
  #endif
  
  // ************************************
  // Drivers for the right arm
  // ************************************

  #if RIGHT_ARM == 1
  
  //Sterno clavicular joint driver
  #if STERNO_CLAVICULAR_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver SCDriverRight ={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Right.SternoClavicularProtraction;
    AnyKinMeasure& ref2 =...HumanModel.Interface.Right.SternoClavicularElevation;
    DriverPos = pi/180*{
      .JntPos.Right.SternoClavicularProtraction,
      .JntPos.Right.SternoClavicularElevation
    };
    DriverVel = {
      .JntVel.Right.SternoClavicularProtraction,
      .JntVel.Right.SternoClavicularElevation
    };
    Reaction.Type={Off,Off};
  };
  #else
  #endif
  
  
  //Glenohumeral joint 
  #if GLENOHUMERAL_ABDUCTION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverRightAbduction={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Right.GlenohumeralAbduction;
     DriverPos=pi/180*{.JntPos.Right.GlenohumeralAbduction};
     DriverVel = pi/180*{.JntVel.Right.GlenohumeralAbduction};    
     Reaction.Type={Off};
  };
  #else
  #endif
  
  #if GLENOHUMERAL_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverRightFlexion={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Right.GlenohumeralFlexion;
     DriverPos=pi/180*{.JntPos.Right.GlenohumeralFlexion};
     DriverVel = pi/180*{.JntVel.Right.GlenohumeralFlexion};    
     Reaction.Type={Off};
  };
  #else
  #endif
  
  #if GLENOHUMERAL_EXTERNAL_ROTATION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverRightExternalRotation={
    AnyKinMeasure& ref3 =...HumanModel.Interface.Right.GlenohumeralExternalRotation;
     DriverPos=pi/180*{.JntPos.Right.GlenohumeralExternalRotation};
     DriverVel = pi/180*{.JntVel.Right.GlenohumeralExternalRotation};    
     Reaction.Type={Off};
  };
  #else
  #endif
  
  
  
  //Elbow flexion driver  
  #if ELBOW_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver ElbowFlexionDriverRight={
    AnyKinMeasureOrg  &Elbow =...HumanModel.Interface.Right.ElbowFlexion;
    DriverPos=pi/180*{.JntPos.Right.ElbowFlexion};  
    DriverVel = pi/180*{.JntVel.Right.ElbowFlexion};  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  
  //Elbow pronation driver
  #if ELBOW_PRONATION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver ElbowPronationDriverRight={
    AnyKinMeasure& Elbow =...HumanModel.Interface.Right.ElbowPronation;
    DriverPos = pi/180*{.JntPos.Right.ElbowPronation };  

    DriverVel=pi/180*{.JntVel.Right.ElbowPronation };  
    Reaction.Type={Off};
  };
  #else
  #endif

  
  
  //Wrist driver 
  #if WRIST_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver WristDriverRightFlexion ={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Right.WristFlexion;
    DriverPos = pi/180*{.JntPos.Right.WristFlexion};
    DriverVel = pi/180*{.JntVel.Right.WristFlexion};
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if WRIST_ABDUCTION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver WristDriverRightAbduction ={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Right.WristAbduction;
    DriverPos = pi/180*{.JntPos.Right.WristAbduction};
    DriverVel = pi/180*{.JntVel.Right.WristAbduction};
    Reaction.Type={Off};
  };
  #else
  #endif

  #else
  #endif

  

  // ************************************
  // Drivers for the left arm
  // ************************************
  
  #if LEFT_ARM == 1
  
  //Sterno clavicular joint driver
  #if STERNO_CLAVICULAR_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver SCDriverLeft ={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Left.SternoClavicularProtraction;
    AnyKinMeasure& ref2 =...HumanModel.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};
  };
  #else
  #endif
  
  
  //Glenohumeral joint driver
  #if GLENOHUMERAL_ABDUCTION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverLeftAbduction={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Left.GlenohumeralAbduction;
    DriverPos=pi/180*{.JntPos.Left.GlenohumeralAbduction};    
    DriverVel = pi/180*{.JntVel.Left.GlenohumeralAbduction};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if GLENOHUMERAL_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverLeftFlexion={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Left.GlenohumeralFlexion;
    DriverPos=pi/180*{.JntPos.Left.GlenohumeralFlexion};    
    DriverVel = pi/180*{.JntVel.Left.GlenohumeralFlexion};    
    Reaction.Type={Off};
  };
  #else
  #endif

  #if GLENOHUMERAL_EXTERNAL_ROTATION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver GHDriverLeftExternalRotation={
    AnyKinMeasure& ref3 =...HumanModel.Interface.Left.GlenohumeralExternalRotation;
    DriverPos=pi/180*{.JntPos.Left.GlenohumeralExternalRotation};    
    DriverVel = pi/180*{.JntVel.Left.GlenohumeralExternalRotation};    
    Reaction.Type={Off};
  };
  #else
  #endif

  
  
  //Elbow flexion driver
  #if ELBOW_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver ElbowFEDriverLeft={
  AnyKinMeasureOrg  &Elbow =...HumanModel.Interface.Left.ElbowFlexion;
    DriverPos=pi/180*{.JntPos.Left.ElbowFlexion};  
    DriverVel = pi/180*{.JntVel.Left.ElbowFlexion};  
    Reaction.Type={Off};
  };
  #else
  #endif

  
  //Elbow pronation driver 
  #if ELBOW_PRONATION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver ElbowPSDriverLeft={
    AnyKinMeasure& Elbow =...HumanModel.Interface.Left.ElbowPronation;
    DriverPos= pi/180*{.JntPos.Left.ElbowPronation };  
    DriverVel = pi/180*{.JntVel.Left.ElbowPronation };  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  
  //Wrist driver
  #if WRIST_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver WristDriverLeftFlexion ={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Left.WristFlexion;
    DriverPos = pi/180*{.JntPos.Left.WristFlexion};
    DriverVel = pi/180*{.JntVel.Left.WristFlexion};
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if WRIST_ABDUCTION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver WristDriverLeftAbduction ={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Left.WristAbduction;
    DriverPos = pi/180*{.JntPos.Left.WristAbduction};
    DriverVel = pi/180*{.JntVel.Left.WristAbduction};
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #else
  #endif
 
  // ************************************
  // Drivers for the right leg
  // ************************************
  
  #if RIGHT_LEG + RIGHT_LEG_TD == 0
  #else
  
   //Hip driver 
  #if HIP_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverRightFlexion={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Right.HipFlexion;
    DriverPos= pi/180*{.JntPos.Right.HipFlexion};    
    DriverVel = pi/180*{.JntVel.Right.HipFlexion};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if HIP_EXTERNAL_ROTATION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverRightExternalRotation={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Right.HipExternalRotation;
    DriverPos= pi/180*{.JntPos.Right.HipExternalRotation};    
    DriverVel = pi/180*{.JntVel.Right.HipExternalRotation};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if HIP_ABDUCTION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverRightAbduction={
    AnyKinMeasure& ref3 =...HumanModel.Interface.Right.HipAbduction;
    DriverPos= pi/180*{.JntPos.Right.HipAbduction};    
    DriverVel = pi/180*{.JntVel.Right.HipAbduction};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  
   
  //Knee driver
  #if KNEE_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver KneeDriverRight={
    AnyKinMeasureOrg  &Knee = ...HumanModel.Interface.Right.KneeFlexion;
    DriverPos=pi/180*{.JntPos.Right.KneeFlexion};  
    DriverVel = pi/180*{.JntVel.Right.KneeFlexion};  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  
  //Ankle driver
  #if ANKLE_FLEXION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverRightFlexion={
    AnyKinMeasure& ref1=...HumanModel.Interface.Right.AnklePlantarFlexion;
    DriverPos = pi/180*{.JntPos.Right.AnklePlantarFlexion};  
    DriverVel=pi/180*{.JntVel.Right.AnklePlantarFlexion};  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if ANKLE_EVERSION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverRightEversion={
    AnyKinMeasure& ref2=...HumanModel.Interface.Right.AnkleEversion;
    DriverPos = pi/180*{.JntPos.Right.AnkleEversion};  
    DriverVel=pi/180*{.JntVel.Right.AnkleEversion};  
    Reaction.Type={Off};
  };
  #else
  #endif
 
  #if ANKLE_SUBTALAR_EVERSION_RIGHT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverRightEversion={
    AnyKinMeasure& ref2=...HumanModel.Interface.Right.SubTalarEversion;
    DriverPos = pi/180*{.JntPos.Right.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Right.SubTalarEversion};  
    Reaction.Type={Off};
  };
  #else
  #endif

  #endif
  

  // ************************************
  // Drivers for the Left leg
  // ************************************
  
  #if LEFT_LEG + LEFT_LEG_TD == 0
  #else
  
  //Left hip driver
  #if HIP_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverLeftFlexion={
    AnyKinMeasure& ref1 =...HumanModel.Interface.Left.HipFlexion;
    DriverPos=pi/180*{.JntPos.Left.HipFlexion};    
    DriverVel = pi/180*{.JntVel.Left.HipFlexion};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if HIP_EXTERNAL_ROTATION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverLeftExternalRotation={
    AnyKinMeasure& ref2 =...HumanModel.Interface.Left.HipExternalRotation;
    DriverPos=pi/180*{.JntPos.Left.HipExternalRotation};    
    DriverVel = pi/180*{.JntVel.Left.HipExternalRotation};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if HIP_ABDUCTION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver HipDriverLeftAbduction={
    AnyKinMeasure& ref3 =...HumanModel.Interface.Left.HipAbduction;
    DriverPos=pi/180*{.JntPos.Left.HipAbduction};    
    DriverVel = pi/180*{.JntVel.Left.HipAbduction};    
    Reaction.Type={Off};
  };
  #else
  #endif
  
  
  
  //Knee driver
  #if KNEE_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver KneeDriverLeft={
    AnyKinMeasure& Knee =...HumanModel.Interface.Left.KneeFlexion;
    DriverPos=pi/180*{.JntPos.Left.KneeFlexion   };  
    DriverVel = pi/180*{.JntVel.Left.KneeFlexion   };  
    Reaction.Type={Off};
  };
  #else
  #endif
  

  
 
//Ankle driver
#if ANKLE_FLEXION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverLeftFlexion={
    AnyKinMeasure& ref1=...HumanModel.Interface.Left.AnklePlantarFlexion;
    DriverPos = pi/180*{.JntPos.Left.AnklePlantarFlexion};  
    DriverVel=pi/180*{.JntVel.Left.AnklePlantarFlexion};  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if ANKLE_EVERSION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverLeftEversion={
    AnyKinMeasure& ref2=...HumanModel.Interface.Left.AnkleEversion;
    DriverPos = pi/180*{.JntPos.Left.AnkleEversion};  
    DriverVel=pi/180*{.JntVel.Left.AnkleEversion};  
    Reaction.Type={Off};
  };
  #else
  #endif
  
  #if ANKLE_SUBTALAR_EVERSION_LEFT_DRIVER == 1
  AnyKinEqSimpleDriver AnkleDriverLeftEversion={
    AnyKinMeasure& ref1=...HumanModel.Interface.Left.AnklePlantarFlexion;
    AnyKinMeasure& ref2=...HumanModel.Interface.Left.SubTalarEversion;
    DriverPos = pi/180*{.JntPos.Left.SubTalarEversion};  
    DriverVel=pi/180*{.JntVel.Left.SubTalarEversion};  
    Reaction.Type={Off};
    MeasureOrganizer={1};
  };
  #else
  #endif
  
  #endif

  
#else
#endif