コード例 #1
0
ファイル: Main.cpp プロジェクト: bingjeff/dart
int main(int argc, char* argv[])
{
    // load a skeleton file
    FileInfoSkel<SkeletonDynamics> model;
    model.loadFile(DART_DATA_PATH"/skel/Chain.skel", SKEL);
    
    // create and initialize the world
    World *myWorld = new World();
    Vector3d gravity(0.0, -9.81, 0.0);
    myWorld->setGravity(gravity);
    myWorld->setTimeStep(1.0/2000);

    myWorld->addSkeleton((SkeletonDynamics*)model.getSkel());
    int nDof =  myWorld->getSkeleton(0)->getNumDofs();
    VectorXd initPose(nDof);
    for (int i = 0; i < nDof; i++)
        initPose[i] = random(-0.5, 0.5);
    myWorld->getSkeleton(0)->setPose(initPose);

    // create a window and link it to the world
    MyWindow window;
    window.setWorld(myWorld);
  
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Forward Simulation");
    glutMainLoop();

    return 0;
}
コード例 #2
0
ファイル: Main.cpp プロジェクト: Tarrasch/dart
int main(int argc, char* argv[])
{  
    FileInfoSkel<SkeletonDynamics> model;
    model.loadFile(DART_DATA_PATH"/skel/mobilManipulator.skel", SKEL);

    MyWindow window((SkeletonDynamics*)model.getSkel(), NULL);
   
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Hybrid");
    glutMainLoop();

    return 0;
}
コード例 #3
0
ファイル: Main.cpp プロジェクト: yutingye/RTQL8
int main(int argc, char* argv[])
{
    FileInfoSkel<SkeletonDynamics> model;
    model.loadFile(RTQL8_DATA_PATH"/skel/Chain.skel", SKEL);

    MyWindow window((SkeletonDynamics*)model.getSkel());
    
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Forward Simulation");
    glutMainLoop();

    return 0;
}
コード例 #4
0
ファイル: testKinematics.cpp プロジェクト: bingjeff/dart
/* ********************************************************************************************* */
TEST(KINEMATICS, VSK_LOADER) {
    using namespace Eigen;
    using namespace kinematics;
  
    FileInfoSkel<Skeleton> modelFile;
    modelFile.loadFile(DART_DATA_PATH"skel/Yuting.vsk");
    Skeleton* skel = modelFile.getSkel();

    EXPECT_TRUE(skel != NULL);

    EXPECT_EQ(skel->getNumDofs(), 66);
    EXPECT_EQ(skel->getNumNodes(), 30);
    EXPECT_EQ(skel->getNumMarkers(), 53);
}
コード例 #5
0
ファイル: teleop_gui.cpp プロジェクト: ehuang3/drchubo
//############################################################
//### PTHREAD START ROUTINE
//############################################################
    void* teleop_gui_t::start_routine(void *args)
    {
        teleop_gui_t *window = ((teleop_gui_t *) args);

        // 1. Load a local copy of Atlas
        DartLoader dart_loader;
        simulation::World *mWorld = dart_loader.parseWorld(DRC_DATA_PATH ROBOT_URDF);
        robotSkel = mWorld->getSkeleton(ROBOT_NAME);
        VectorXd dofs = robotSkel->getPose();
        robotSkel->setPose(dofs);

        // 2. Load the ground
        FileInfoSkel<SkeletonDynamics> model;
        model.loadFile(DRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
        ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
        ground->setName("ground");
        mWorld->addSkeleton(ground);
        dofs = ground->getPose();
        ground->setPose(dofs);
    
        // 3. Zero atlas's feet on ground
        kinematics::BodyNode* left_foot = robotSkel->getNode(ROBOT_LEFT_FOOT);
        kinematics::Shape* left_shoe = left_foot->getCollisionShape();
        // height from ankle to base of shoe
        foot2ground = left_shoe->getDim()(2)/2 - left_shoe->getTransform().matrix()(2,3);
        // height from ground center to surface
        double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
        // invert to get height below left ankle
        foot2ground *= -1;
        // set into ground
        dofs(2) = -ground_z;
        ground->setPose(dofs);

        // 4. Initialize GL stuff
        QUAD_OBJ_INIT;

        // 4. Finish init and 
        window->setWorld(mWorld);
        // fake argc, argv
        char *argv[1];
        int argc = 1;
        argv[0] = "robot-viz";
        // gl stuff
        glutInit(&argc, argv);
        window->initWindow(640, 480, "Robot Viz");
        glutMainLoop();

        return args;
    }
コード例 #6
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;

}
コード例 #7
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();

}
コード例 #8
0
ファイル: testKinematics.cpp プロジェクト: bingjeff/dart
/* ********************************************************************************************* */
TEST(KINEMATICS, TRANS_AND_DERIV) {
    using namespace Eigen;
    using namespace kinematics;
  
    FileInfoSkel<Skeleton> modelFile;
    bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk");
    ASSERT_TRUE(loadModelResult);
    
    Skeleton* skel = modelFile.getSkel();
    EXPECT_TRUE(skel != NULL);
    /* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */
    /* LOG(INFO) << "# Nodes  = " << skel->getNumNodes(); */


    FileInfoDof dofFile(skel);
    bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof");
    ASSERT_TRUE(loadDofResult);
    /* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */

    VectorXd pose = dofFile.getPoseAtFrame(0);
    skel->setPose(pose, true, true);

    const int nodeIndex = 1;
    BodyNode* node = skel->getNode(nodeIndex);
    EXPECT_TRUE(node != NULL);

    const double TOLERANCE = 0.000001;

    // Check the one global transform matrix
    const Matrix4d& W = skel->getNode(20)->getWorldTransform();
    Matrix4d W_truth;
    W_truth << -0.110662, 0.991044, 0.074734, 0.642841
        , -0.987564, -0.118099, 0.103775, 0.327496
        , 0.111672, -0.0623207, 0.991789, -0.12855
        , 0, 0, 0, 1;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE);
        }
    }

    // Check the derivative of one global transform matrix
    // const Matrix4d& Wq = skel->getNode(10)->mWq.at(4);
    const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4);
    Matrix4d Wq_truth;
    Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838
        ,-0.0175714, 0.00698451, 0.0149899, 0.00571836
        ,-0.992336, 0.0556535, 0.107702, 0.175059
        ,0, 0, 0, 0;
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE);
        }
    }


    // Check Jacobians
    BodyNode *nodecheck = skel->getNode(23);

    // Linear Jacobian
    const MatrixXd Jv = nodecheck->getJacobianLinear();
    MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols());
    Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535, 
        0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917,
        0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314;
    for (int i = 0; i < Jv.rows(); i++) {
        for (int j = 0; j < Jv.cols(); j++) {
            EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE);
        }
    }

    // Angular Jacobian
    const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose()
        * nodecheck->getJacobianAngular();
    MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols());
    Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389,
        0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729,
        0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159;

    for (int i = 0; i < Jwbody.rows(); i++) {
        for (int j = 0; j < Jwbody.cols(); j++) {
            EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE);
        }
    }
}