312 lines (266 with data), 11.2 kB

Main.ModelSetup ={
  /// Various EnvironmentParameters for adjusting the Box
  AnyFolder EnvironmentParameters = {
    AnyInt GravityDirection  = iffun(Main.ModelSetup.LabSpecificData.Gravity[0],0,iffun(Main.ModelSetup.LabSpecificData.Gravity[1],1,2)); // 0 if x, 1 if y, 2 if z
    AnyVar BoxWeight = 10; // kg
    AnyVec3 BoxJii= {0.1,0.1,0.1}; // Inertia
    AnyVec3 BoxDimensions ={0.36,0.40,0.53}; // m (length, height, width)
    AnyVec3 RightHandle ={0,0.18,0.265}; 
    AnyVec3 LeftHandle ={0,0.18,-0.265};
    AnyMat33 RightHandleOrientation =RotMat(0,x)*RotMat(0,y)*RotMat(0,z);
    AnyMat33 LeftHandleOrientation =RotMat(0,x)*RotMat(0,y)*RotMat(0,z);
  };
};

AnySeg Box ={
  Mass= Main.ModelSetup.EnvironmentParameters.BoxWeight;
  Jii= Main.ModelSetup.EnvironmentParameters.BoxJii;
  
  // Position and orient the box according to the hands
  r0 = (Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand.r0 +Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand.r0)/2 -
       (Main.ModelSetup.EnvironmentParameters.RightHandle + Main.ModelSetup.EnvironmentParameters.LeftHandle)/2*Axes0';
  Axes0 = RotMat(Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand.r0, 
                 Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand.r0, 
                 Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand.r0 - Main.ModelSetup.LabSpecificData.Gravity)*RotMat(pi/2,y);
  
  AnyFunTransform3DLin GeomScale = {
    ScaleMat ={{1,0,0},{0,1,0},{0,0,1}};
    Offset={0,0,0};
  };

  AnyRefNode FrameAlignedWithGround ={};
  
  AnyDrawSurf Box ={
    FileName = "box";
    ScaleXYZ =(1 / 0.3) * Main.ModelSetup.EnvironmentParameters.BoxDimensions;
    Opacity = 0.1;
  };
  
  AnyRefNode RightHandle ={       
    sRel =Main.ModelSetup.EnvironmentParameters.RightHandle;  
    ARel=Main.ModelSetup.EnvironmentParameters.RightHandleOrientation;  
  };
  
  AnyRefNode LeftHandle  ={     
    sRel =Main.ModelSetup.EnvironmentParameters.LeftHandle;  
    ARel=Main.ModelSetup.EnvironmentParameters.LeftHandleOrientation;
  };
  
  AnyRefNode BoxCorner1 ={
    sRel= 0.5 * {
      Main.ModelSetup.EnvironmentParameters.BoxDimensions[0],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[1],
      Main.ModelSetup.EnvironmentParameters.BoxDimensions[2]
    };
  };
  
  AnyRefNode BoxCorner2 ={
    sRel= 0.5 * {
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[0],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[1],
      Main.ModelSetup.EnvironmentParameters.BoxDimensions[2]
    };
  };
  
  AnyRefNode BoxCorner3 ={
    sRel= 0.5 * {
      Main.ModelSetup.EnvironmentParameters.BoxDimensions[0],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[1],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[2]
    };
  };
  
  AnyRefNode BoxCorner4 ={
    sRel= 0.5 * {
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[0],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[1],
      -Main.ModelSetup.EnvironmentParameters.BoxDimensions[2]
    };
  };
};

AnyKinLinear Lin_Box ={
  AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
  AnySeg &ref2=Main.EnvironmentModel.Box;
};

AnyKinRotational Rot_Box ={
  AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
  AnySeg &ref2=Main.EnvironmentModel.Box;
  Type=RotAxesAngles;
};

