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; */ }