bool URDFtoRSG::rsgFromUrdfModel(const urdf::ModelInterface& robot_model)
{
std::cout << "Found a robot with root :" << robot_model.getRoot()->name << std::endl;
// add all children
for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToRSG(robot_model.getRoot()->child_links[i], wm->getRootNodeId())) return false;
return true;
}
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
{
tree = Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial)
std::cout<<"The root link "<<robot_model.getRoot()->name.c_str()<<" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF."<<std::endl;
// ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree, const bool consider_root_link_inertia)
{
if (consider_root_link_inertia) {
//For giving a name to the root of KDL using the robot name,
//as it is not used elsewhere in the KDL tree
std::string fake_root_name = "__kdl_import__" + robot_model.getName()+"__fake_root__";
std::string fake_root_fixed_joint_name = "__kdl_import__" + robot_model.getName()+"__fake_root_fixed_joint__";
tree = Tree(fake_root_name);
const urdf::ConstLinkPtr root = robot_model.getRoot();
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = Joint(fake_root_fixed_joint_name, Joint::None);
// construct the kdl segment
Segment sgm(root->name, jnt, Frame::Identity(), inert);
// add segment to tree
tree.addSegment(sgm, fake_root_name);
} else {
tree = Tree(robot_model.getRoot()->name);
// warn if root link has inertia. KDL does not support this
if (robot_model.getRoot()->inertial)
std::cerr << "The root link " << robot_model.getRoot()->name <<
" has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." << std::endl;
}
// add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
return true;
}
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
// clear out any data (properties, shapes, etc) from a previously loaded robot.
clear();
// the root link is discovered below. Set to NULL until found.
root_link_ = NULL;
// Create properties for each link.
{
typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
for( ; link_it != link_end; ++link_it )
{
const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
std::string parent_joint_name;
if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
{
parent_joint_name = urdf_link->parent_joint->name;
}
RobotLink* link = new RobotLink(this, urdf_link, parent_joint_name,
visual, collision, link_pointclouds_);
if (urdf_link == urdf.getRoot())
{
root_link_ = link;
}
links_[urdf_link->name] = link;
}
}
// Create properties for each joint.
{
typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
for( ; joint_it != joint_end; ++joint_it )
{
const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
RobotJoint* joint = new RobotJoint(this, urdf_joint);
joints_[urdf_joint->name] = joint;
}
}
link_pointclouds_.syncToDevice(); // after all links have been created, we sync them to the GPU
// finally create a KDL representation of the kinematic tree:
if (!kdl_parser::treeFromUrdfModel(urdf, tree)){
LOGGING_ERROR_C(RobotLog, Robot,
"Failed to extract kdl tree from xml robot description!" << endl);
exit(-1);
}
LOGGING_INFO_C(RobotLog, Robot,
"Constructed KDL tree has " << tree.getNrOfJoints() << " Joints and "
<< tree.getNrOfSegments() << " segments." << endl);
}
void Robot::load( const urdf::ModelInterface &urdf, bool visual, bool collision )
{
link_tree_->hide(); // hide until loaded
robot_loaded_ = false;
// clear out any data (properties, shapes, etc) from a previously loaded robot.
clear();
// the root link is discovered below. Set to NULL until found.
root_link_ = NULL;
// Create properties for each link.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
typedef std::map<std::string, boost::shared_ptr<urdf::Link> > M_NameToUrdfLink;
M_NameToUrdfLink::const_iterator link_it = urdf.links_.begin();
M_NameToUrdfLink::const_iterator link_end = urdf.links_.end();
for( ; link_it != link_end; ++link_it )
{
const boost::shared_ptr<const urdf::Link>& urdf_link = link_it->second;
std::string parent_joint_name;
if (urdf_link != urdf.getRoot() && urdf_link->parent_joint)
{
parent_joint_name = urdf_link->parent_joint->name;
}
RobotLink* link = link_factory_->createLink( this,
urdf_link,
parent_joint_name,
visual,
collision );
if (urdf_link == urdf.getRoot())
{
root_link_ = link;
}
links_[urdf_link->name] = link;
link->setRobotAlpha( alpha_ );
}
}
// Create properties for each joint.
// Properties are not added to display until changedLinkTreeStyle() is called (below).
{
typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > M_NameToUrdfJoint;
M_NameToUrdfJoint::const_iterator joint_it = urdf.joints_.begin();
M_NameToUrdfJoint::const_iterator joint_end = urdf.joints_.end();
for( ; joint_it != joint_end; ++joint_it )
{
const boost::shared_ptr<const urdf::Joint>& urdf_joint = joint_it->second;
RobotJoint* joint = link_factory_->createJoint( this, urdf_joint );
joints_[urdf_joint->name] = joint;
joint->setRobotAlpha( alpha_ );
}
}
// robot is now loaded
robot_loaded_ = true;
link_tree_->show();
// set the link tree style and add link/joint properties to rviz pane.
setLinkTreeStyle(LinkTreeStyle(link_tree_style_->getOptionInt()));
changedLinkTreeStyle();
// at startup the link tree is collapsed since it is large and not often needed.
link_tree_->collapse();
setVisualVisible( isVisualVisible() );
setCollisionVisible( isCollisionVisible() );
}