// Solve for a balanced pose using IK (Lesson 7 Answer) Eigen::VectorXd solveIK(SkeletonPtr biped) { // Modify the intial pose to one-foot stance before IK biped->setPosition(biped->getDof("j_shin_right")-> getIndexInSkeleton(), -1.4); biped->setPosition(biped->getDof("j_bicep_left_x")-> getIndexInSkeleton(), 0.8); biped->setPosition(biped->getDof("j_bicep_right_x")-> getIndexInSkeleton(), -0.8); Eigen::VectorXd newPose = biped->getPositions(); BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left"); BodyNodePtr leftToe = biped->getBodyNode("h_toe_left"); double initialHeight = -0.8; for(std::size_t i = 0; i < default_ik_iterations; ++i) { Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM(); Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel); LinearJacobian jacobian = biped->getCOMLinearJacobian() - biped->getLinearJacobian(leftHeel, localCOM); // Sagittal deviation double error = deviation[0]; Eigen::VectorXd gradient = jacobian.row(0); Eigen::VectorXd newDirection = -0.2 * error * gradient; // Lateral deviation error = deviation[2]; gradient = jacobian.row(2); newDirection += -0.2 * error * gradient; // Position constraint on four (approximated) corners of the left foot Eigen::Vector3d offset(0.0, -0.04, -0.03); error = (leftHeel->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftHeel, offset).row(1); newDirection += -0.2 * error * gradient; offset[2] = 0.03; error = (leftHeel->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftHeel, offset).row(1); newDirection += -0.2 * error * gradient; offset[0] = 0.04; error = (leftToe->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftToe, offset).row(1); newDirection += -0.2 * error * gradient; offset[2] = -0.03; error = (leftToe->getTransform() * offset)[1] - initialHeight; gradient = biped->getLinearJacobian(leftToe, offset).row(1); newDirection += -0.2 * error * gradient; newPose += newDirection; biped->setPositions(newPose); biped->computeForwardKinematics(true, false, false); } return newPose; }
int main(int argc, char* argv[]) { // Create empty soft world WorldPtr myWorld(new World); // Load ground and Atlas robot and add them to the world DartLoader urdfLoader; SkeletonPtr ground = urdfLoader.parseSkeleton( DART_DATA_PATH"sdf/atlas/ground.urdf"); // SkeletonPtr atlas = SoftSdfParser::readSkeleton( // DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.sdf"); SkeletonPtr atlas = SoftSdfParser::readSkeleton( DART_DATA_PATH"sdf/atlas/atlas_v3_no_head_soft_feet.sdf"); myWorld->addSkeleton(atlas); myWorld->addSkeleton(ground); // Set initial configuration for Atlas robot VectorXd q = atlas->getPositions(); q[0] = -0.5 * DART_PI; atlas->setPositions(q); // Set gravity of the world myWorld->setGravity(Vector3d(0.0, -9.81, 0.0)); // Create a window and link it to the world MyWindow window(new Controller(atlas, myWorld->getConstraintSolver())); window.setWorld(myWorld); // Print manual cout << "space bar: simulation on/off" << endl; cout << "'p': playback/stop" << endl; cout << "'[' and ']': play one frame backward and forward" << endl; cout << "'v': visualization on/off" << endl; cout << endl; cout << "'h': harness pelvis on/off" << endl; cout << "'j': harness left foot on/off" << endl; cout << "'k': harness right foot on/off" << endl; cout << "'r': reset robot" << endl; cout << "'n': transite to the next state manually" << endl; cout << endl; cout << "'1': standing controller" << endl; cout << "'2': walking controller" << endl; // Run glut loop glutInit(&argc, argv); window.initWindow(640, 480, "Atlas Robot"); glutMainLoop(); return 0; }
// Solve for a balanced pose using IK Eigen::VectorXd solveIK(SkeletonPtr biped) { // Lesson 7 Eigen::VectorXd newPose = biped->getPositions(); return newPose; }
void setupWholeBodySolver(const SkeletonPtr& atlas) { // The default std::shared_ptr<dart::optimizer::GradientDescentSolver> solver = std::dynamic_pointer_cast<dart::optimizer::GradientDescentSolver>( atlas->getIK(true)->getSolver()); solver->setNumMaxIterations(10); size_t nDofs = atlas->getNumDofs(); double default_weight = 0.01; Eigen::VectorXd weights = default_weight * Eigen::VectorXd::Ones(nDofs); weights[2] = 0.0; weights[3] = 0.0; weights[4] = 0.0; weights[6] *= 0.2; weights[7] *= 0.2; weights[8] *= 0.2; Eigen::VectorXd lower_posture = Eigen::VectorXd::Constant( nDofs, -std::numeric_limits<double>::infinity()); lower_posture[0] = -0.35; lower_posture[1] = -0.35; lower_posture[5] = 0.600; lower_posture[6] = -0.1; lower_posture[7] = -0.1; lower_posture[8] = -0.1; Eigen::VectorXd upper_posture = Eigen::VectorXd::Constant( nDofs, std::numeric_limits<double>::infinity()); upper_posture[0] = 0.35; upper_posture[1] = 0.35; upper_posture[5] = 0.885; upper_posture[6] = 0.1; upper_posture[7] = 0.1; upper_posture[8] = 0.1; std::shared_ptr<RelaxedPosture> objective = std::make_shared<RelaxedPosture>( atlas->getPositions(), lower_posture, upper_posture, weights); atlas->getIK()->setObjective(objective); std::shared_ptr<dart::constraint::BalanceConstraint> balance = std::make_shared<dart::constraint::BalanceConstraint>(atlas->getIK()); atlas->getIK()->getProblem()->addEqConstraint(balance); // // Shift the center of mass towards the support polygon center while trying // // to keep the support polygon where it is // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM); // // Keep shifting the center of mass towards the center of the support // // polygon, even if it is already inside. This is useful for trying to // // optimize a stance // balance->setErrorMethod(dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM); // // Try to leave the center of mass where it is while moving the support // // polygon to be under the current center of mass location balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID); balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT); // // Try to leave the center of mass where it is while moving the support // // point that is closest to the center of mass // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_EDGE); // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT); // Note that using the FROM_EDGE error method is liable to leave the center of // mass visualization red even when the constraint was successfully solved. // This is because the constraint solver has a tiny bit of tolerance that // allows the Problem to be considered solved when the center of mass is // microscopically outside of the support polygon. This is an inherent risk of // using FROM_EDGE instead of FROM_CENTROID. // Feel free to experiment with the different balancing methods. You will find // that some work much better for user interaction than others. }