kdl_parser.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include "kdl_parser/kdl_parser.hpp"
00038 #include <urdf/model.h>
00039 #include <urdf/urdfdom_compatibility.h>
00040 #include <kdl/frames_io.hpp>
00041 #include <ros/console.h>
00042 
00043 using namespace std;
00044 using namespace KDL;
00045 
00046 namespace kdl_parser{
00047 
00048 
00049 // construct vector
00050 Vector toKdl(urdf::Vector3 v)
00051 {
00052   return Vector(v.x, v.y, v.z);
00053 }
00054 
00055 // construct rotation
00056 Rotation toKdl(urdf::Rotation r)
00057 {
00058   return Rotation::Quaternion(r.x, r.y, r.z, r.w);
00059 }
00060 
00061 // construct pose
00062 Frame toKdl(urdf::Pose p)
00063 {
00064   return Frame(toKdl(p.rotation), toKdl(p.position));
00065 }
00066 
00067 // construct joint
00068 Joint toKdl(urdf::JointSharedPtr jnt)
00069 {
00070   Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
00071 
00072   switch (jnt->type){
00073   case urdf::Joint::FIXED:{
00074     return Joint(jnt->name, Joint::None);
00075   }
00076   case urdf::Joint::REVOLUTE:{
00077     Vector axis = toKdl(jnt->axis);
00078     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00079   }
00080   case urdf::Joint::CONTINUOUS:{
00081     Vector axis = toKdl(jnt->axis);
00082     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00083   }
00084   case urdf::Joint::PRISMATIC:{
00085     Vector axis = toKdl(jnt->axis);
00086     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
00087   }
00088   default:{
00089     ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
00090     return Joint(jnt->name, Joint::None);
00091   }
00092   }
00093   return Joint();
00094 }
00095 
00096 // construct inertia
00097 RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
00098 {
00099   Frame origin = toKdl(i->origin);
00100   
00101   // the mass is frame indipendent 
00102   double kdl_mass = i->mass;
00103   
00104   // kdl and urdf both specify the com position in the reference frame of the link
00105   Vector kdl_com = origin.p;
00106   
00107   // kdl specifies the inertia matrix in the reference frame of the link, 
00108   // while the urdf specifies the inertia matrix in the inertia reference frame
00109   RotationalInertia urdf_inertia = 
00110     RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
00111     
00112   // Rotation operators are not defined for rotational inertia,
00113   // so we use the RigidBodyInertia operators (with com = 0) as a workaround
00114   RigidBodyInertia kdl_inertia_wrt_com_workaround =
00115     origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
00116   
00117   // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
00118   // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
00119   // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
00120   RotationalInertia kdl_inertia_wrt_com = 
00121     kdl_inertia_wrt_com_workaround.getRotationalInertia();
00122     
00123   return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);  
00124 }
00125 
00126 
00127 // recursive function to walk through tree
00128 bool addChildrenToTree(urdf::LinkConstSharedPtr root, Tree& tree)
00129 {
00130   std::vector<urdf::LinkSharedPtr > children = root->child_links;
00131   ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
00132 
00133   // constructs the optional inertia
00134   RigidBodyInertia inert(0);
00135   if (root->inertial) 
00136     inert = toKdl(root->inertial);
00137 
00138   // constructs the kdl joint
00139   Joint jnt = toKdl(root->parent_joint);
00140 
00141   // construct the kdl segment
00142   Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
00143 
00144   // add segment to tree
00145   tree.addSegment(sgm, root->parent_joint->parent_link_name);
00146 
00147   // recurslively add all children
00148   for (size_t i=0; i<children.size(); i++){
00149     if (!addChildrenToTree(children[i], tree))
00150       return false;
00151   }
00152   return true;
00153 }
00154 
00155 
00156 bool treeFromFile(const string& file, Tree& tree)
00157 {
00158   TiXmlDocument urdf_xml;
00159   urdf_xml.LoadFile(file);
00160   return treeFromXml(&urdf_xml, tree);
00161 }
00162 
00163 bool treeFromParam(const string& param, Tree& tree)
00164 {
00165   urdf::Model robot_model;
00166   if (!robot_model.initParam(param)){
00167     ROS_ERROR("Could not generate robot model");
00168     return false;
00169   }
00170   return treeFromUrdfModel(robot_model, tree);
00171 }
00172 
00173 bool treeFromString(const string& xml, Tree& tree)
00174 {
00175   TiXmlDocument urdf_xml;
00176   urdf_xml.Parse(xml.c_str());
00177   return treeFromXml(&urdf_xml, tree);
00178 }
00179 
00180 bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
00181 {
00182   urdf::Model robot_model;
00183   if (!robot_model.initXml(xml_doc)){
00184     ROS_ERROR("Could not generate robot model");
00185     return false;
00186   }
00187   return treeFromUrdfModel(robot_model, tree);
00188 }
00189 
00190 
00191 bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
00192 {
00193   tree = Tree(robot_model.getRoot()->name);
00194 
00195   // warn if root link has inertia. KDL does not support this
00196   if (robot_model.getRoot()->inertial)
00197     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());
00198 
00199   //  add all children
00200   for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
00201     if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
00202       return false;
00203 
00204   return true;
00205 }
00206 
00207 }
00208 


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Thu Jun 6 2019 21:11:45