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