// TODO: Use link lengths in expectations explicitly
TEST(FORWARD_KINEMATICS, TWO_ROLLS) {
// Create the world
const double link1 = 1.5, link2 = 1.0;
SkeletonDynamics* robot = createTwoLinkRobot(Vector3d(0.3, 0.3, link1), DOF_ROLL, Vector3d(0.3, 0.3, link2),
DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector positions
const size_t numTests = 2;
Vector2d joints [numTests] = {Vector2d(0.0, M_PI/2.0), Vector2d(3*M_PI/4.0, -M_PI/4.0)};
Vector3d expectedPos [numTests] = {Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06)};
// Check each case by setting the joint values and obtaining the end-effector position
for(size_t i = 0; i < numTests; i++) {
robot->setConfig(twoLinkIndices, joints[i]);
Vector3d actual = robot->getNode("ee")->getWorldTransform().topRightCorner(3,1);
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality) {
cout << "Joint values: " << joints[i].transpose() << endl;
cout << "Actual pos: " << actual.transpose() << endl;
cout << "Expected pos: " << expectedPos[i].transpose() << endl;
}
}
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
mCollisionChecker->addCollisionSkeletonNode(skel->getNode(j));
mBodyIndexToSkelIndex.push_back(i);
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
VectorXd qDot = skel->getQDotVector();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
void ConstraintDynamics::addSkeleton(SkeletonDynamics* _newSkel)
{
mSkels.push_back(_newSkel);
int nSkels = mSkels.size();
int nNodes = _newSkel->getNumNodes();
for (int j = 0; j < nNodes; j++)
{
kinematics::BodyNode* node = _newSkel->getNode(j);
// TODO: (test)
if (node->getCollisionShape() == NULL)
continue;
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(nSkels-1);
}
// Add all body nodes into mCollisionChecker
int rows = mMInv.rows();
int cols = mMInv.cols();
if (!_newSkel->getImmobileState())
{
// Immobile objets have mass of infinity
rows += _newSkel->getMassMatrix().rows();
cols += _newSkel->getMassMatrix().cols();
}
Eigen::VectorXd newConstrForce;
if (!_newSkel->getImmobileState())
newConstrForce = VectorXd::Zero(_newSkel->getNumDofs());
mContactForces.push_back(newConstrForce);
mTotalConstrForces.push_back(newConstrForce);
mMInv = MatrixXd::Zero(rows, cols);
mTauStar = VectorXd::Zero(rows);
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < mSkels.size(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nDofs = skel->getNumDofs();
if (mSkels[i]->getImmobileState())
nDofs = 0;
sumNDofs += nDofs;
mIndices.push_back(sumNDofs);
}
mJ.resize(mSkels.size());
mPreJ.resize(mSkels.size());
mJMInv.resize(mSkels.size());
mZ = MatrixXd(rows, cols);
}
/* ******************************************************************************************** */
void computeExternal (const Vector6d& input, SkeletonDynamics& robot, Vector6d& external,
bool left) {
// Get the point transform wrench due to moving the affected position from com to sensor origin
// The transform is an identity with the bottom left a skew symmetric of the point translation
Matrix6d pTcom_sensor = MatrixXd::Identity(6,6);
pTcom_sensor.bottomLeftCorner<3,3>() << 0.0, -s2com(2), s2com(1), s2com(2), 0.0, -s2com(0),
-s2com(1), s2com(0), 0.0;
// Get the rotation between the world frame and the sensor frame by setting the arm values
// and the imu/waist values
const char* nodeName = left ? "lGripper" : "rGripper";
Matrix3d Rsw = robot.getNode(nodeName)->getWorldTransform().topLeftCorner<3,3>().transpose();
// vector <int> dofs;
// for(size_t i = 0; i < 25; i++) dofs.push_back(i);
// if(myDebug) cout << "\nq in computeExternal: " << robot.getConfig(dofs).transpose() << endl;
// Create the wrench with computed rotation to change the frame from the world to the sensor
Matrix6d pSsensor_world = MatrixXd::Identity(6,6);
pSsensor_world.topLeftCorner<3,3>() = Rsw;
pSsensor_world.bottomRightCorner<3,3>() = Rsw;
// Get the weight vector (note that we use the world frame for gravity so towards -y)
// static const double eeMass = 0.169; // kg - ft extension
Vector6d weightVector_in_world;
weightVector_in_world << 0.0, 0.0, -eeMass * 9.81, 0.0, 0.0, 0.0;
// Compute what the force and torque should be without any external values by multiplying the
// position and rotation transforms with the expected effect of the gravity
Vector6d wrenchWeight = pTcom_sensor * pSsensor_world * weightVector_in_world;
// Remove the effect from the sensor value and convert the wrench into the world frame
external = input - wrenchWeight;
external = pSsensor_world.transpose() * external;
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
// If the collision shape of the node is NULL, then the node is
// uncollidable object. We don't care uncollidable object in
// ContactDynamics.
if (node->getCollisionShape() == NULL)
continue;
if (node->getCollisionShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}
/* ******************************************************************************************** */
void computeOffset (double imu, double waist, const somatic_motor_t& lwa, const Vector6d& raw,
SkeletonDynamics& robot, Vector6d& offset, bool left) {
// Get the point transform wrench due to moving the affected position from com to sensor origin
// The transform is an identity with the bottom left a skew symmetric of the point translation
Matrix6d pTcom_sensor = MatrixXd::Identity(6,6);
pTcom_sensor.bottomLeftCorner<3,3>() << 0.0, -s2com(2), s2com(1), s2com(2), 0.0, -s2com(0),
-s2com(1), s2com(0), 0.0;
// Get the rotation between the world frame and the sensor frame.
robot.setConfig(imuWaist_ids, Vector2d(-imu + M_PI_2, waist));
robot.setConfig(left ? left_arm_ids : right_arm_ids, Map <Vector7d> (lwa.pos));
const char* nodeName = left ? "lGripper" : "rGripper";
Matrix3d R = robot.getNode(nodeName)->getWorldTransform().topLeftCorner<3,3>().transpose();
cout << "Transform : "<< endl << R << endl;
vector <int> dofs;
for(size_t i = 0; i < 25; i++) dofs.push_back(i);
cout << "\nq in computeExternal: " << robot.getConfig(dofs).transpose() << endl;
// Create the wrench with computed rotation to change the frame from the bracket to the sensor
Matrix6d pSsensor_bracket = MatrixXd::Identity(6,6);
pSsensor_bracket.topLeftCorner<3,3>() = R;
pSsensor_bracket.bottomRightCorner<3,3>() = R;
// Get the weight vector (note that we use the bracket frame for gravity so towards -y)
Vector6d weightVector_in_bracket;
weightVector_in_bracket << 0.0, 0.0, -eeMass * 9.81, 0.0, 0.0, 0.0;
// Compute what the force and torque should be without any external values by multiplying the
// position and rotation transforms with the expected effect of the gravity
Vector6d expectedFT = pTcom_sensor * pSsensor_bracket * weightVector_in_bracket;
pv(raw);
pv(expectedFT);
// Compute the difference between the actual and expected f/t values
offset = expectedFT - raw;
pv(offset);
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
mCollisionChecker = new SkeletonCollision();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
int rows = 0;
int cols = 0;
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
if (node->getShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
if (!mSkels[i]->getImmobileState()) {
// Immobile objets have mass of infinity
rows += skel->getMassMatrix().rows();
cols += skel->getMassMatrix().cols();
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mMInv = MatrixXd::Zero(rows, cols);
mTauStar = VectorXd::Zero(rows);
// Initialize the index vector:
// If we have 3 skeletons,
// mIndices[0] = 0
// mIndices[1] = nDof0
// mIndices[2] = nDof0 + nDof1
// mIndices[3] = nDof0 + nDof1 + nDof2
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nDofs = skel->getNumDofs();
if (mSkels[i]->getImmobileState())
nDofs = 0;
sumNDofs += nDofs;
mIndices.push_back(sumNDofs);
}
}
// Compute the wrench on the wheel as an effect of the wrench acting on the sensor
void computeWheelWrench(const Vector6d& wrenchSensor, SkeletonDynamics& robot, Vector6d& wheelWrench, bool left) {
// Get the position vector of the sensor with respect to the wheels
const char* nodeName = left ? "lGripper" : "rGripper";
Vector3d Tws = robot.getNode(nodeName)->getWorldTransform().topRightCorner<3,1>();
if(0 && myDebug) cout << Tws.transpose() << endl;
// Get the wrench shift operator to move the wrench from sensor origin to the wheel axis
// TODO: This shifting is to the origin of the world frame in dart. It works now because it is not
// being updated by the amc encoders. We need to shift the wrench to a frame having origin at the
// wheel axis.
Tws(2) -= 0.264;
Matrix6d pTsensor_wheel = MatrixXd::Identity(6,6);
pTsensor_wheel.bottomLeftCorner<3,3>() << 0.0, -Tws(2), Tws(1), Tws(2), 0.0, -Tws(0),
-Tws(1), Tws(0), 0.0;
// Shift the wrench from the sensor origin to the wheel axis
wheelWrench = pTsensor_wheel * wrenchSensor;
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
SkeletonDynamics LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNodeDynamics* node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHY");
Joint* joint = new Joint(NULL, node, "LHY");
add_XyzRpy(joint, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_YAW);
Shape* shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****
BodyNodeDynamics* parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHY");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHR");
joint = new Joint(parent_node, node, "LHR");
add_XyzRpy(joint, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHR");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHP");
joint = new Joint(parent_node, node, "LHP");
add_XyzRpy(joint, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new ShapeEllipsoid(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setVisualizationShape(shape1);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// Initialize the skeleton
LeftLegSkel.initSkel();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}
///////////////////////////////////////////////////////////////////////////////
// MAIN
///////////////////////////////////////////////////////////////////////////////
int main( int argc, char** argv ) {
// Command line inputs
bool show_gui = false;
bool use_ros = false;
walktype walk_type = walk_canned;
double walk_circle_radius = 5.0;
double walk_dist = 20;
double footsep_y = 0.085; // half of horizontal separation distance between feet
double foot_liftoff_z = 0.05; // foot liftoff height
double step_length = 0.15;
bool walk_sideways = false;
double com_height = 0.48; // height of COM above ANKLE
double com_ik_ascl = 0;
double zmpoff_y = 0; // lateral displacement between zmp and ankle
double zmpoff_x = 0;
double lookahead_time = 2.5;
double startup_time = 1.0;
double shutdown_time = 1.0;
double double_support_time = 0.05;
double single_support_time = 0.70;
size_t max_step_count = 4;
double zmp_jerk_penalty = 1e-8; // jerk penalty on ZMP controller
ik_error_sensitivity ik_sense = ik_strict;
// Parse command line inputs
const struct option long_options[] = {
{ "show-gui", no_argument, 0, 'g' },
{ "use-ros", no_argument, 0, 'R' },
{ "ik-errors", required_argument, 0, 'I' },
{ "walk-type", required_argument, 0, 'w' },
{ "walk-distance", required_argument, 0, 'D' },
{ "walk-circle-radius", required_argument, 0, 'r' },
{ "max-step-count", required_argument, 0, 'c' },
{ "foot-separation-y", required_argument, 0, 'y' },
{ "foot-liftoff-z", required_argument, 0, 'z' },
{ "step-length", required_argument, 0, 'l' },
{ "walk-sideways", no_argument, 0, 'S' },
{ "com-height", required_argument, 0, 'h' },
{ "comik-angle-weight", required_argument, 0, 'a' },
{ "zmp-offset-y", required_argument, 0, 'Y' },
{ "zmp-offset-x", required_argument, 0, 'X' },
{ "lookahead-time", required_argument, 0, 'T' },
{ "startup-time", required_argument, 0, 'p' },
{ "shutdown-time", required_argument, 0, 'n' },
{ "double-support-time", required_argument, 0, 'd' },
{ "single-support-time", required_argument, 0, 's' },
{ "zmp-jerk-penalty", required_argument, 0, 'P' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
const char* short_options = "gRI:w:D:r:c:y:z:l:Sh:a:Y:X:T:p:n:d:s:P:H";
int opt, option_index;
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 ) {
switch (opt) {
case 'g': show_gui = true; break;
case 'R': use_ros = true; break;
case 'I': ik_sense = getiksense(optarg); break;
case 'w': walk_type = getwalktype(optarg); break;
case 'D': walk_dist = getdouble(optarg); break;
case 'r': walk_circle_radius = getdouble(optarg); break;
case 'c': max_step_count = getlong(optarg); break;
case 'y': footsep_y = getdouble(optarg); break;
case 'z': foot_liftoff_z = getdouble(optarg); break;
case 'l': step_length = getdouble(optarg); break;
case 'S': walk_sideways = true; break;
case 'h': com_height = getdouble(optarg); break;
case 'a': com_ik_ascl = getdouble(optarg); break;
case 'Y': zmpoff_y = getdouble(optarg); break;
case 'X': zmpoff_x = getdouble(optarg); break;
case 'T': lookahead_time = getdouble(optarg); break;
case 'p': startup_time = getdouble(optarg); break;
case 'n': shutdown_time = getdouble(optarg); break;
case 'd': double_support_time = getdouble(optarg); break;
case 's': single_support_time = getdouble(optarg); break;
case 'P': zmp_jerk_penalty = getdouble(optarg); break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
/* Initialize ROS */
double frequency = 200;
//FIXME: ROS dependent
if(use_ros) {
ros::init( argc, argv, "publish_and_readonce" );
rosnode = new ros::NodeHandle();
loop_rate = new ros::Rate(frequency);
ros::Time last_ros_time_;
// Wait until sim is active (play)
bool wait = true;
while( wait ) {
last_ros_time_ = ros::Time::now();
if( last_ros_time_.toSec() > 0 ) {
wait = false;
}
}
// init ros joints
RosJointInit();
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/atlas/joint_states", 1, GetJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
jointStatesSo.transport_hints = ros::TransportHints().unreliable();
ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo);
pub_joint_commands_ =
rosnode->advertise<osrf_msgs::JointCommands>(
"/atlas/joint_commands", 1, true);
}
/* Initialize AK */
if(!_ak) {
DartLoader dart_loader;
World *mWorld = dart_loader.parseWorld(ATLAS_DATA_PATH "atlas/atlas_world.urdf");
_atlas = mWorld->getSkeleton("atlas");
_ak = new AtlasKinematics();
_ak->init(_atlas);
}
_atlas->setPose(_atlas->getPose().setZero(), true);
AtlasKinematics *AK = _ak;
/* Begin generating trajectories */
/* Trajectory that stores dof ticks */
vector<VectorXd> joint_traj;
/* Setup dofs initial conditions */
/* Relax */
VectorXd dofs = _atlas->getPose().setZero();
_atlas->setPose(dofs, true);
const int relax_ticks = 1000;
Relax(AK, _atlas, dofs, joint_traj, relax_ticks);
/* Walking variables */
IK_Mode mode[NUM_MANIPULATORS];
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
Vector3d comDelta = Vector3d::Zero();
Vector3d leftDelta = Vector3d::Zero();
Vector3d rightDelta = Vector3d::Zero();
int N = 0;
Matrix4d Twm[NUM_MANIPULATORS];
Twm[MANIP_L_FOOT] = AK->getLimbTransB(_atlas, MANIP_L_FOOT);
Twm[MANIP_R_FOOT] = AK->getLimbTransB(_atlas, MANIP_R_FOOT);
Matrix4d Twb;
Twb.setIdentity();
VectorXd dofs_save;
/* Move COM down */
comDelta << 0, 0, -0.04;
leftDelta.setZero();
rightDelta.setZero();
const int com_ticks = 1000;
genCOMIKTraj(AK, _atlas, Twb, Twm, dofs, comDelta, leftDelta, rightDelta, joint_traj, com_ticks);
/* ZMP Walking */
/* ZMP parameters */
// number of steps to walk
int numSteps = 12;
// lenght of a half step
double stepLength = 0.15;
// half foot seperation
dofs_save = _atlas->getPose();
cout << "********************************************" << endl;
cout << "Start ZMP walking" << endl;
cout << "*************************************" << endl;
cout << "POS ERROR: " << (dofs_save - dofs).norm() << endl;
cout << "*************************************" << endl;
_atlas->setPose(dofs);
double footSeparation = (AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(1,3) -
AK->getLimbTransW(_atlas, Twb, MANIP_R_FOOT)(1,3) ) / 2;
cout << "Half foot seperation: " << footSeparation << endl;
// one step time
double stepDuration = 1.0;
// move ZMP time
double slopeTime = 0.15;
// keep ZMP time
double levelTime = 0.85;
// command sending period
double dt = 1/frequency;
// height of COM
double zg = AK->getCOMW(_atlas, Twb)(2) - AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(2,3);
cout << "zg " << zg << endl;
// preview how many steps
int numPreviewSteps = 2;
// double Qe = 1;
// double R = 1e-6;
double Qe = 1e7;
double R = 10;
/****************************************
* Generate joint trajectory
***************************************/
gZU.setParameters( dt, 9.81, dofs );
gZU.generateZmpPositions( numSteps, true,
stepLength, footSeparation,
stepDuration,
slopeTime,
levelTime );
gZU.getControllerGains( Qe, R, zg, numPreviewSteps );
gZU.generateCOMPositions();
gZU.getJointTrajectories();
gZU.print( "jointsWholeBody.txt", gZU.mWholeBody );
gZU.mDartDofs;
joint_traj.insert(joint_traj.end(), gZU.mDartDofs.begin(), gZU.mDartDofs.end());
// Bake me some GUI viz
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(ATLAS_DATA_PATH"/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
vector<SkeletonDynamics *> skels;
skels.push_back(ground);
skels.push_back(_atlas);
ZmpGUI gui(skels);
gui.bake(joint_traj);
glutInit(&argc, argv);
gui.initWindow(640, 480, "Atlas ZMP Walking");
glutMainLoop();
/**************************************
* Publish joint trajectory
****************************************/
//FIXME: ROS Dependent
//MoveJointTractory(AK, _atlas, dofs, gZU.mWholeBody, 20, 20, 20, 1.2, 1.2, 1.2);
return 0;
}
int main(int argc, char* argv[])
{
////////////////////////////////////////////////////////////////////////////
/// SETUP WORLD
////////////////////////////////////////////////////////////////////////////
// load a robot
DartLoader dart_loader;
string robot_file = VRC_DATA_PATH;
robot_file = argv[1];
World *mWorld = dart_loader.parseWorld(VRC_DATA_PATH ROBOT_URDF);
SkeletonDynamics *robot = mWorld->getSkeleton("atlas");
atlas_state_t state;
state.init(robot);
atlas_kinematics_t kin;
kin.init(robot);
robot->setPose(robot->getPose().setZero());
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(VRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
mWorld->addSkeleton(ground);
// Zero atlas's feet at ground
kinematics::BodyNode* foot = robot->getNode(ROBOT_LEFT_FOOT);
kinematics::Shape* shoe = foot->getCollisionShape();
double body_z = -shoe->getTransform().matrix()(2,3) + shoe->getDim()(2)/2;
body_z += -foot->getWorldTransform()(2,3);
VectorXd robot_dofs = robot->getPose();
cout << "body_z = " << body_z << endl;
state.dofs(2) = body_z;
robot->setPose(state.dart_pose());
cout << robot->getNode(ROBOT_BODY)->getWorldTransform() << endl;
// Zero ground
double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
ground->getNode("ground1_root")->getVisualizationShape()->setColor(Vector3d(0.5,0.5,0.5));
VectorXd ground_dofs = ground->getPose();
ground_dofs(1) = foot->getWorldTransform()(1,3);
ground_dofs(2) = -ground_z;
cout << "ground z = " << ground_z << endl;
ground->setPose(ground_dofs);
////////////////////////////////////////////////////////////////////////////
/// COM IK
////////////////////////////////////////////////////////////////////////////
Vector3d world_com;
Isometry3d end_effectors[NUM_MANIPULATORS];
IK_Mode mode[NUM_MANIPULATORS];
BodyNode* left_foot = state.robot()->getNode(ROBOT_LEFT_FOOT);
BodyNode* right_foot = state.robot()->getNode(ROBOT_RIGHT_FOOT);
end_effectors[MANIP_L_FOOT] = state.robot()->getNode(ROBOT_LEFT_FOOT)->getWorldTransform();
end_effectors[MANIP_R_FOOT] = state.robot()->getNode(ROBOT_RIGHT_FOOT)->getWorldTransform();
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
world_com = state.robot()->getWorldCOM();
world_com(2) -= 0.1;
Isometry3d body;
state.get_body(body);
body.rotate(AngleAxisd(M_PI/4, Vector3d::UnitY()));
state.set_body(body);
kin.com_ik(world_com, end_effectors, mode, state);
robot->setPose(state.dart_pose());
MyWindow window;
window.setWorld(mWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Robot Viz");
glutMainLoop();
}
