bool DartLoader::createSkeletonRecursive( dynamics::SkeletonPtr _skel, const urdf::Link* _lk, dynamics::BodyNode* _parentNode, const common::Uri& _baseUri, const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::BodyNode::Properties properties; if (!createDartNodeProperties(_lk, properties, _baseUri, _resourceRetriever)) return false; dynamics::BodyNode* node = createDartJointAndNode( _lk->parent_joint.get(), properties, _parentNode, _skel, _baseUri, _resourceRetriever); if(!node) return false; for(size_t i = 0; i < _lk->child_links.size(); ++i) { if (!createSkeletonRecursive(_skel, _lk->child_links[i].get(), node, _baseUri, _resourceRetriever)) return false; } return true; }
/** * @function modelInterfaceToSkeleton * @brief Read the ModelInterface and spits out a Skeleton object */ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton( const urdf::ModelInterface* _model, const common::Uri& _baseUri, const common::ResourceRetrieverPtr& _resourceRetriever) { dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(_model->getName()); dynamics::BodyNode* rootNode = nullptr; const urdf::Link* root = _model->getRoot().get(); if(root->name == "world") { if(_model->getRoot()->child_links.size() != 1) { dterr << "[DartLoader::modelInterfaceToSkeleton] No unique link attached to world.\n"; } else { root = root->child_links[0].get(); dynamics::BodyNode::Properties rootProperties; if (!createDartNodeProperties(root, rootProperties, _baseUri, _resourceRetriever)) return nullptr; rootNode = createDartJointAndNode( root->parent_joint.get(), rootProperties, nullptr, skeleton, _baseUri, _resourceRetriever); if(nullptr == rootNode) { dterr << "[DartLoader::modelInterfaceToSkeleton] Failed to create root node!\n"; return nullptr; } } } else { dynamics::BodyNode::Properties rootProperties; if (!createDartNodeProperties(root, rootProperties, _baseUri, _resourceRetriever)) return nullptr; std::pair<dynamics::Joint*, dynamics::BodyNode*> pair = skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>( nullptr, dynamics::FreeJoint::Properties( dynamics::MultiDofJoint<6>::Properties( dynamics::Joint::Properties("rootJoint"))), rootProperties); rootNode = pair.second; } for(size_t i = 0; i < root->child_links.size(); i++) { if (!createSkeletonRecursive( skeleton, root->child_links[i].get(), rootNode, _baseUri, _resourceRetriever)) return nullptr; } return skeleton; }
void DartLoader::createSkeletonRecursive(dynamics::Skeleton* _skel, const urdf::Link* _lk, dynamics::BodyNode* _parentNode, std::string _rootToSkelPath) { dynamics::BodyNode* node = createDartNode(_lk, _rootToSkelPath); dynamics::Joint* joint = createDartJoint(_lk->parent_joint.get()); node->setParentJoint(joint); _parentNode->addChildBodyNode(node); _skel->addBodyNode(node); for(unsigned int i = 0; i < _lk->child_links.size(); i++) { createSkeletonRecursive(_skel, _lk->child_links[i].get(), node, _rootToSkelPath); } }
/** * @function modelInterfaceToSkeleton * @brief Read the ModelInterface and spits out a Skeleton object */ dynamics::Skeleton* DartLoader::modelInterfaceToSkeleton(const urdf::ModelInterface* _model, std::string _rootToSkelPath) { if( _rootToSkelPath.empty() ) { std::cout<< "[DartLoader] Absolute path to skeleton "<<_model->getName()<<" is not set. Probably I will crash!"<<std::endl; } dynamics::Skeleton* skeleton = new dynamics::Skeleton(_model->getName()); dynamics::BodyNode* rootNode; dynamics::Joint* rootJoint; const urdf::Link* root = _model->getRoot().get(); if(root->name == "world") { if(_model->getRoot()->child_links.size() != 1) { std::cout<< "[ERROR] Not unique link attached to world." << std::endl; } else { root = root->child_links[0].get(); rootNode = createDartNode(root, _rootToSkelPath); rootJoint = createDartJoint(root->parent_joint.get()); if(!rootJoint) return NULL; } } else { rootNode = createDartNode(root, _rootToSkelPath); rootJoint = new dynamics::FreeJoint(); rootJoint->setName("rootJoint"); rootJoint->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity()); rootJoint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity()); } rootNode->setParentJoint(rootJoint); skeleton->addBodyNode(rootNode); for(unsigned int i = 0; i < root->child_links.size(); i++) { createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode, _rootToSkelPath); } return skeleton; }