C++ (Cpp) Skeleton::getMass示例

int main(int argc, char* argv[])
{
   // google::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging((const char*)argv[0]);
    // Define logging flag
    FLAGS_alsologtostderr = true;
    FLAGS_minloglevel = google::INFO;
#ifndef _DEBUG
    FLAGS_log_dir = "./glog/";
#endif
    LOG(INFO) << "BioloidGP program begins...";

    
    srand( (unsigned int) time (NULL) );
	DecoConfig::GetSingleton()->Init("../config.ini");

    World* myWorld = new World;
    myWorld->setTimeStep(0.017);



    // // Load ground and Atlas robot and add them to the world
    DartLoader urdfLoader;
    Skeleton* ground = urdfLoader.parseSkeleton(
        DATA_DIR"/sdf/ground.urdf");
    Skeleton* robot
        = urdfLoader.parseSkeleton(
            DATA_DIR"/urdf/BioloidGP/BioloidGP.URDF");
    robot->enableSelfCollision();

	//dart::simulation::World* testWorld
	//	= dart::utils::SkelParser::readWorld("../../data/skel/fullbody1.skel");

	Skeleton* mocapSkel = ReadCMUSkeleton("../../mocap/oneFootBalance.asf");
	
    myWorld->addSkeleton(robot);
    myWorld->addSkeleton(ground);
	myWorld->addSkeleton(mocapSkel);
	AddMarkers(mocapSkel, robot);

    // Print some info
    LOG(INFO) << "robot.mass = " << robot->getMass();

    // Set gravity of the world
    myWorld->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
	//AddWeldConstraint(myWorld);

    // Create a humanoid controller
    bioloidgp::robot::HumanoidController* con =
        new bioloidgp::robot::HumanoidController(robot, myWorld->getConstraintSolver());
	
	//Eigen::VectorXd q6 = Eigen::VectorXd::Zero(6);
	//q6[4] = 0;
	//con->setFreeDofs(q6);

    // Create a window and link it to the world
    // MyWindow window(new Controller(robot, myWorld->getConstraintSolver()));

	MocapReader mocapReader;
	string fileName;
	DecoConfig::GetSingleton()->GetString("Mocap", "OriginalFileName", fileName);
	mocapReader.Read(fileName);

	fileName.replace(fileName.length() - 4, 4, ".state");
	SupportInfo supportInfo(fileName);
	supportInfo.SetSkeletons(mocapSkel, con->robot());
	//supportInfo.SetLeftGlobal(0.1 * Eigen::Vector3d(0, 0, 1));
	supportInfo.SetLeftGlobal( 0.1 * Eigen::Vector3d(-1, 0, 0));
    MyWindow window(con);
    window.setWorld(myWorld);
	window.setMocap(&mocapReader);
	window.setSupportInfo(&supportInfo);
    // Print manual
    LOG(INFO) << "space bar: simulation on/off";
    LOG(INFO) << "'p': playback/stop";
    LOG(INFO) << "'[' and ']': play one frame backward and forward";
    LOG(INFO) << "'v': visualization on/off";
    LOG(INFO) << endl;

    // Run glut loop
    glutInit(&argc, argv);
    window.initWindow(1280, 720, "BioloidGP Robot - with Dart4.0");

    glutMainLoop();

    return 0;
}