432 lines (353 with data), 14.6 kB

AnyFolder Jnt={
  
  AnyFolder &RefP=..EnvironmentModel;
  AnyFolder &RefS=.Segments;
  AnyFolder &RefH = ..BodyModel;
  
  //----------------------------------------------
  // Joints - Pelvis - Seat
  //----------------------------------------------
  
  AnyFolder PelvisSeatJnt = {
    
    AnyKinLinear PelvisSeatLinMeasure = {
      AnyRefFrame &SeatRef = ..RefP.Seg.Seat.SeatPelvisJntNode;
      SeatRef = {
        AnyDrawRefFrame test ={ScaleXYZ={0.4,0.4,0.4};};
      };
      AnyRefFrame &PelvisRef = ..RefS.Pelvis.Seat_contactCenter;
      Ref = 0;
    };
    
    AnyKinRotational PelvisSeatRotMeasure = {
      AnyRefFrame &SeatRef = ..RefP.Seg.Seat.SeatPelvisJntNode;
      AnyRefFrame &PelvisRef = ..RefS.Pelvis.Seat_contactCenter;  
      Type = RotVector;
    };
    
    AnyKinEqSimpleDriver PelvisSeatEq = {
      AnyKinLinear &LinearMeasure  = .PelvisSeatLinMeasure;
      AnyKinRotational &RotMeasure = .PelvisSeatRotMeasure;        
      MeasureOrganizer = {0,2,3,4};
      DriverPos = {-0.04,0,0,0};
      DriverVel = {0,0,0,0};
      Reaction.Type = {Off,Off,Off,Off};
    };          
    
    AnyKinMeasureOrg PelvisSeatMeasureOrg = {
      AnyKinLinear &LinearMeasure = .PelvisSeatLinMeasure;
      MeasureOrganizer = {0};
    };
  }; // PelvisSeatJnt
  
  //------------------------------------------------
  // Joint - BackRest - Thorax 
  //------------------------------------------------
  AnyFolder BackRestThoraxJnt = {
    
    AnyKinLinear BackRestThoraxLinMeasure = {
      AnyRefFrame &BackRestRef = ..RefP.Seg.BackRest;
      AnyRefFrame &ThoraxRef = ..RefH.Trunk.SegmentsThorax.ThoraxSeg.T2SupportNode;//ThoraxBackRestJntNode;
      Ref = 0;
    };
    
    AnyKinEqSimpleDriver BackRestThoraxEq = {
      AnyKinLinear &LinearMeasure = .BackRestThoraxLinMeasure;
      MeasureOrganizer = {0};
      DriverPos = {0.03};
      DriverVel = {0};
      Reaction.Type = {Off};
    };
  }; // BackRestThoraxJnt
  
  AnyFolder FootRestFootJnt = {
    
    AnyFolder Right = {      
      
      AnyKinLinear FootRestFootLinMeasure = {
       AnyRefFrame &FootRestRef = ...RefP.Seg.FootRest.FootRestRotNodeRight;//FootRestFootJntNodeRight;
        AnyRefFrame &FootRef = ...RefH.Right.Leg.Seg.Foot.AnatomicalFrame.FootFootRestJntNode;
        Ref = 0;
      };
      
      AnyKinRotational FootRestFootRotMeasure = {
        AnyRefFrame &FootRestRef = .FootRestFootLinMeasure.FootRestRef;
        AnyRefFrame &FootRef = .FootRestFootLinMeasure.FootRef;
        Type = RotVector;
      };
      
      AnyKinEqSimpleDriver FootRestFootEq = {
        AnyKinLinear &LinMeasure = .FootRestFootLinMeasure;
        AnyKinRotational &RotMeasure = .FootRestFootRotMeasure;
        MeasureOrganizer = {0,1,2,4,5};
        DriverPos = {0.01,0,0,0,0};
        DriverVel = {0,0,0,0,0};       
        Reaction.Type = {Off,Off,Off,Off,Off};        
      };  
      
      AnyKinMeasureOrg FootRestFootMeasureOrg = {
        AnyKinLinear &LinearMeasure = .FootRestFootLinMeasure;
        MeasureOrganizer = {0};
      };
      
      AnyKinEqSimpleDriver AnkleDriver ={
        #if BM_LEG_MODEL == _LEG_MODEL_LEG_
          AnyJoint &ref = ...RefH.Right.Leg.Jnt.Ankle;
          MeasureOrganizer={1};
        #else
          AnyJoint &ref = ...RefH.Right.Leg.Jnt.SubTalar;
        #endif        
        DriverPos={0};
        DriverVel={0};
      };
    }; //Right
    
    AnyFolder Left = {
      
      AnyKinLinear FootRestFootLinMeasure = {
          AnyRefFrame &FootRestRef = ...RefP.Seg.FootRest.FootRestRotNodeLeft;//FootRestFootJntNodeRight;
        AnyRefFrame &FootRef = ...RefH.Left.Leg.Seg.Foot.AnatomicalFrame.FootFootRestJntNode;
        Ref = 0;
      };
      
      AnyKinRotational FootRestFootRotMeasure = {
        AnyRefFrame &FootRestRef = .FootRestFootLinMeasure.FootRestRef;
        AnyRefFrame &FootRef = .FootRestFootLinMeasure.FootRef;

        Type = RotVector;
      };
      AnyKinEqSimpleDriver FootRestFootEq = {
        AnyKinLinear &LinMeasure = .FootRestFootLinMeasure;
        AnyKinRotational &RotMeasure = .FootRestFootRotMeasure;
        MeasureOrganizer = {0,1,2,4,5};
        DriverPos = {0.01,0,0,0,0};
        DriverVel = {0,0,0,0,0};  
        Reaction.Type = {Off,Off,Off,Off,Off};        
      };       
      
      AnyKinMeasureOrg FootRestFootMeasureOrg = {
        AnyKinLinear &LinearMeasure = .FootRestFootLinMeasure;
        MeasureOrganizer = {0};
      };
      
      AnyKinEqSimpleDriver AnkleDriver ={
        #if BM_LEG_MODEL == _LEG_MODEL_LEG_
          AnyJoint &ref = ...RefH.Left.Leg.Jnt.Ankle;
          MeasureOrganizer={1};
        #else
          AnyJoint &ref = ...RefH.Left.Leg.Jnt.SubTalar;
        #endif        
        DriverPos={0};
        DriverVel={0};
      };
    }; //Left    
    
  }; // FootFootRestJnt  
  
  
  
  
  
  
  
  
  
  
  
  
}; //Jnt

