// 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.
}
