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