157 lines (113 with data), 6.4 kB

//This is a model which can predict the posture as a consequence of applied loads in hands. 
//It does this by minimizing joint torques and apply balance drivers which account for external 
//applied loads.
//
//
//The model is driven by a combination of the following drivers:
// * Drivers which minimize the joint moments (arising from gravity and applied loads in hands) in elbow, shoulder, L4L5 and knee
// * Driver which tries to keep the CoP inside the foot stance area.
// * Feet maintain contact with the ground, but the position can be controlled by widgets
// * Hands are linked to an object, of which positioning can be altered using widgets
//
//Two type of loads can be applied, either a fixed weight of the object and/or a force vector 
//
//The current model has a force vector applied on the object being held between the hands with a zero weight of the object (this is editable)
//
//
//To run the model
// * Load the model
// * Try to drag (click and drag) one of the widgets in the ModelView (seen as small coordinate systems)  
// * When the widget is release the model will run the analysis

#include "libdef.any"

#include "jointlimit/Balance_template_foot_area.any"
#include "MinTorqueClass/MinTorqueClass.any"



//Switch to define if load is applied to both hands or a single hand. 
//Three combinations LoadInRightHand,LoadInLeftHand,LoadInRightHand+LoadInLeftHand
#define LoadInRightHand 1   
#define LoadInLeftHand  1
 #include "<ANYBODY_PATH_MODELUTILS>/Video/VideoLookAtCamera.any"


Main = {
  
  #include "Model/BodyModelConfig.any"
  
  // Using your own Mannequin.any file in the Model folder of your model
  #include "Model\Mannequin.any"
  
  // Include default human model
  #include "<ANYBODY_PATH_BODY>\HumanModel.any"

  AnyFolder InputParameters ={
  
    //Model can be loaded with both a force and a weight of an object
    AnyVec3 ForceDirection = DesignVar({0,0.01,-1});  // this is the initial direction of the force it can be manipulated by widget (vertical blue line upon load)
    AnyVar ForceMagnitude = 100; //this is the initial value of the force it can be manipulated by dragging the widget

    //Initial value of handforce as a function of time, each element has corresponding timevar in HandForceTime
    //These lines should generally not be changed, they are generic and made using ForceMagnitude and ForceDirection
    AnyFloat HandForceTime ={-10,0,1,3,4,10}; //this time vector needs to be larger than tStart-tEnd
    AnyFloat HandForce =  { ForceDirection[0]*ForceMagnitude*{1,1,1,1,1,1}, ForceDirection[1]*ForceMagnitude*{1,1,1,1,1,1},  ForceDirection[2]*ForceMagnitude*{1,1,1,1,1,1}};

    //weight of object and location
    AnyVar ObjectWeight = 5;   //weight of mass applied
    AnyVec3 ObjectPoint = DesignVar({0.65,0,1.0});  //initial position of object, dragging the widget will change this
    AnyMat33 ObjectOrientation = RotMat(0*pi/180,x)*RotMat(0*pi/180,y)*RotMat(0*pi/180,z); //initial orientation of object
    AnyVec3 HandPosRight = {0,-0.1,0};  //position of the handle coordinates wrt CoM of Object
    AnyVec3 HandPosLeft = {0,0.1,0};   //position of the handle coordinates wrt CoM of Object 
    
    AnyVec3 RightFootPoint =DesignVar({0.1,-0.1,0}); //initial position of the right foot
    AnyVec3 LeftFootPoint =DesignVar({0.5,0.1,0}); //initial position of the left foot

  };  
  
  AnyFolder Model = {  
    
    AnyFolder &InputParams=Main.InputParameters;
    
    //This file contains the balance and torque minimization drivers for the model
    #include "Posture_Motion/ExternalForceBalanceControlSimpleWithClass.any"
    
    // A link to the human model
    AnyFolder &HumanModel=.HumanModel.BodyModelWithDefaultDrivers;
    
    // Environment files are used to include objects surrounding human, e.g. global reference frame, widgets, box for visualization
    #include "Model\Environment.any"   
    
    AnyFolder ModelEnvironmentConnection = {
      // This file contains all joint and foot reaction forces to simulate the standing human
      #include "Model\JointsAndDrivers.any"
      #include "Model\Force.any"
    };
  };
  
  //This kin study runs before the normal study with a coarser kin tolerance and some drivers exluded
  AnyBodyStudy Kinematic_Pre_Study= {
    
    AnyMechObjectExcluder ExcludeDrivers = {
      //Exclude some drivers from this study to make it kinematically safer.
      Objects = arrcat(
        ObjSearch("Main.Model.OverallWeightBalanceControl.EquilibriumEq_Driver","AnyMechObject"),
        ObjSearch("Main.Model.MinMomentOnL5SacrumJnt.EquilibriumEq_Driver","AnyMechObject")
      );
    };
    
    AnyFolder &Model = .Model;  
    
    tStart=.Study.tStart;
    tEnd=.Study.tEnd;
    
    Gravity={0.0, 0.0,-9.81};
    nStep = 1;
    InitialConditions.MaxIteration=Kinematics.MaxIteration;
    InitialConditions.Relax=1;
    Kinematics.MaxIteration =5000;
    //Note the high tolerance
    Kinematics.KinematicTol=0.5;
    InitialConditions.KinematicTol= Kinematics.KinematicTol;
    Kinematics.SolverType = KinSolOverDeterminate;
    InitialConditions.SolverType = Kinematics.SolverType ;
    Kinematics.PosAnalysisOnlyOnOff =On;
    InitialConditions.PosAnalysisOnlyOnOff=On;
    
    InitialConditions.UseStartGuessOnOff =Off;//Setting this to OFF makes model start from last solved frame, it does not use load positions 
    Kinematics.UseStartGuessOnOff =Off;
  };
  
  
  
  AnyBodyStudy Study = {
    AnyFolder &Model = .Model;  
    Gravity={0.0,0.0, -9.81};
    nStep = 1;
    tEnd=1;
    Kinematics.SolverType = KinSolOverDeterminate;
    InitialConditions.SolverType = Kinematics.SolverType ;
    Kinematics.PosAnalysisOnlyOnOff =On;
    InitialConditions.PosAnalysisOnlyOnOff=On;
    Kinematics.MaxIteration =5000;
    Kinematics.KinematicTol=5e-6;
    
    Kinematics.SmallStepAssumptionOnOff =Off;
    
    InitialConditions.UseStartGuessOnOff =Off; //Setting this to OFF makes model start from last solved frame, it does not use load positions 
    Kinematics.UseStartGuessOnOff =Off;
    
  };
  
  AnyOperationSequence WidgetOperation = {
    
    AnyOperation &Ini=Main.Kinematic_Pre_Study.InitialConditions;
    AnyOperation &Inv=Main.Study.InverseDynamics;
    
  };  
  
  
  #include "Model\RunAppSequence.any"
  
  
}; //Main