/**
This method makes it possible to evaluate the debug pose at any phase angle
*/
void SimBiController::updateTrackingPose(DynamicArray<double>& trackingPose, double phiToUse){
if( phiToUse < 0 )
phiToUse = phi;
if( phiToUse > 1 )
phiToUse = 1;
trackingPose.clear();
this->character->getState(&trackingPose);
ReducedCharacterState debugRS(&trackingPose);
//always start from a neutral desired pose, and build from there...
for (int i=0;i<jointCount;i++){
debugRS.setJointRelativeOrientation(Quaternion(1, 0, 0, 0), i);
debugRS.setJointRelativeAngVelocity(Vector3d(), i);
controlParams[i].relToCharFrame = false;
}
//and this is the desired orientation for the root
Quaternion qRootD(1, 0, 0, 0);
SimBiConState* curState = states[FSMStateIndex];
for (int i=0;i<curState->getTrajectoryCount();i++){
//now we have the desired rotation angle and axis, so we need to see which joint this is intended for
int jIndex = curState->sTraj[i]->getJointIndex(stance);
//if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error
if (curState->sTraj[i]->relToCharFrame == true || jIndex == swingHipIndex)
controlParams[jIndex].relToCharFrame = true;
Vector3d d0, v0;
computeD0( phiToUse, &d0 );
computeV0( phiToUse, &v0 );
Quaternion newOrientation = curState->sTraj[i]->evaluateTrajectory(this, character->getJoint(jIndex), stance, phiToUse, d - d0, v - v0);
if (jIndex == -1){
qRootD = newOrientation;
}else{
debugRS.setJointRelativeOrientation(newOrientation, jIndex);
}
}
debugRS.setOrientation(qRootD);
//now, we'll make one more pass, and make sure that the orientations that are relative to the character frame are drawn that way
for (int i=0;i<jointCount;i++){
if (controlParams[i].relToCharFrame){
Quaternion temp;
Joint* j = character->getJoint(i);
ArticulatedRigidBody* parent = j->getParent();
while (parent != root){
j = parent->getParentJoint();
parent = j->getParent();
temp = debugRS.getJointRelativeOrientation(character->getJointIndex(j->name)) * temp;
}
temp = qRootD * temp;
temp = temp.getComplexConjugate() * debugRS.getJointRelativeOrientation(i);
debugRS.setJointRelativeOrientation(temp, i);
}
}
}
int getTrajectoryNames (ClientData clientData, Tcl_Interp *interp, int argc, CONST84 char **argv){
// getComponentNames stateIdx
if( argc != 2 ) return TCL_ERROR;
ControllerEditor* obj = (ControllerEditor*)clientData;
int idx = atoi( argv[1] );
SimBiConState* state = obj->getFramework()->getController()->getState( idx );
if( !state ) return TCL_ERROR;
DynamicArray<const char*> trajectoryNames;
for( int i = 0; i < state->getTrajectoryCount(); ++i ) {
Trajectory* trajectory = state->getTrajectory( i );
trajectoryNames.push_back( trajectory->jName );
}
char* result = stringListToTclList( trajectoryNames );
Tcl_SetResult(interp, result, TCL_DYNAMIC);
return TCL_OK;
}
/**
This method is used to compute the torques that are to be applied at the next step.
*/
void SimBiController::computeTorques(DynamicArray<ContactPoint> *cfs){
if (FSMStateIndex >= (int)states.size()){
tprintf("Warning: no FSM state was selected in the controller!\n");
return;
}
ReducedCharacterState poseRS(&desiredPose);
//d and v are specified in the rotation (heading) invariant coordinate frame
updateDAndV();
//there are two stages here. First we will compute the pose (i.e. relative orientations), using the joint trajectories for the current state
//and then we will compute the PD torques that are needed to drive the links towards their orientations - here the special case of the
//swing and stance hips will need to be considered
//always start from a neutral desired pose, and build from there...
for (int i=0;i<jointCount;i++){
poseRS.setJointRelativeOrientation(Quaternion(1, 0, 0, 0), i);
poseRS.setJointRelativeAngVelocity(Vector3d(), i);
controlParams[i].controlled = true;
controlParams[i].relToCharFrame = false;
}
//and this is the desired orientation for the root
Quaternion qRootD(1, 0, 0, 0);
double phiToUse = phi;
//make sure that we only evaluate trajectories for phi values between 0 and 1
if (phiToUse>1)
phiToUse = 1;
SimBiConState* curState = states[FSMStateIndex];
Quaternion newOrientation;
for (int i=0;i<curState->getTrajectoryCount();i++){
//now we have the desired rotation angle and axis, so we need to see which joint this is intended for
int jIndex = curState->sTraj[i]->getJointIndex(stance);
//get the desired joint orientation to track - include the feedback if necessary/applicable
Vector3d d0, v0;
computeD0( phiToUse, &d0 );
computeV0( phiToUse, &v0 );
newOrientation = curState->sTraj[i]->evaluateTrajectory(this, character->getJoint(jIndex), stance, phiToUse, d - d0, v - v0);
//if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error
if (jIndex == -1){
qRootD = newOrientation;
rootControlParams.strength = curState->sTraj[i]->evaluateStrength(phiToUse);
}else{
if (curState->sTraj[i]->relToCharFrame == true || jIndex == swingHipIndex){
controlParams[jIndex].relToCharFrame = true;
controlParams[jIndex].charFrame = characterFrame;
}
poseRS.setJointRelativeOrientation(newOrientation, jIndex);
controlParams[jIndex].strength = curState->sTraj[i]->evaluateStrength(phiToUse);
}
}
//compute the torques now, using the desired pose information - the hip torques will get overwritten below
PoseController::computeTorques(cfs);
double stanceHipToSwingHipRatio = getStanceFootWeightRatio(cfs);
if (stanceHipToSwingHipRatio < 0)
rootControlParams.strength = 0;
//and now separetely compute the torques for the hips - together with the feedback term, this is what defines simbicon
computeHipTorques(qRootD, poseRS.getJointRelativeOrientation(swingHipIndex), stanceHipToSwingHipRatio);
double h = character->getRoot()->getCMPosition().y;
double hMax = 0.4;
double hMin = 0.2;
if (h > hMax)
h = hMax;
if ( h < hMin)
h = hMin;
h = (h-hMin) / (hMax-hMin);
for (uint i=0;i<torques.size();i++){
torques[i] = torques[i]*h + Vector3d(0,0,0) * (1-h);
}
}
