// recursive function to walk through tree
bool addChildrenToTree(urdf::LinkPtr root, Tree& tree)
{
urdf::LinkVector children = root->child_links;
//std::cerr << "[INFO] Link " << root->name << " had " << children.size() << " children" << std::endl;
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
// construct joint
Joint toKdl(urdf::JointPtr jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
std::cerr << "Converting unknown joint type of joint " << jnt->name << " into a fixed joint" << std::endl;
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
// construct joint
Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
{
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
switch (jnt->type){
case urdf::Joint::FIXED:{
return Joint(jnt->name, Joint::None);
}
case urdf::Joint::REVOLUTE:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::CONTINUOUS:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
}
case urdf::Joint::PRISMATIC:{
Vector axis = toKdl(jnt->axis);
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
}
default:{
std::cout<<"Converting unknown joint type of joint "<<jnt->name.c_str()<<" into a fixed joint."<<std::endl;
// ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
return Joint(jnt->name, Joint::None);
}
}
return Joint();
}
// recursive function to walk through tree
bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
{
std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
std::cout<<"Link "<<root->name.c_str()<<" had "<<(int)children.size()<<" children."<<std::endl;
// ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
// constructs the optional inertia
RigidBodyInertia inert(0);
if (root->inertial)
inert = toKdl(root->inertial);
// constructs the kdl joint
Joint jnt = toKdl(root->parent_joint);
// construct the kdl segment
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
// add segment to tree
tree.addSegment(sgm, root->parent_joint->parent_link_name);
// recurslively add all children
for (size_t i=0; i<children.size(); i++){
if (!addChildrenToTree(children[i], tree))
return false;
}
return true;
}
// construct inertia
RigidBodyInertia toKdl(urdf::InertialPtr i)
{
Frame origin = toKdl(i->origin);
// the mass is frame indipendent
double kdl_mass = i->mass;
// kdl and urdf both specify the com position in the reference frame of the link
Vector kdl_com = origin.p;
// kdl specifies the inertia matrix in the reference frame of the link,
// while the urdf specifies the inertia matrix in the inertia reference frame
RotationalInertia urdf_inertia =
RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
// Rotation operators are not defined for rotational inertia,
// so we use the RigidBodyInertia operators (with com = 0) as a workaround
RigidBodyInertia kdl_inertia_wrt_com_workaround =
origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
// Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
// while the getRotationalInertia method returns the 3d inertia wrt the frame origin
// (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
RotationalInertia kdl_inertia_wrt_com =
kdl_inertia_wrt_com_workaround.getRotationalInertia();
return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
}
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;
}
// construct pose
Frame toKdl(urdf::Pose p)
{
return Frame(toKdl(p.rotation), toKdl(p.position));
}