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;
}
/* ------------------------ 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");
  }
}
Esempio n. 3
0
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;
}