コード例 #1
0
// 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;
		}
	}
}
コード例 #2
0
ファイル: ContactDynamics.cpp プロジェクト: bingjeff/dart
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());
}
コード例 #3
0
ファイル: PointConstraint.cpp プロジェクト: yutingye/RTQL8
 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;
 }
コード例 #4
0
ファイル: ConstraintDynamics.cpp プロジェクト: ehuang3/dart
        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);
        }
コード例 #5
0
/* ******************************************************************************************** */
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;	
}
コード例 #6
0
ファイル: ContactDynamics.cpp プロジェクト: ehuang3/dart
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());
}
コード例 #7
0
/* ******************************************************************************************** */
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);
}
コード例 #8
0
ファイル: ContactDynamics.cpp プロジェクト: jmainpri/dart
        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);
            }
        }
コード例 #9
0
// 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;
}
コード例 #10
0
ファイル: Main.cpp プロジェクト: Tarrasch/dart
/* ********************************************************************************************* */
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;
}
コード例 #11
0
ファイル: main.cpp プロジェクト: XinyanGT/ROSZMPWalker
///////////////////////////////////////////////////////////////////////////////
// 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;

}
コード例 #12
0
ファイル: main.cpp プロジェクト: ehuang3/drchubo
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();

}