int getComponentNames (ClientData clientData, Tcl_Interp *interp, int argc, CONST84 char **argv){
// getComponentNames stateIdx trajectoryIdx
if( argc != 3 ) return TCL_ERROR;
ControllerEditor* obj = (ControllerEditor*)clientData;
int idx = atoi( argv[1] );
SimBiConState* state = obj->getFramework()->getController()->getState( idx );
if( !state ) return TCL_ERROR;
idx = atoi( argv[2] );
Trajectory* trajectory = state->getTrajectory( idx );
if( !trajectory ) return TCL_ERROR;
DynamicArray<const char*> componentNames;
for( uint i = 0; i < trajectory->components.size(); ++i ) {
char* componentName = new char[ 32 ];
sprintf( componentName, "Component %d", i );
componentNames.push_back( componentName );
}
char* result = stringListToTclList( componentNames );
for( uint i = 0; i < componentNames.size(); ++i )
delete[] componentNames[i];
Tcl_SetResult(interp, result, TCL_DYNAMIC);
return TCL_OK;
}
/**
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 trajectoryToEdit(ClientData clientData, Tcl_Interp *interp, int argc, CONST84 char **argv){
// selectCurveToEdit stateIdx trajectoryIdx
if( argc != 3 ) return TCL_ERROR;
ControllerEditor* obj = (ControllerEditor*)clientData;
int idx = atoi( argv[1] );
SimBiConState* state = obj->getFramework()->getController()->getState( idx );
if( !state ) return TCL_ERROR;
idx = atoi( argv[2] );
Trajectory* trajectory = state->getTrajectory( idx );
if( !trajectory ) return TCL_ERROR;
obj->clearEditedCurves();
for( uint i = 0; i < trajectory->components.size(); ++i ) {
obj->addEditedCurve( &trajectory->components[i]->baseTraj );
}
if( trajectory->strengthTraj != NULL ) {
obj->addEditedCurve( trajectory->strengthTraj );
}
return TCL_OK;
}
// Following are wrappers for TCL functions that can access the object
int getStateNames (ClientData clientData, Tcl_Interp *interp, int argc, CONST84 char **argv){
ControllerEditor* obj = (ControllerEditor*)clientData;
SimBiController* controller = obj->getFramework()->getController();
DynamicArray<const char*> stateNames;
uint i = 0;
while( true ) {
SimBiConState* state = controller->getState( i++ );
if( !state ) break;
stateNames.push_back( state->getDescription());
}
char* result = stringListToTclList( stateNames );
Tcl_SetResult(interp, result, TCL_DYNAMIC);
return TCL_OK;
}
void ControllerEditor::stepTaken() {
if( Globals::updateDVTraj ) {
if (conF) {
SimBiConState *state = conF->getController()->states[ lastFSMState ];
dTrajX.simplify_catmull_rom( 0.05 );
dTrajZ.simplify_catmull_rom( 0.05 );
vTrajX.simplify_catmull_rom( 0.05 );
vTrajZ.simplify_catmull_rom( 0.05 );
state->updateDVTrajectories(NULL, NULL, dTrajX, dTrajZ, vTrajX, vTrajZ );
clearEditedCurves();
addEditedCurve( state->dTrajX );
addEditedCurve( state->dTrajZ );
addEditedCurve( state->vTrajX );
addEditedCurve( state->vTrajZ );
}
}
if( Globals::drawControlShots ) {
char stateFileName[100], fName[100];
sprintf(stateFileName, "..\\controlShots\\cs%05d.rs", nextControlShot);
conF->getCharacter()->saveReducedStateToFile(stateFileName);
sprintf(fName, "..\\controlShots\\cs%05d.sbc", nextControlShot);
conF->getController()->writeToFile(fName,stateFileName);
Globals::changeCurrControlShotStr( nextControlShot );
maxControlShot = nextControlShot;
nextControlShot++;
Globals::drawControlShots = false;
Tcl_UpdateLinkedVar( Globals::tclInterpreter, "toggleControlshots" );
conF->getState(&conState);
}
}
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 loads all the pertinent information regarding the simbicon controller from a file.
*/
void SimBiController::loadFromFile(char* fName){
if (fName == NULL)
throwError("NULL file name provided.");
FILE *f = fopen(fName, "r");
if (f == NULL)
throwError("Could not open file: %s", fName);
//to be able to load multiple controllers from multiple files,
//we will use this offset to make sure that the state numbers
//mentioned in each input file are updated correctly
int stateOffset = this->states.size();
SimBiConState* tempState;
int tempStateNr = -1;
//have a temporary buffer used to read the file line by line...
char buffer[200];
//this is where it happens.
while (!feof(f)){
//get a line from the file...
fgets(buffer, 200, f);
if (feof(f))
break;
if (strlen(buffer)>195)
throwError("The input file contains a line that is longer than ~200 characters - not allowed");
char *line = lTrim(buffer);
int lineType = getConLineType(line);
switch (lineType) {
case CON_PD_GAINS_START:
readGains(f);
break;
case CON_STATE_START:
tempState = new SimBiConState();
sscanf(line, "%d", &tempStateNr);
if (tempStateNr != stateOffset + this->states.size())
throwError("Inccorect state offset specified: %d", tempStateNr);
states.push_back(tempState);
tempState->readState(f, stateOffset);
//now we have to resolve all the joint names (i.e. figure out which joints they apply to).
resolveJoints(tempState);
break;
case CON_STANCE_HIP_DAMPING:
sscanf(line, "%lf", &stanceHipDamping);
break;
case CON_STANCE_HIP_MAX_VELOCITY:
sscanf(line, "%lf", &stanceHipMaxVelocity);
break;
case CON_ROOT_PRED_TORQUE_SCALE:
sscanf(line, "%lf", &rootPredictiveTorqueScale);
break;
case CON_CHARACTER_STATE:
character->loadReducedStateFromFile(trim(line));
strcpy(initialBipState, trim(line));
break;
case CON_START_AT_STATE:
if (sscanf(line, "%d", &tempStateNr) != 1)
throwError("A starting state must be specified!");
transitionToState(tempStateNr);
startingState = tempStateNr;
break;
case CON_COMMENT:
break;
case CON_STARTING_STANCE:
if (strncmp(trim(line), "left", 4) == 0){
setStance(LEFT_STANCE);
startingStance = LEFT_STANCE;
}
else if (strncmp(trim(line), "right", 5) == 0){
setStance(RIGHT_STANCE);
startingStance = RIGHT_STANCE;
}
else
throwError("When using the \'reverseTargetOnStance\' keyword, \'left\' or \'right\' must be specified!");
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
}
/**
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);
}
}
/**
this method is used to set the offsets for any and all components of trajectories..
*/
void BipV3BalanceController::applyTrajectoryOffsets(){
// return;
//huge hack for now...
SimBiConState* curState = standingBalanceController->states[con->FSMStateIndex];
Trajectory* tmpTraj;
SimGlobals::rootSagittal = SimGlobals::stanceKnee / 10.0;
tmpTraj = curState->getTrajectory("root");
tmpTraj->components[2]->offset = SimGlobals::rootSagittal;
tmpTraj = curState->getTrajectory("pelvis_lowerback");
tmpTraj->components[2]->offset = SimGlobals::rootSagittal*1.5;
tmpTraj = curState->getTrajectory("lowerback_torso");
tmpTraj->components[2]->offset = SimGlobals::rootSagittal*2;
tmpTraj = curState->getTrajectory("torso_head");
tmpTraj->components[2]->offset = SimGlobals::rootSagittal*2.1;
/*
tmpTraj = curState->getTrajectory("root");
tmpTraj->components[0]->offset = SimGlobals::rootSagittal;
tmpTraj = curState->getTrajectory("pelvis_lowerback");
tmpTraj->components[0]->offset = SimGlobals::rootSagittal*1.5;
tmpTraj = curState->getTrajectory("lowerback_torso");
tmpTraj->components[0]->offset = SimGlobals::rootSagittal*2;
tmpTraj = curState->getTrajectory("torso_head");
tmpTraj->components[0]->offset = SimGlobals::rootSagittal*3;
*/
/*
tmpTraj = curState->getTrajectory("root");
tmpTraj->components[1]->offset = SimGlobals::rootSagittal;
tmpTraj = curState->getTrajectory("pelvis_lowerback");
tmpTraj->components[1]->offset = SimGlobals::rootSagittal*1.5;
tmpTraj = curState->getTrajectory("lowerback_torso");
tmpTraj->components[1]->offset = SimGlobals::rootSagittal*2;
tmpTraj = curState->getTrajectory("torso_head");
tmpTraj->components[1]->offset = SimGlobals::rootSagittal*3;
*/
tmpTraj = curState->getTrajectory("STANCE_Knee");
tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
tmpTraj = curState->getTrajectory("SWING_Knee");
tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
/*
tmpTraj = curState->getTrajectory("SWING_Hip");
tmpTraj->components[0]->offset = SimGlobals::swingHipSagittal;
// tmpTraj = curState->getTrajectory("root");
// tmpTraj->components[0]->offset = SimGlobals::swingHipSagittal;
// tmpTraj->components[1]->offset = SimGlobals::swingHipLateral;
// tmpTraj = curState->getTrajectory("STANCE_Hip");
// tmpTraj->components[0]->offset = 0;
// tmpTraj->components[1]->offset = 0;
// tmpTraj = curState->getTrajectory("STANCE_Ankle");
// tmpTraj->components[0]->offset = SimGlobals::stanceAngleSagittal;
// tmpTraj->components[1]->offset = SimGlobals::stanceAngleLateral;
// tmpTraj = curState->getTrajectory("SWING_Ankle");
// tmpTraj->components[0]->offset = 0;
// tmpTraj->components[1]->offset = 0;
tmpTraj = curState->getTrajectory("STANCE_Knee");
tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
tmpTraj = curState->getTrajectory("SWING_Knee");
tmpTraj->components[0]->offset = SimGlobals::stanceKnee;
*/
}
