//------------------------------------------------------------------------------------
void stopPlayback()
{
SkeletonMap &map = skeleton::SkeletonManager::getInstance().getSkeletons();
for (SkeletonMap::iterator it = map.begin(); it!=map.end(); ++it) {
SkeletonPtr skeleton = it->second;
skeleton->stopPlayback();
}
}
//------------------------------------------------------------------------------------
void setLoopPlayback(bool bLoop)
{
SkeletonMap &map = skeleton::SkeletonManager::getInstance().getSkeletons();
for (SkeletonMap::iterator it = map.begin(); it!=map.end(); ++it) {
SkeletonPtr skeleton = it->second;
ofLoopType loop = bLoop ? OF_LOOP_NORMAL : OF_LOOP_NONE;
skeleton->setLoopPlayback(loop);
}
}
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
{
// Set the color of all the shapes in the object
for(size_t i=0; i < object->getNumBodyNodes(); ++i)
{
BodyNode* bn = object->getBodyNode(i);
for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
bn->getVisualizationShape(j)->setColor(color);
}
}
void setupStartConfiguration(const SkeletonPtr& atlas)
{
// Squat with the right leg
atlas->getDof("r_leg_hpy")->setPosition(-45.0*M_PI/180.0);
atlas->getDof("r_leg_kny")->setPosition( 90.0*M_PI/180.0);
atlas->getDof("r_leg_aky")->setPosition(-45.0*M_PI/180.0);
// Squat with the left left
atlas->getDof("l_leg_hpy")->setPosition(-45.0*M_PI/180.0);
atlas->getDof("l_leg_kny")->setPosition( 90.0*M_PI/180.0);
atlas->getDof("l_leg_aky")->setPosition(-45.0*M_PI/180.0);
// Get the right arm into a comfortable position
atlas->getDof("r_arm_shx")->setPosition( 65.0*M_PI/180.0);
atlas->getDof("r_arm_ely")->setPosition( 90.0*M_PI/180.0);
atlas->getDof("r_arm_elx")->setPosition(-90.0*M_PI/180.0);
atlas->getDof("r_arm_wry")->setPosition( 65.0*M_PI/180.0);
// Get the left arm into a comfortable position
atlas->getDof("l_arm_shx")->setPosition(-65.0*M_PI/180.0);
atlas->getDof("l_arm_ely")->setPosition( 90.0*M_PI/180.0);
atlas->getDof("l_arm_elx")->setPosition( 90.0*M_PI/180.0);
atlas->getDof("l_arm_wry")->setPosition( 65.0*M_PI/180.0);
// Prevent the knees from bending backwards
atlas->getDof("r_leg_kny")->setPositionLowerLimit( 10*M_PI/180.0);
atlas->getDof("l_leg_kny")->setPositionLowerLimit( 10*M_PI/180.0);
}
// Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer)
void setVelocityAccuators(SkeletonPtr biped)
{
Joint* wheel1 = biped->getJoint("joint_front_left");
Joint* wheel2 = biped->getJoint("joint_front_right");
Joint* wheel3 = biped->getJoint("joint_back_left");
Joint* wheel4 = biped->getJoint("joint_back_right");
wheel1->setActuatorType(Joint::VELOCITY);
wheel2->setActuatorType(Joint::VELOCITY);
wheel3->setActuatorType(Joint::VELOCITY);
wheel4->setActuatorType(Joint::VELOCITY);
}
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
{
// Set the color of all the shapes in the object
for(size_t i=0; i < object->getNumBodyNodes(); ++i)
{
BodyNode* bn = object->getBodyNode(i);
auto visualShapeNodes = bn->getShapeNodesWith<VisualAddon>();
for(auto visualShapeNode : visualShapeNodes)
visualShapeNode->getVisualAddon()->setColor(color);
}
}
// Load a skateboard model and connect it to the biped model via an Euler joint
// (Lesson 5 Answer)
void modifyBipedWithSkateboard(SkeletonPtr biped)
{
// Load the Skeleton from a file
WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");
SkeletonPtr skateboard = world->getSkeleton(0);
EulerJoint::Properties properties = EulerJoint::Properties();
properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
skateboard->getRootBodyNode()->moveTo<EulerJoint>
(biped->getBodyNode("h_heel_left"), properties);
}
int main(int argc, char* argv[])
{
// Create empty soft world
WorldPtr myWorld(new World);
// Load ground and Atlas robot and add them to the world
DartLoader urdfLoader;
SkeletonPtr ground = urdfLoader.parseSkeleton(
DART_DATA_PATH"sdf/atlas/ground.urdf");
// SkeletonPtr atlas = SoftSdfParser::readSkeleton(
// DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.sdf");
SkeletonPtr atlas
= SoftSdfParser::readSkeleton(
DART_DATA_PATH"sdf/atlas/atlas_v3_no_head_soft_feet.sdf");
myWorld->addSkeleton(atlas);
myWorld->addSkeleton(ground);
// Set initial configuration for Atlas robot
VectorXd q = atlas->getPositions();
q[0] = -0.5 * DART_PI;
atlas->setPositions(q);
// Set gravity of the world
myWorld->setGravity(Vector3d(0.0, -9.81, 0.0));
// Create a window and link it to the world
MyWindow window(new Controller(atlas, myWorld->getConstraintSolver()));
window.setWorld(myWorld);
// Print manual
cout << "space bar: simulation on/off" << endl;
cout << "'p': playback/stop" << endl;
cout << "'[' and ']': play one frame backward and forward" << endl;
cout << "'v': visualization on/off" << endl;
cout << endl;
cout << "'h': harness pelvis on/off" << endl;
cout << "'j': harness left foot on/off" << endl;
cout << "'k': harness right foot on/off" << endl;
cout << "'r': reset robot" << endl;
cout << "'n': transite to the next state manually" << endl;
cout << endl;
cout << "'1': standing controller" << endl;
cout << "'2': walking controller" << endl;
// Run glut loop
glutInit(&argc, argv);
window.initWindow(640, 480, "Atlas Robot");
glutMainLoop();
return 0;
}
void check_self_consistency(SkeletonPtr skeleton)
{
for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
{
BodyNode* bn = skeleton->getBodyNode(i);
EXPECT_TRUE(bn->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);
Joint* joint = bn->getParentJoint();
EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);
for(size_t j=0; j<joint->getNumDofs(); ++j)
{
DegreeOfFreedom* dof = joint->getDof(j);
EXPECT_TRUE(dof->getIndexInJoint() == j);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
for(size_t i=0; i<skeleton->getNumDofs(); ++i)
{
DegreeOfFreedom* dof = skeleton->getDof(i);
EXPECT_TRUE(dof->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
int main(int argc, char* argv[])
{
SkeletonPtr floor = createFloor();
// Lesson 1
SkeletonPtr biped = loadBiped();
// Lesson 2
setInitialPose(biped);
// Lesson 5
modifyBipedWithSkateboard(biped);
// Lesson 6
setVelocityAccuators(biped);
// Lesson 7
Eigen::VectorXd balancedPose = solveIK(biped);
biped->setPositions(balancedPose);
WorldPtr world = std::make_shared<World>();
world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
#if HAVE_BULLET_COLLISION
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
#endif
world->addSkeleton(floor);
world->addSkeleton(biped);
// Create a window for rendering the world and handling user input
MyWindow window(world);
// Print instructions
std::cout << "'.': forward push" << std::endl;
std::cout << "',': backward push" << std::endl;
std::cout << "'s': increase skateboard forward speed" << std::endl;
std::cout << "'a': increase skateboard backward speed" << std::endl;
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': replay simulation" << std::endl;
std::cout << "'v': Turn contact force visualization on/off" << std::endl;
std::cout <<
"'[' and ']': replay one frame backward and forward" << std::endl;
// Initialize glut, initialize the window, and begin the glut event loop
glutInit(&argc, argv);
window.initWindow(640, 480, "Multi-Pendulum Tutorial");
glutMainLoop();
}
/// Constructor
Controller(const SkeletonPtr& biped)
: mBiped(biped),
mPreOffset(0.0),
mSpeed(0.0)
{
int nDofs = mBiped->getNumDofs();
mForces = Eigen::VectorXd::Zero(nDofs);
mKp = Eigen::MatrixXd::Identity(nDofs, nDofs);
mKd = Eigen::MatrixXd::Identity(nDofs, nDofs);
for(std::size_t i = 0; i < 6; ++i)
{
mKp(i, i) = 0.0;
mKd(i, i) = 0.0;
}
for(std::size_t i = 6; i < biped->getNumDofs(); ++i)
{
mKp(i, i) = 1000;
mKd(i, i) = 50;
}
setTargetPositions(mBiped->getPositions());
}
//==============================================================================
Eigen::MatrixXd standardJacobian(
const SkeletonPtr& skeleton,
const Eigen::VectorXd& q,
const std::vector<std::size_t>& active_indices,
JacobianNode* node)
{
skeleton->setPositions(active_indices, q);
Eigen::MatrixXd J = skeleton->getJacobian(node).bottomRows<3>();
Eigen::MatrixXd reduced_J(3, q.size());
for(int i=0; i < q.size(); ++i)
reduced_J.col(i) = J.col(active_indices[i]);
return reduced_J;
}
TEST(Skeleton, ZeroDofJointInReferential)
{
// This is a regression test which makes sure that the BodyNodes of
// ZeroDofJoints will be correctly included in linkages.
SkeletonPtr skel = Skeleton::create();
BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
BodyNode* zeroDof2 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
BranchPtr branch = Branch::create(skel->getBodyNode(0));
EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes());
EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX);
EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX);
}
//------------------------------------------------------------------------------------
void finishRecording()
{
bOpeningRecorder = false;
SkeletonMap &map = skeleton::SkeletonManager::getInstance().getSkeletons();
if (map.empty()) {
ofLogNotice("Recorder") << "No skeleton avairable";
return;
}
for (SkeletonMap::iterator it = map.begin(); it!=map.end(); ++it) {
SkeletonPtr skeleton = it->second;
skeleton->stopRecording();
skeleton->finishRecording();
}
}
//------------------------------------------------------------------------------------
void SkeletonManager::addSkeleton(const string &hostName)
{
if (mSkeletonMap.find(hostName)==mSkeletonMap.end()) {
SkeletonPtr skl = ram::skeleton::createSkeleton();
skl->setup(hostName, DEFAULT_SKELETON_SETTINGS);
mSkeletonMap[hostName] = skl; /// insert
UI::getInstance().updateDeviceList();/// update ui drop down menu
UI::getInstance().getInspector().setDevice(skl->getHostName()); /// set ui current skeleton
UI::getInstance().getInspector().setSettingsFilePath(DEFAULT_SKELETON_SETTINGS);
}
else {
ofxThrowException(ofxException, "Skeleton \""+hostName+"\" already exsists");
}
}
// Load a biped model and enable joint limits and self-collision
// (Lesson 1 Answer)
SkeletonPtr loadBiped()
{
// Create the world with a skeleton
WorldPtr world = SkelParser::readWorld(DART_DATA_PATH"skel/biped.skel");
assert(world != nullptr);
SkeletonPtr biped = world->getSkeleton("biped");
// Set joint limits
for(size_t i = 0; i < biped->getNumJoints(); ++i)
biped->getJoint(i)->setPositionLimitEnforced(true);
// Enable self collision check but ignore adjacent bodies
biped->enableSelfCollision();
return biped;
}
void enableDragAndDrops(osgDart::Viewer& viewer, const SkeletonPtr& atlas)
{
// Turn on drag-and-drop for the whole Skeleton
for(size_t i=0; i < atlas->getNumBodyNodes(); ++i)
viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false);
for(size_t i=0; i < atlas->getNumEndEffectors(); ++i)
{
EndEffector* ee = atlas->getEndEffector(i);
if(!ee->getIK())
continue;
// Check whether the target is an interactive frame, and add it if it is
if(const auto& frame = std::dynamic_pointer_cast<osgDart::InteractiveFrame>(
ee->getIK()->getTarget()))
viewer.enableDragAndDrop(frame.get());
}
}
void checkForBodyNodes(
size_t& count,
const ReferentialSkeletonPtr& refSkel,
const SkeletonPtr& skel,
const std::string& name,
Args ... args)
{
bool contains = refSkel->getIndexOf(skel->getBodyNode(name)) != INVALID_INDEX;
EXPECT_TRUE(contains);
if(!contains)
{
dtmsg << "The ReferentialSkeleton [" << refSkel->getName() << "] does NOT "
<< "contain the BodyNode [" << name << "] of the Skeleton ["
<< skel->getName() << "]\n";
}
++count;
checkForBodyNodes(count, refSkel, skel, args...);
}
SkeletonPtr createGround()
{
// Create a Skeleton to represent the ground
SkeletonPtr ground = Skeleton::create("ground");
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
double thickness = 0.01;
tf.translation() = Eigen::Vector3d(0,0,-thickness/2.0);
WeldJoint::Properties joint;
joint.mT_ParentBodyToJoint = tf;
ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
ShapePtr groundShape =
std::make_shared<BoxShape>(Eigen::Vector3d(10,10,thickness));
groundShape->setColor(dart::Color::Blue(0.2));
ground->getBodyNode(0)->addVisualizationShape(groundShape);
ground->getBodyNode(0)->addCollisionShape(groundShape);
return ground;
}
TeleoperationWorld(WorldPtr _world, SkeletonPtr _robot)
: osgDart::WorldNode(_world),
mAtlas(_robot),
iter(0),
l_foot(_robot->getEndEffector("l_foot")),
r_foot(_robot->getEndEffector("r_foot"))
{
mMoveComponents.resize(NUM_MOVE, false);
mAnyMovement = false;
}
int main(int argc, char* argv[])
{
// Create an empty Skeleton with the name "pendulum"
SkeletonPtr pendulum = Skeleton::create("pendulum");
// Add each body to the last BodyNode in the pendulum
BodyNode* bn = makeRootBody(pendulum, "body1");
bn = addBody(pendulum, bn, "body2");
bn = addBody(pendulum, bn, "body3");
bn = addBody(pendulum, bn, "body4");
bn = addBody(pendulum, bn, "body5");
// Set the initial position of the first DegreeOfFreedom so that the pendulum
// starts to swing right away
pendulum->getDof(1)->setPosition(120 * M_PI / 180.0);
// Create a world and add the pendulum to the world
WorldPtr world(new World);
world->addSkeleton(pendulum);
// Create a window for rendering the world and handling user input
MyWindow window(world);
// Print instructions
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'p': replay simulation" << std::endl;
std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl;
std::cout << "'-': Change sign of applied joint torques" << std::endl;
std::cout << "'q': Increase joint rest positions" << std::endl;
std::cout << "'a': Decrease joint rest positions" << std::endl;
std::cout << "'w': Increase joint spring stiffness" << std::endl;
std::cout << "'s': Decrease joint spring stiffness" << std::endl;
std::cout << "'e': Increase joint damping" << std::endl;
std::cout << "'d': Decrease joint damping" << std::endl;
std::cout << "'r': add/remove constraint on the end of the chain" << std::endl;
std::cout << "'f': switch between applying joint torques and body forces" << std::endl;
// Initialize glut, initialize the window, and begin the glut event loop
glutInit(&argc, argv);
window.initWindow(640, 480, "Multi-Pendulum Tutorial");
glutMainLoop();
}
SkeletonHandler ResourceManager::GetSkeletonFromFile(std::string fileName,std::string resID)
{
unsigned int id = hasher(resID);
if(pool->SkeletonExist(id))
{
return SkeletonHandler(pool->GetSkeleton(id),id,pool);
}
else
{
SkeletonPtr data = AssetLoader::GetPtr()->LoadSkeleton(fileName);
if(data.IsNull())
return SkeletonHandler(false);
pool->AddSkeleton(id,data);
return SkeletonHandler(data,id,pool);
}
}
SkeletonPtr createAtlas()
{
// Parse in the atlas model
DartLoader urdf;
SkeletonPtr atlas =
urdf.parseSkeleton(DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.urdf");
// Add a box to the root node to make it easier to click and drag
double scale = 0.25;
ShapePtr boxShape =
std::make_shared<BoxShape>(scale*Eigen::Vector3d(1.0, 1.0, 0.5));
boxShape->setColor(dart::Color::Black());
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.1*Eigen::Vector3d(0.0, 0.0, 1.0));
boxShape->setLocalTransform(tf);
atlas->getBodyNode(0)->addVisualizationShape(boxShape);
return atlas;
}
SkeletonHandler ResourceManager::GetSkeletonFromMemory(SkeletonPtr skeleton,std::string resID)
{
unsigned int id = hasher(resID);
if(pool->SkeletonExist(id))
{
return SkeletonHandler(pool->GetSkeleton(id),id,pool);
}
else
{
if(skeleton.IsNull())
return SkeletonHandler(false);
SkeletonPtr res = skeleton.Get()->Copy();
pool->AddSkeleton(id,res);
return SkeletonHandler(res,id,pool);
}
}
//==============================================================================
TEST(FORWARD_KINEMATICS, YAW_ROLL)
{
// Checks forward kinematics for two DoF arm manipulators.
// NOTE: The following is the reference frame description of the world
// frame. The x-axis is into the page, z-axis is to the top of the
// page and the y-axis is to the left. At the zero angle, the links
// are parallel to the z-axis and face the +x-axis.
// Create the world
const double l1 = 1.5, l2 = 1.0;
SkeletonPtr robot = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_YAW,
Vector3d(0.3, 0.3, l2), DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector
// positions
const std::size_t numTests = 2;
double temp = sqrt(0.5*l2*l2);
Vector2d joints [numTests] = { Vector2d( constantsd::pi()/4.0, constantsd::pi()/2.0),
Vector2d(-constantsd::pi()/4.0, -constantsd::pi()/4.0) };
Vector3d expectedPos [numTests] = { Vector3d(temp, -temp, l1),
Vector3d(temp / sqrt(2.0),
temp / sqrt(2.0), l1+temp) };
// Check each case by setting the joint values and obtaining the end-effector
// position
for (std::size_t i = 0; i < numTests; i++)
{
robot->setPositions(Eigen::VectorXd(joints[i]));
BodyNode* bn = robot->getBodyNode("ee");
Vector3d actual = bn->getTransform().translation();
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality)
{
std::cout << "Joint values: " << joints[i].transpose() << std::endl;
std::cout << "Actual pos: " << actual.transpose() << std::endl;
std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
}
}
}
//==============================================================================
TEST(SdfParser, SDFSingleBodyWithoutJoint)
{
// Regression test for #444
WorldPtr world
= SdfParser::readSdfFile(
DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world");
EXPECT_TRUE(world != nullptr);
SkeletonPtr skel = world->getSkeleton(0);
EXPECT_TRUE(skel != nullptr);
EXPECT_EQ(skel->getNumBodyNodes(), 1u);
EXPECT_EQ(skel->getNumJoints(), 1u);
BodyNodePtr bodyNode = skel->getBodyNode(0);
EXPECT_TRUE(bodyNode != nullptr);
EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1u);
EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u);
JointPtr joint = skel->getJoint(0);
EXPECT_TRUE(joint != nullptr);
EXPECT_EQ(joint->getType(), FreeJoint::getStaticType());
}
void OgreSample19App::tweakSneakAnim()
{
SkeletonPtr skel = SkeletonManager::getSingleton().load("jaiqua.skeleton",ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
Animation * anim = skel->getAnimation("Sneak");
Animation::NodeTrackIterator tracks = anim->getNodeTrackIterator();
while(tracks.hasMoreElements())
{
NodeAnimationTrack * track = tracks.getNext();
TransformKeyFrame oldKf(0,0);
track->getInterpolatedKeyFrame(ANIM_CHOP,&oldKf);
while (track->getKeyFrame(track->getNumKeyFrames()-1)->getTime() >= ANIM_CHOP - 0.3f)
{
track->removeKeyFrame(track->getNumKeyFrames()-1);
}
TransformKeyFrame * newKf = track->createNodeKeyFrame(ANIM_CHOP);
TransformKeyFrame * startKf = track->getNodeKeyFrame(0);
Bone * bone = skel->getBone(track->getHandle());
if (bone->getName() == "Spineroot")
{
mSneakStartPos = startKf->getTranslate() + bone->getInitialPosition();
mSneakEndPos = oldKf.getTranslate() + bone->getInitialPosition();
mSneakStartPos.y = mSneakEndPos.y;
newKf->setTranslate(oldKf.getTranslate());
newKf->setRotation(oldKf.getRotation());
newKf->setScale(oldKf.getScale());
}
else
{
newKf->setTranslate(startKf->getTranslate());
newKf->setRotation(startKf->getRotation());
newKf->setScale(startKf->getScale());
}
}
}
//-----------------------------------------------------------------------------
const AxisAlignedBox& XsiSkeletonExporter::exportSkeleton(const String& skeletonFileName,
DeformerMap& deformers, float framesPerSecond, AnimationList& animList)
{
LogOgreAndXSI(L"** Begin OGRE Skeleton Export **");
copyDeformerMap(deformers);
SkeletonPtr skeleton = SkeletonManager::getSingleton().create("export",
ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
// construct the hierarchy
buildBoneHierarchy(skeleton.get(), deformers, animList);
// progress report
ProgressManager::getSingleton().progress();
establishInitialTransforms(deformers);
// create animations
mAABB.setNull();
createAnimations(skeleton.get(), deformers, framesPerSecond, animList, mAABB);
// progress report
ProgressManager::getSingleton().progress();
// Optimise
skeleton->optimiseAllAnimations();
SkeletonSerializer ser;
ser.exportSkeleton(skeleton.get(), skeletonFileName);
// progress report
ProgressManager::getSingleton().progress();
LogOgreAndXSI(L"** OGRE Skeleton Export Complete **");
cleanup();
return mAABB;
}
void skeletonToXML(XmlOptions opts)
{
std::ifstream ifs;
ifs.open(opts.source.c_str(), std::ios_base::in | std::ios_base::binary);
if (ifs.bad())
{
cout << "Unable to load file " << opts.source << endl;
exit(1);
}
SkeletonPtr skel = SkeletonManager::getSingleton().create("conversion",
ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
// pass false for freeOnClose to FileStreamDataStream since ifs is created locally on stack
DataStreamPtr stream(new FileStreamDataStream(opts.source, &ifs, false));
skeletonSerializer->importSkeleton(stream, skel.getPointer());
xmlSkeletonSerializer->exportSkeleton(skel.getPointer(), opts.dest);
// Clean up the conversion skeleton
SkeletonManager::getSingleton().remove("conversion");
}
SkeletonPtr createHubo()
{
dart::utils::DartLoader loader;
loader.addPackageDirectory("drchubo", DART_DATA_PATH"/urdf/drchubo");
SkeletonPtr hubo =
loader.parseSkeleton(DART_DATA_PATH"/urdf/drchubo/drchubo.urdf");
for(size_t i = 0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
if(bn->getName().substr(0, 7) == "Body_LF"
|| bn->getName().substr(0, 7) == "Body_RF"
|| bn->getName().substr(0, 7) == "Body_NK")
{
bn->remove();
--i;
}
}
hubo->setPosition(5, 0.97);
for(size_t i=1; i < hubo->getNumJoints(); ++i)
{
hubo->getJoint(i)->setActuatorType(Joint::VELOCITY);
}
for(size_t i=0; i < hubo->getNumBodyNodes(); ++i)
{
BodyNode* bn = hubo->getBodyNode(i);
for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
{
const ShapePtr& shape = bn->getVisualizationShape(j);
shape->setColor(Eigen::Vector3d(0.2, 0.2, 0.2));
if(MeshShapePtr mesh = std::dynamic_pointer_cast<MeshShape>(shape))
mesh->setColorMode(MeshShape::SHAPE_COLOR);
}
}
hubo->setName("drchubo");
return hubo;
}
