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