Fawkes API  Fawkes Development Version
kdl_parser.cpp
1 /***************************************************************************
2  * kdl_parser.cpp - KDL Parser
3  *
4  * Created: Fri Feb 14 17:35:15 2014
5  * Copyright 2014 Till Hofmann
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 /* This code is based on ROS kdl_parser with the following copyright and license:
23  *
24  * Copyright (c) 2008, Willow Garage, Inc.
25  * All rights reserved.
26  *
27  * Redistribution and use in source and binary forms, with or without
28  * modification, are permitted provided that the following conditions
29  * are met:
30  *
31  * * Redistributions of source code must retain the above copyright
32  * notice, this list of conditions and the following disclaimer.
33  * * Redistributions in binary form must reproduce the above
34  * copyright notice, this list of conditions and the following
35  * disclaimer in the documentation and/or other materials provided
36  * with the distribution.
37  * * Neither the name of the Willow Garage nor the names of its
38  * contributors may be used to endorse or promote products derived
39  * from this software without specific prior written permission.
40  *
41  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
42  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
43  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
44  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
45  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
46  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
47  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
48  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
49  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
50  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
51  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52  * POSSIBILITY OF SUCH DAMAGE.
53  */
54 
55 #include <kdl_parser/kdl_parser.h>
56 #include <kdl_parser/model.h>
57 #include <urdf_model/pose.h>
58 #include <kdl/frames_io.hpp>
59 #include <kdl_parser/exceptions.h>
60 
61 using namespace std;
62 using namespace KDL;
63 
64 namespace fawkes {
65 
66 namespace kdl_parser {
67 
68 #if 0 /* just to make Emacs auto-indent happy */
69  }
70 }
71 #endif
72 
73 
74 // construct vector
75 Vector to_kdl(urdf::Vector3 v)
76 {
77  return Vector(v.x, v.y, v.z);
78 }
79 
80 // construct rotation
81 Rotation to_kdl(urdf::Rotation r)
82 {
83  return Rotation::Quaternion(r.x, r.y, r.z, r.w);
84 }
85 
86 // construct pose
87 Frame to_kdl(urdf::Pose p)
88 {
89  return Frame(to_kdl(p.rotation), to_kdl(p.position));
90 }
91 
92 // construct joint
93 Joint to_kdl(boost::shared_ptr<urdf::Joint> jnt)
94 {
95  Frame F_parent_jnt = to_kdl(jnt->parent_to_joint_origin_transform);
96 
97  switch (jnt->type){
98  case urdf::Joint::FIXED:{
99  return Joint(jnt->name, Joint::None);
100  }
101  case urdf::Joint::REVOLUTE:{
102  Vector axis = to_kdl(jnt->axis);
103  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
104  }
105  case urdf::Joint::CONTINUOUS:{
106  Vector axis = to_kdl(jnt->axis);
107  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
108  }
109  case urdf::Joint::PRISMATIC:{
110  Vector axis = to_kdl(jnt->axis);
111  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
112  }
113  default:{
114  throw KDLParserUnknownJointTypeException(jnt->name.c_str());
115  return Joint(jnt->name, Joint::None);
116  }
117  }
118  return Joint();
119 }
120 
121 // construct inertia
122 RigidBodyInertia to_kdl(boost::shared_ptr<urdf::Inertial> i)
123 {
124  Frame origin = to_kdl(i->origin);
125  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
126  return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
127 }
128 
129 
130 // recursive function to walk through tree
131 bool add_children_to_tree(boost::shared_ptr<const urdf::Link> root, Tree& tree)
132 {
133  std::vector<boost::shared_ptr<urdf::Link> > children = root->child_links;
134  //ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
135 
136  // constructs the optional inertia
137  RigidBodyInertia inert(0);
138  if (root->inertial)
139  inert = to_kdl(root->inertial);
140 
141  // constructs the kdl joint
142  Joint jnt = to_kdl(root->parent_joint);
143 
144  // construct the kdl segment
145  Segment sgm(root->name, jnt, to_kdl(root->parent_joint->parent_to_joint_origin_transform), inert);
146 
147  // add segment to tree
148  tree.addSegment(sgm, root->parent_joint->parent_link_name);
149 
150  // recursively add all children
151  for (size_t i=0; i<children.size(); i++){
152  if (!add_children_to_tree(children[i], tree))
153  return false;
154  }
155  return true;
156 }
157 
158 
159 bool tree_from_file(const string& file, Tree& tree)
160 {
161  TiXmlDocument urdf_xml;
162  urdf_xml.LoadFile(file);
163  return tree_from_xml(&urdf_xml, tree);
164 }
165 
166 bool tree_from_string(const string& xml, Tree& tree)
167 {
168  TiXmlDocument urdf_xml;
169  urdf_xml.Parse(xml.c_str());
170  return tree_from_xml(&urdf_xml, tree);
171 }
172 
173 bool tree_from_xml(TiXmlDocument *xml_doc, Tree& tree)
174 {
175  urdf::Model robot_model;
176  if (!robot_model.initXml(xml_doc)){
177  throw KDLParserModelGenerationFailedException();
178  }
179  return tree_from_urdf_model(robot_model, tree);
180 }
181 
182 
183 bool tree_from_urdf_model(const urdf::ModelInterface& robot_model, Tree& tree)
184 {
185  tree = Tree(robot_model.getRoot()->name);
186 
187  // add all children
188  for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
189  if (!add_children_to_tree(robot_model.getRoot()->child_links[i], tree))
190  return false;
191 
192  return true;
193 }
194 
195 } // namespace kdl_parser
196 
197 } // namespace fawkes
Fawkes library namespace.
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
Definition: model.cpp:134
STL namespace.
This class represents an URDF model.
Definition: model.h:66