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