/* ------------------------ KinematicModel ------------------------ */
planning_models::KinematicModel::KinematicModel(const urdf::Model &model,
const std::vector<GroupConfig>& group_configs,
const std::vector<MultiDofConfig>& multi_dof_configs)
{
model_name_ = model.getName();
if (model.getRoot())
{
const urdf::Link *root = model.getRoot().get();
root_ = buildRecursive(NULL, root, multi_dof_configs);
buildGroups(group_configs);
}
else
{
root_ = NULL;
ROS_WARN("No root link found");
}
}
bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif
// Save the root link name
m_nameURDFRoot = model.getRoot()->name;
m_indexURDFRoot = (unsigned int) -1;
// Reset root mass to zero and give it the proper name
mBodies[0].mMass = 0;
// Set gravity to ROS standards
gravity << 0, 0, -9.81;
std::vector<boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);
if(oldRoot == newRoot || !newRoot)
process(*oldRoot, 0, Math::SpatialTransform());
else
processReverse(*newRoot, 0, NULL, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif
return true;
}