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


kdl_parser
Author(s): Wim Meeussen wim@willowgarage.com
autogenerated on Mon Aug 19 2013 11:02:14