131 lines (113 with data), 5.7 kB

//------------------------------------------------------------------------
AnyFolder Drivers = {
  AnyFolder &refJoints = ..BodyModel.Trunk.JointsLumbar;
  AnyFolder &JntPos=..Mannequin.Posture;  
  AnyFolder &JntVel=..Mannequin.PostureVel;  
  
  // this definition switches on the force-dependent constraints, otherwise the original spine rhythm is used
  #if BM_TRUNK_LUMBAR_RHYTHM == OFF
  
  // the following section is used to control the order of rotational axes
  refJoints.T12L1Jnt.Orientation.Type = RotAxesAngles;
  refJoints.L1L2Jnt.Orientation.Type = RotAxesAngles;
  refJoints.L2L3Jnt.Orientation.Type = RotAxesAngles; 
  refJoints.L3L4Jnt.Orientation.Type = RotAxesAngles;
  refJoints.L4L5Jnt.Orientation.Type = RotAxesAngles;
  refJoints.L5SacrumJnt.Orientation.Type = RotAxesAngles;  
  
  // FORCE-DEPENDENT KINEMATICS DRIVER
  // Force-dependent constraints allow the solver to find optimal positions for the degrees of freedom 
  // based on the current mechanical force configuration of the system (external and muscular forces).
  // The driver below shows how it is done to compute the curvature of the lumbar spine:
  // All degrees of freedom associated with the spherical joints and CType = ForceDep are free to move and 
  // find local force equilibrium, however, an initial guess for all the angles is given in the DriverPos variable
  
  AnyKinEqSimpleDriver FDK_Measure = 
  {
    // Initial guess for all joint angles, except freed up, to be used by the force-dependent kinematics solver.
    // Thorax-pelvis angles are distributed between all joint angles with the application of the following weight vectors:
    AnyVector FE = {0.2538951, 0.2318493, 0.1894594, 0.1468712, 0.1020785}*.JntPos.PelvisThoraxExtension/180*pi;
    AnyVector LB = {0.2538951, 0.2318493, 0.1894594, 0.1468712, 0.1020785}*.JntPos.PelvisThoraxLateralBending/180*pi;
    AnyVector AT = {0.2908131, 0.2403102, 0.1937058, 0.1427486, 0.0910942}*.JntPos.PelvisThoraxRotation/180*pi;
    DriverPos = arrcat({ FE, AT, LB }');
    
    DriverVel = {
      0.0,0.0,0.0,
      0.0,0.0,0.0,      
      0.0,0.0,0.0,
      0.0,0.0,0.0,
      0.0,0.0,0.0};
    
    // define joints, which have force-dependent behaviour
    AnySphericalJoint &jntt12l1 = .refJoints.T12L1Jnt;
    AnySphericalJoint &jnt12 = .refJoints.L1L2Jnt;
    AnySphericalJoint &jnt23 = .refJoints.L2L3Jnt; 
    AnySphericalJoint &jnt34 = .refJoints.L3L4Jnt;
    AnySphericalJoint &jnt45 = .refJoints.L4L5Jnt;
    //AnySphericalJoint &jnt5sacrum = .refJoints.L5SacrumJnt;      // this joint is commented to avoid over-constrained situation
    
    // constraints type has to be set ForceDep for the force-dependent 
    // degrees of freedom of the joint
    CType = {
      ForceDep, ForceDep,ForceDep,
      ForceDep, ForceDep,ForceDep,
      ForceDep, ForceDep,ForceDep,
      ForceDep, ForceDep,ForceDep,
      ForceDep, ForceDep,ForceDep      
      //ForceDep, ForceDep,ForceDep                                // commented joint
    };
    
    Reaction.Type = {
      Off,Off,Off,
      Off,Off,Off,
      Off,Off,Off,
      Off,Off,Off,
      Off,Off,Off
      //Off,Off,Off                                                // commented joint
      };
  };
   // negligible motion between sacrum and pelvis
   AnyKinEqSimpleDriver SacrumPelvisDriver = {
     AnySphericalJoint &Jnt = Main.HumanModel.BodyModel.Trunk.JointsLumbar.SacrumPelvisJnt;
     DriverPos = pi/180*{0.0, 0.0, 0.0};
     DriverVel = pi/180*{0.0, 0.0, 0.0};
     Reaction.Type = {Off, Off, Off};
   };    
  #endif
  
  // NECK JOINT DRIVER 
  // controls flexion/extension of the neck
  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 
  // controls posture of the human 
  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};  
  };  

  // CONSTRAINTS 
  // Pelvis is rigidly fixed to the origin of the global reference frame
  AnyKinEqSimpleDriver GroundConstraint ={
    AnyKinRotational PelvisRot = {
      Type = RotAxesAngles;
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &refPelvisNode = ....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.GroundPelvisJntNode;
    };
    AnyKinLinear PelvisPos = {
      AnyFixedRefFrame &Ground = Main.Model.EnvironmentModel.GlobalRef;
      AnyRefNode &refPelvisNode = ....BodyModel.Trunk.SegmentsLumbar.PelvisSeg.GroundPelvisJntNode;
    };
    
    DriverPos = {0.0, 0, 0.0,0.0, 0.0, 0.0};
    DriverVel = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    Reaction.Type = {On, On, On, On, On, On};  
  };  
  
};//Drivers