bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// 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, 0, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
return true;
}
/// /////////////////////////////////////////////////////////////////////////////
/// @brief load URDF model description from string and create search operations data structures
void URDFRenderer::loadURDFModel
(urdf::Model &model)
{
typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
V_Link links;
model.getLinks(links);
V_Link::iterator it = links.begin();
V_Link::iterator end = links.end();
for (; it != end; ++it)
process_link (*it);
}
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;
}
/**
* TaskSpacePosition annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateTaskSpace(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::FeatureStructure ts;
boost::shared_ptr<urdf::Link> link;
std::vector< boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
for (std::size_t i = 0, size = links.size(); i < size; i++) {
link = links[i];
ts = cas.createFS(TaskSpacePosition);
ts.setFSValue(tsXyzFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
ts.setFSValue(tsRpyFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
index.addFS(ts);
}
return UIMA_ERR_NONE;
}