//--------------------------------------------------
// Drivers
//--------------------------------------------------

AnyFolder Drivers = {
  
  AnyFolder &RefHM = Main.Model.BodyModel;
  AnyFolder &RefPM = Main.Model.EnvironmentModel;
  AnyFolder &DrvPos = Main.DrvPos;
  AnyFolder &DrvVel = Main.DrvVel;
  AnyFolder &JntPos=..Mannequin.Posture;
  AnyFolder &JntVel=..Mannequin.PostureVel;
  
  //-------------------------------------------------------
  //  Drivers - spine
  //-------------------------------------------------------
  
  AnyKinEqSimpleDriver PelvisSeatDrv = {
    AnyKinMeasure& LinMeasure = ..Jnt.PelvisSeatJnt.PelvisSeatLinMeasure;
    MeasureOrganizer = {1};    
    DriverPos = {-.DrvPos.PelvisSeatLinXPos};
    DriverVel = {-.DrvVel.PelvisSeatLinXVel};
    Reaction.Type = {Off}; 
  };    
  
  //-------------------------------------------------------
  //  Drivers - spine
  //-------------------------------------------------------
  
  //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};  
  };  
  
  //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, -.JntPos.NeckExtension};
    DriverVel = pi/180*{.JntVel.NeckLateralBending, .JntVel.NeckRotation, -.JntVel.NeckExtension};
    Reaction.Type = {Off, Off, Off};
  };
  
  //This was not previously... but is easier to use with the detailed spine
  AnyKinRotational PelvisGlobalRotMeasure = {
    AnyRefFrame &PelvisRef = .RefHM.Trunk.SegmentsLumbar.PelvisSeg;  
    AnyRefFrame &GlobalRef = .RefPM.GlobalRef;
    Type = RotVector;
  };
  
  AnyKinMeasureOrg PelvisGlobalRotZMeasure = {
    AnyKinMeasure& RotMeasure = .PelvisGlobalRotMeasure;
    MeasureOrganizer = {2};
  };
  
  AnyKinRotational ThoraxGlobalRotMeasure = {
    AnyRefFrame &ThoraxRef = .RefHM.Trunk.SegmentsThorax.ThoraxSeg;  
    AnyRefFrame &GlobalRef = .RefPM.GlobalRef;
    Type = RotVector;
  }; 
  
  AnyKinMeasureOrg ThoraxGlobalRotZMeasure = {
    AnyKinMeasure& RotMeasure = .ThoraxGlobalRotMeasure;
    MeasureOrganizer = {2};
  }; 
  
  
