void WorldConstructor::Debug()
{
return;
// Create a humanoid controller
//msHumanoid = new bioloidgp::robot::HumanoidController(robot, msWorld->getConstraintSolver());
DartLoader urdfLoader;
Skeleton* robot
= urdfLoader.parseSkeleton(
DATA_DIR"/urdf/BioloidGP/BioloidGP.URDF");
robot->enableSelfCollision();
msWorld->withdrawSkeleton(msHumanoid->robot());
msWorld->addSkeleton(robot);
msHumanoid->reset();
Skeleton* tmp = msHumanoid->robot();
msHumanoid->robot() = robot;
msHumanoid->setJointDamping(0.0);
msHumanoid->reset();
Eigen::VectorXd qOrig = tmp->getPositions();
Eigen::VectorXd q = msHumanoid->robot()->getPositions();
Eigen::VectorXd err = qOrig - q;
Eigen::VectorXd qDotOrig = tmp->getVelocities();
Eigen::VectorXd qDot = msHumanoid->robot()->getVelocities();
Eigen::VectorXd errDot = qDotOrig - qDot;
}
void WorldConstructor::commonConstruction(World* world)
{
DecoConfig::GetSingleton()->GetDouble("Sim", "TimeStep", msTimeStep);
world->setTimeStep(msTimeStep);
// Set gravity of the world
world->setGravity(Eigen::Vector3d(0.0, -9.81, 0));
dart::constraint::ContactConstraint::setErrorReductionParameter(0.0);
dart::constraint::ContactConstraint::setMaxErrorReductionVelocity(0.1);
// // 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");
int isHybridDynamics = 0;
DecoConfig::GetSingleton()->GetInt("Sim", "HybridDynamics", isHybridDynamics);
if (isHybridDynamics)
{
dart::dynamics::Joint* joint0 = robot->getJoint(0);
joint0->setActuatorType(dart::dynamics::Joint::PASSIVE);
for (size_t i = 1; i < robot->getNumBodyNodes(); ++i)
{
dart::dynamics::Joint* joint = robot->getJoint(i);
joint->setActuatorType(dart::dynamics::Joint::VELOCITY);
}
}
robot->enableSelfCollision();
world->addSkeleton(robot);
world->addSkeleton(ground);
// Create a humanoid controller
msHumanoid = new bioloidgp::robot::HumanoidController(robot, world->getConstraintSolver());
}
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;
}