// Adds joints between human and the box.
// these will be include in kinematic model and not the inverse dynamics study
Main.ModelSetup.MocapExtraDrivers.ExtraDrivers ={
  AnyKinLinear HandleHandRight_lin ={
    Ref=0;
    AnyRefNode &ref1= Main.EnvironmentModel.Box.RightHandle;
    AnyRefNode &ref2= Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.PalmJoint;
  };
  
  AnyKinRotational HandleHandRight_rot={
    AnyRefNode &ref1= Main.EnvironmentModel.Box.RightHandle;
    AnyRefNode &ref2= Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand.Ref.PalmJoint;
    Type=RotAxesAngles;
  };
  
  // We allow linear translations of the box but restrain it from rotating
  AnyFunConst WeightFunLin ={Value ={1,1,1}*1;};
  AnyFunConst WeightFunRot ={Value ={1,1,1}*0.00;};
  
  AnyKinEqSimpleDriver HandleHandRightDriver_lin ={
    AnyKinLinear &ref=.HandleHandRight_lin ;
    DriverPos ={0,0,0};
    DriverVel ={0,0,0};
    Reaction.Type ={Off,Off,Off};
    CType={Soft,Soft,Soft};
    WeightFun ={ &.WeightFunLin};
  };
  
  AnyKinEqSimpleDriver HandleHandRightDriver_Rot ={
    AnyKinRotational &ref=.HandleHandRight_rot ;
    DriverPos ={0,0,0};
    DriverVel ={0,0,0};
    Reaction.Type ={Off,Off,Off};
    CType={Soft,Soft,Soft};
    WeightFun = {&.WeightFunRot};
  };
  
  AnyKinLinear HandleHandLeft_lin ={
    Ref=0;
    AnyRefNode &ref1= Main.EnvironmentModel.Box.LeftHandle;
    AnyRefNode &ref2= Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand.Ref.PalmJoint;
  };
  
  AnyKinRotational HandleHandLeft_rot={
    AnyRefNode &ref1= Main.EnvironmentModel.Box.LeftHandle;
    AnyRefNode &ref2= Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand.Ref.PalmJoint;
    Type=RotAxesAngles;
  };
  
  AnyKinEqSimpleDriver  HandleHandLeftDriver_lin ={
    AnyKinLinear &ref=.HandleHandLeft_lin ;
    DriverPos ={0,0,0};
    DriverVel ={0,0,0};
    Reaction.Type ={Off,Off,Off};
    CType={Soft,Soft,Soft};
    WeightFun ={ &.WeightFunLin};
  };
  
  AnyKinEqSimpleDriver  HandleHandLeftDriver_Rot ={
    AnyKinRotational &ref=.HandleHandLeft_rot ;
    DriverPos ={0,0,0};
    DriverVel ={0,0,0};
    Reaction.Type ={Off,Off,Off};
    CType={Soft,Soft,Soft};
    WeightFun = {&.WeightFunRot};
  };
  
  
  // Here we ensure that the box is always parallel to the ground
  AnyFunConst WeightPlaneToGround={
    Value ={1,1}*0.1;
  };

  // rotational measure from ground to box
  AnyKinRotational BoxGroundRotation ={
    AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
    AnyRefFrame &ref2=Main.EnvironmentModel.Box.FrameAlignedWithGround;
    Type=RotAxesAngles;
  };

  AnyKinMeasureOrg BoxCorner1VerticalToGround ={
    AnyKinLinear lin ={
      AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
      AnyRefNode &ref2=Main.EnvironmentModel.Box.BoxCorner1;
    } ;
    MeasureOrganizer={Main.ModelSetup.EnvironmentParameters.GravityDirection};
  };
    
  AnyKinMeasureOrg BoxCorner2VerticalToGround ={
    AnyKinLinear lin ={
      AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
      AnyRefNode &ref2=Main.EnvironmentModel.Box.BoxCorner2;
    } ;
    MeasureOrganizer={Main.ModelSetup.EnvironmentParameters.GravityDirection};
  }; 
  
  AnyKinMeasureOrg BoxCorner3VerticalToGround ={
    AnyKinLinear lin ={
      AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
      AnyRefNode &ref2=Main.EnvironmentModel.Box.BoxCorner3;
    } ;
    MeasureOrganizer={Main.ModelSetup.EnvironmentParameters.GravityDirection};
  };
    
  AnyKinMeasureOrg BoxCorner4VerticalToGround ={
    AnyKinLinear lin ={
      AnyRefFrame &ref1=Main.EnvironmentModel.GlobalRef;
      AnyRefNode &ref2=Main.EnvironmentModel.Box.BoxCorner4;
    } ;
    MeasureOrganizer={Main.ModelSetup.EnvironmentParameters.GravityDirection};
  };
  
  AnyKinMeasureLinComb Corner1_Corner2_Vertical_Identical ={
    AnyKinMeasureOrg &ref1=.BoxCorner1VerticalToGround;
    AnyKinMeasureOrg &ref2=.BoxCorner2VerticalToGround;
    Coef={{1,-1}};
    OutDim=1;
  };
  
  AnyKinMeasureLinComb Corner1_Corner3_Vertical_Identical ={
    AnyKinMeasureOrg &ref1=.BoxCorner1VerticalToGround;
    AnyKinMeasureOrg &ref2=.BoxCorner3VerticalToGround;
    Coef={{1,-1}};
    OutDim=1;
  };
  
  AnyKinEqSimpleDriver BoxBottomPlaneToGround ={
    AnyKinMeasureLinComb &ref1=.Corner1_Corner2_Vertical_Identical;
    AnyKinMeasureLinComb &ref2=.Corner1_Corner3_Vertical_Identical;
    DriverPos ={0,0};
    DriverVel ={0,0};
    Reaction.Type ={Off,Off};
    WeightFun ={ &.WeightPlaneToGround};
  };
};

