vruiMatrix *SGVruiMatrix::preTranslated(double x, double y, double z, vruiMatrix *matrix) { Matrix4d translate; translate.setIdentity(); translate.setTranslate(x, y, z); SGVruiMatrix *osgVruiMatrix = dynamic_cast<SGVruiMatrix *>(matrix); translate.mult(osgVruiMatrix->matrix); this->matrix = translate; return this; }
/////////////////////////////////////////////////////////////////////////////// // 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; }
// Current code only works for the left leg with only one constraint VectorXd MyWorld::updateGradients() { mJ.setZero(); mC.setZero(); // compute c(q) //std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl; mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget; std::cout << "C(q) = " << mC << std::endl; // compute J(q) Vector4d offset; offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates //Setup vars BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode(); Joint *joint = node->getParentJoint(); Matrix4d worldToParent; Matrix4d parentToJoint; //Declare Vars Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d Matrix4d R; Matrix4d R1; Matrix4d R2; Matrix4d jointToChild; Vector4d jCol; int colIndex; //TODO: Might want to change this to check if root using given root fcn //Iterate until we get to the root node while(true) { //std::cout << "HAMY DEBUG: Beginning new looop" << std::endl; if(node->getParentBodyNode() == NULL) { worldToParent = worldToParent.setIdentity(); } else { worldToParent = node->getParentBodyNode()->getTransform().matrix(); } parentToJoint = joint->getTransformFromParentBodyNode().matrix(); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix(); //TODO: R1, R2, ... Rn code depending on DOFs int nDofs = joint->getNumDofs(); //std::cout << "HAMY: nDofs=" << nDofs << std::endl; //Can only have up to 3 DOFs on any one piece switch(nDofs){ case 1: //std::cout << "HAMY: 1 nDOF" << std::endl; dR = joint->getTransformDerivative(0); jCol = worldToParent * parentToJoint * dR * jointToChild * offset; colIndex = joint->getIndexInSkeleton(0); mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset; break; case 2: //std::cout << "HAMY: 2 nDOF" << std::endl; dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d R = joint->getTransform(1).matrix(); jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix(); jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset; colIndex = joint->getIndexInSkeleton(0); mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol dR = joint->getTransformDerivative(1); R = joint->getTransform(0).matrix(); jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset; colIndex = joint->getIndexInSkeleton(1); mJ.col(colIndex) = jCol.head(3); offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd break; case 3: //std::cout << "HAMY: 3 nDOF" << std::endl; dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d R1 = joint->getTransform(1).matrix(); R2 = joint->getTransform(2).matrix(); jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix(); jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset; colIndex = joint->getIndexInSkeleton(0); mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J R1 = joint->getTransform(0).matrix(); dR = joint->getTransformDerivative(1); R2 = joint->getTransform(2).matrix(); jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset; colIndex = joint->getIndexInSkeleton(1); mJ.col(colIndex) = jCol.head(3); R1 = joint->getTransform(0).matrix(); R2 = joint->getTransform(1).matrix(); dR = joint->getTransformDerivative(2); jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset; colIndex = joint->getIndexInSkeleton(2); mJ.col(colIndex) = jCol.head(3); offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset; break; default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl; break; } if(node != mSkel->getRootBodyNode()) { //std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl; node = node->getParentBodyNode(); // return NULL if node is the root node joint = node->getParentJoint(); } else { break; } } // compute gradients VectorXd gradients = 2 * mJ.transpose() * mC; return gradients; }