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 <kdl/frames_io.hpp>
00040 #include <ros/console.h>
00041 
00042 using namespace std;
00043 using namespace KDL;
00044 
00045 namespace kdl_parser{
00046 
00047 
00048 // construct vector
00049 Vector toKdl(urdf::Vector3 v)
00050 {
00051   return Vector(v.x, v.y, v.z);
00052 }
00053 
00054 // construct rotation
00055 Rotation toKdl(urdf::Rotation r)
00056 {
00057   return Rotation::Quaternion(r.x, r.y, r.z, r.w);
00058 }
00059 
00060 // construct pose
00061 Frame toKdl(urdf::Pose p)
00062 {
00063   return Frame(toKdl(p.rotation), toKdl(p.position));
00064 }
00065 
00066 // construct joint
00067 Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
00068 {
00069   Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
00070 
00071   switch (jnt->type){
00072   case urdf::Joint::FIXED:{
00073     return Joint(jnt->name, Joint::None);
00074   }
00075   case urdf::Joint::REVOLUTE:{
00076     Vector axis = toKdl(jnt->axis);
00077     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00078   }
00079   case urdf::Joint::CONTINUOUS:{
00080     Vector axis = toKdl(jnt->axis);
00081     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
00082   }
00083   case urdf::Joint::PRISMATIC:{
00084     Vector axis = toKdl(jnt->axis);
00085     return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
00086   }
00087   default:{
00088     ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
00089     return Joint(jnt->name, Joint::None);
00090   }
00091   }
00092   return Joint();
00093 }
00094 
00095 // construct inertia
00096 RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
00097 {
00098   Frame origin = toKdl(i->origin);
00099   // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
00100   return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
00101 }
00102 
00103 
00104 // recursive function to walk through tree
00105 bool addChildrenToTree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
00106 {
00107   std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
00108   ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
00109 
00110   // constructs the optional inertia
00111   RigidBodyInertia inert(0);
00112   if (root->inertial) 
00113     inert = toKdl(root->inertial);
00114 
00115   // constructs the kdl joint
00116   Joint jnt = toKdl(root->parent_joint);
00117 
00118   // construct the kdl segment
00119   Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
00120 
00121   // add segment to tree
00122   tree.addSegment(sgm, root->parent_joint->parent_link_name);
00123 
00124   // recurslively add all children
00125   for (size_t i=0; i<children.size(); i++){
00126     if (!addChildrenToTree(children[i], tree))
00127       return false;
00128   }
00129   return true;
00130 }
00131 
00132 
00133 bool treeFromFile(const string& file, Tree& tree)
00134 {
00135   TiXmlDocument urdf_xml;
00136   urdf_xml.LoadFile(file);
00137   return treeFromXml(&urdf_xml, tree);
00138 }
00139 
00140 bool treeFromParam(const string& param, Tree& tree)
00141 {
00142   urdf::Model robot_model;
00143   if (!robot_model.initParam(param)){
00144     ROS_ERROR("Could not generate robot model");
00145     return false;
00146   }
00147   return treeFromUrdfModel(robot_model, tree);
00148 }
00149 
00150 bool treeFromString(const string& xml, Tree& tree)
00151 {
00152   TiXmlDocument urdf_xml;
00153   urdf_xml.Parse(xml.c_str());
00154   return treeFromXml(&urdf_xml, tree);
00155 }
00156 
00157 bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
00158 {
00159   urdf::Model robot_model;
00160   if (!robot_model.initXml(xml_doc)){
00161     ROS_ERROR("Could not generate robot model");
00162     return false;
00163   }
00164   return treeFromUrdfModel(robot_model, tree);
00165 }
00166 
00167 
00168 bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
00169 {
00170   tree = Tree(robot_model.getRoot()->name);
00171 
00172   // warn if root link has inertia. KDL does not support this
00173   if (robot_model.getRoot()->inertial)
00174     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());
00175 
00176   //  add all children
00177   for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
00178     if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
00179       return false;
00180 
00181   return true;
00182 }
00183 
00184 }
00185 


kdl_parser
Author(s): Wim Meeussen
autogenerated on Mon Oct 6 2014 04:13:50