RAD_Skeleton::RAD_Skeleton(Skeleton skel){
frame = skel.getFrame();
addJoint(CENT,skel.getJoint(CENT));
addJoint(HEAD,skel.getJoint(HEAD));
addJoint(HANDLEFT,skel.getJoint(HANDLEFT));
addJoint(HANDRIGHT,skel.getJoint(HANDRIGHT));
addJoint(FOOTLEFT,skel.getJoint(FOOTLEFT));
addJoint(FOOTRIGHT,skel.getJoint(FOOTRIGHT));
addThetaRho(HEAD,HANDLEFT);
addThetaRho(HANDLEFT,FOOTLEFT);
addThetaRho(FOOTLEFT,FOOTRIGHT);
addThetaRho(FOOTRIGHT,HANDRIGHT);
addThetaRho(HANDRIGHT,HEAD);
}
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());
}