// We need to manually ensure these exists here because this code is included 
// before the MoCap framework ensure that they are created. 
#ifndef MOCAP_TEST_FILENAME_PREFIX 
#define MOCAP_TEST_FILENAME_PREFIX ""
#endif
#ifndef MOCAP_OUTPUT_FILENAME_PREFIX 
#define MOCAP_OUTPUT_FILENAME_PREFIX ""
#endif


// We save the box kinematic information so we can use it to drive the box in the inverse study
Main.Studies.MarkerTracking.JointAngleOutputs ={ 
  AnyOutputFile BoxPos = {
    FileName = ANYBODY_PATH_OUTPUT + "/" + MOCAP_TEST_FILENAME_PREFIX + MOCAP_OUTPUT_FILENAME_PREFIX 
              + Main.ModelSetup.TrialSpecificData.TrialFileName + "_BoxPos.txt";
    SepSign = ",";
    SepSpaceAutoOnOff = Off;
    Header.TitleSectionOnOff = Off;
    Header.ConstSectionOnOff = Off;
    Header.ConstSectionSaveOptionsOnOff = Off;
    Header.VarSectionOnOff = Off;
    AnyVector PosLin = Main.EnvironmentModel.Lin_Box.Pos;
    AnyVector PosRot = Main.EnvironmentModel.Rot_Box.Pos;
  };
};

// Here we load in the kinematic data for the box to use in the inverse study
Main.Studies.InverseDynamicStudy.ModelEnvironmentConnection.JointsAndDrivers  ={
 AnyKinDriver BoxDriver = {
   #include "<ANYBODY_PATH_AMMR>/Tools/AnyMocap/InterpolateJointAngleData.any"
   FileReader.FileName = ANYBODY_PATH_OUTPUT + "/" + MOCAP_TEST_FILENAME_PREFIX + MOCAP_OUTPUT_FILENAME_PREFIX 
                         + Main.ModelSetup.TrialSpecificData.TrialFileName + "_BoxPos.txt";
   /// The default data if no data has been generated.
   FileReader.Data0 = repmat(6, FileReader.Length0, {0});
   AnyKinMeasure& PosLin = Main.EnvironmentModel.Lin_Box;
   AnyKinMeasure& PosRot = Main.EnvironmentModel.Rot_Box;
   //important reaction needs to be off... if not driver will carry loads
   Reaction.Type = repmat(nDim, Off);
 };
};
  
  
// this folder adds reactions between human hands and the box
// this helps avoid large moments around the wrist joint since we have no box rotation
AnyFolder Support ={   
  AnyFolder HandBoxConnection ={
    AnyKinLinear HandBoxRight_Lin ={
      AnyRefFrame &ref1=Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand;
      AnyRefFrame &ref2=Main.EnvironmentModel.Box.RightHandle;
      Ref=0;
    };
    
    AnyKinRotational HandBoxRight_Rot ={
      AnyRefFrame &ref1=Main.HumanModel.BodyModel.Right.ShoulderArm.Seg.Hand;
      AnyRefFrame &ref2=Main.EnvironmentModel.Box.RightHandle;
      Type=RotAxesAngles;
    };
    
    AnyReacForce RightHandReactions={
      AnyKinLinear &ref1=.HandBoxRight_Lin;
      AnyKinRotational &ref2 =.HandBoxRight_Rot;
    };
    
    AnyKinLinear HandBoxLeft_Lin ={
      AnyRefFrame &ref1=Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand;
      AnyRefFrame &ref2=Main.EnvironmentModel.Box.LeftHandle;
      Ref=0;
      
    };
    
    AnyKinRotational HandBoxLeft_Rot ={
      AnyRefFrame &ref1=Main.HumanModel.BodyModel.Left.ShoulderArm.Seg.Hand;
      AnyRefFrame &ref2=Main.EnvironmentModel.Box.LeftHandle;
      Type=RotAxesAngles;
    };
    
    
    AnyReacForce LeftHandReactions={
      AnyKinLinear &ref1=.HandBoxLeft_Lin;
      AnyKinRotational &ref2 =.HandBoxLeft_Rot;
    }; 
  };
};