//  AnyKinEqSimpleDriver PelvisSeatRhythmDrv = {
//    AnyKinMeasureLinComb Measure = {
//      AnyKinMeasure& u1 = ..PelvisGlobalRotZMeasure;
//      AnyKinMeasure& u2 = ..ThoraxGlobalRotZMeasure;
//      Coef = {{1/(0.0533), -1/(0.0533)}};
//      Const = {-(0.6665/(-0.1433))};
//    }; // Measure
//    DriverPos = {180*pi/180+Main.DrvPos.GlobalBackRestRotZPos};
//    DriverVel = {Main.DrvVel.GlobalBackRestRotZVel};
//    Reaction.Type = {Off};
//  }; // SeatBackRestDrv

//AnyKinEqSimpleDriver tmp = {
//  AnyKinRotational PelvisThorax = {
//    AnyRefFrame &Thorax = Main.Model.BodyModel.Trunk.SegmentsThorax.ThoraxSeg;
//    AnyRefFrame &Pelvis = Main.Model.BodyModel.Trunk.SegmentsLumbar.PelvisSeg;
//    Type = RotAxesAngles;
//  };
//  MeasureOrganizer = {0};
//  DriverPos = {30*pi/180};
//  DriverVel = {-15*pi/180};
//};


  AnyKinEq ThoraxThighRhythm = {
    AnyKinMeasureLinComb ThoraxThighRhythmComb = {
      AnyKinMeasureOrg Org = {
        AnySphericalJoint &rhip1 = Main.Model.BodyModel.Right.Leg.Jnt.Hip;
        AnySphericalJoint &lhip2 = Main.Model.BodyModel.Left.Leg.Jnt.Hip;
        AnyKinRotational PelvisThorax = {
          AnyRefFrame &Thorax = Main.Model.BodyModel.Trunk.SegmentsThorax.ThoraxSeg;
          AnyRefFrame &Pelvis = Main.Model.BodyModel.Trunk.SegmentsLumbar.PelvisSeg;
          Type = RotAxesAngles;
        };
        MeasureOrganizer = {2, 5, 6};
      };
      OutDim = 1;
      Coef = {{0.5, 0.5, 4.0}};
      Const = {0};
    };
    Reaction.Type={Off}; 
  };
  
  AnyFolder Right = {
    
    // ************************************
    // 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& ref3 = ....BodyModel.Interface.Right.GlenohumeralExternalRotation;
      
      DriverPos=pi/180*{
        ..JntPos.Right.GlenohumeralAbduction  //GH joint
//        ..JntPos.Right.GlenohumeralExternalRotation  //GH joint
      };
      DriverVel = pi/180*{
        ..JntVel.Right.GlenohumeralAbduction  //GH joint
//        ..JntVel.Right.GlenohumeralExternalRotation  //GH joint
      };
      Reaction.Type={Off};
    };
    
    
    AnyReacForce ForeArmRestingInLap = {
      AnyKinMeasureOrg LinY = {
       AnyKinLinear lin = {
         AnyRefFrame& Ulna = Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Ulna.UlnaArmRestJntNode;
         AnyRefFrame& Thigh = Main.HumanModel.BodyModel.Right.Leg.Seg.Thigh;
       };
       MeasureOrganizer = {1};
     };
    };
    
    //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};
    };
    
  }; //End Right
  
  AnyFolder Left = {
    
    // ************************************
    // 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 
    AnyKinEqSimpleDriver GHDriverLeft={
      AnyKinMeasure& ref1 = ....BodyModel.Interface.Left.GlenohumeralAbduction;
//      AnyKinMeasure& ref3 = ....BodyModel.Interface.Left.GlenohumeralExternalRotation;
      
      DriverPos=pi/180*{
        ..JntPos.Left.GlenohumeralAbduction  //GH joint
//        ..JntPos.Left.GlenohumeralExternalRotation  //GH joint
      };
      DriverVel = pi/180*{
//        ..JntVel.Left.GlenohumeralAbduction  //GH joint
        ..JntVel.Left.GlenohumeralExternalRotation  //GH joint
      };
      Reaction.Type={Off};
    };
    
    AnyReacForce ForeArmRestingInLap = {
      AnyKinMeasureOrg LinY = {
       AnyKinLinear lin = {
         AnyRefFrame& Ulna = Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Ulna.UlnaArmRestJntNode;
         AnyRefFrame& Thigh = Main.HumanModel.BodyModel.Left.Leg.Seg.Thigh;
       };
       MeasureOrganizer = {1};
     };
    };
    
    
    //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};
    };
  }; //End Left    
}; // Drivers