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; }