virtual public void setUpperBodyPose(int index, float rootLean, float torsoLean, float headLean)
{
upperBodyPos[index] = rootLean;
SimBiConState curState = lowLCon.getCurrentState();
Trajectory tmpTraj = curState.getTrajectory("root");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = rootLean;
}
tmpTraj = curState.getTrajectory("pelvis_waist");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = torsoLean;
}
tmpTraj = curState.getTrajectory("torso_head");
if (tmpTraj != null)
{
tmpTraj.getTrajectoryComponent(index).offset = headLean;
}
}
void FixedUpdate()
{
if (controller == null)
{
return;
}
controller.getCurrentState().doubleStanceMode = conState.doubleStanceMode;
controller.getCurrentState().doIPBelence = conState.doIPBelence;
controller.getCurrentState().comOffsetSagittal = conState.comOffsetSagittal;
controller.getCurrentState().comOffsetCoronal = conState.comOffsetCoronal;
controller.getCurrentState().velSagittal = conState.velSagittal;
controller.getCurrentState().velCoronal = conState.velCoronal;
controller.getCurrentState().belenceError = conState.belenceError;
controller.getCurrentState().stepHeight = conState.stepHeight;
controller.getCurrentState().swingStepLength = conState.swingStepLength;
controller.getCurrentState().standStepLength = conState.standStepLength;
controller.getCurrentState().sagittalStepWidth = conState.sagittalStepWidth;
controller.getCurrentState().coronalStepWidth = conState.coronalStepWidth;
controller.getCurrentState().rootExternalTorqueKp = conState.rootExternalTorqueKp;
controller.getCurrentState().rootExternalTorqueKd = conState.rootExternalTorqueKd;
controller.getCurrentState().rootExternalForceKp = conState.rootExternalForceKp;
controller.getCurrentState().rootExternalForceKd = conState.rootExternalForceKd;
controller.getCurrentState().fixedPositionKp = conState.fixedPositionKp;
controller.getCurrentState().fixedPositionKd = conState.fixedPositionKd;
controller.getCurrentState().velSagittalInAir = conState.velSagittalInAir;
controller.getCurrentState().bodySpringKp = conState.bodySpringKp;
controller.getCurrentState().bodySpringKd = conState.bodySpringKd;
controller.getCurrentState().continueTrajectory = conState.continueTrajectory;
controller.getCurrentState().setDuration(conState.stepTime);
controller.getCurrentState().setTransitionOnFootState((int)conState.transitionOnFootState);
controller.getCurrentState().setStance((int)conState.stateStance);
//root
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("root"), conState.root);
//SWING_Hip
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("SWING_Hip"), conState.SWING_Hip);
//STANCE_Hip
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("STANCE_Hip"), conState.STANCE_Hip);
//SWING_Knee
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("SWING_Knee"), conState.SWING_Knee);
//STANCE_Knee
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("STANCE_Knee"), conState.STANCE_Knee);
//SWING_Shoulder
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("SWING_Shoulder"), conState.SWING_Shoulder);
//STANCE_Shoulder
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("STANCE_Shoulder"), conState.STANCE_Shoulder);
//SWING_Elbow
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("SWING_Elbow"), conState.SWING_Elbow);
//STANCE_Elbow
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("STANCE_Elbow"), conState.STANCE_Elbow);
//SWING_Wrist
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("SWING_Wrist"), conState.SWING_Wrist);
//STANCE_Wrist
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("STANCE_Wrist"), conState.STANCE_Wrist);
//pelvis_torso
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("pelvis_waist"), conState.pelvis_waist);
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("waist_torso"), conState.waist_torso);
//torso_head
setEnginTrajectoryFromEditor(controller.getCurrentState().getTrajectory("torso_head"), conState.torso_head);
}