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.h"
56 
57 #include "model.h"
58 
59 #include <kdl_parser/exceptions.h>
60 #include <urdf_model/pose.h>
61 
62 #include <kdl/frames_io.hpp>
63 
64 using namespace std;
65 using namespace KDL;
66 
67 namespace fawkes {
68 
69 namespace kdl_parser {
70 
71 // construct vector
72 Vector
73 to_kdl(urdf::Vector3 v)
74 {
75  return Vector(v.x, v.y, v.z);
76 }
77 
78 // construct rotation
79 Rotation
80 to_kdl(urdf::Rotation r)
81 {
82  return Rotation::Quaternion(r.x, r.y, r.z, r.w);
83 }
84 
85 // construct pose
86 Frame
87 to_kdl(urdf::Pose p)
88 {
89  return Frame(to_kdl(p.rotation), to_kdl(p.position));
90 }
91 
92 // construct joint
93 Joint
94 to_kdl(urdf::JointSharedPtr jnt)
95 {
96  Frame F_parent_jnt = to_kdl(jnt->parent_to_joint_origin_transform);
97 
98  switch (jnt->type) {
99  case urdf::Joint::FIXED: {
100  return Joint(jnt->name, Joint::None);
101  }
102  case urdf::Joint::REVOLUTE: {
103  Vector axis = to_kdl(jnt->axis);
104  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
105  }
106  case urdf::Joint::CONTINUOUS: {
107  Vector axis = to_kdl(jnt->axis);
108  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
109  }
110  case urdf::Joint::PRISMATIC: {
111  Vector axis = to_kdl(jnt->axis);
112  return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
113  }
114  default: {
115  throw KDLParserUnknownJointTypeException(jnt->name.c_str());
116  return Joint(jnt->name, Joint::None);
117  }
118  }
119  return Joint();
120 }
121 
122 // construct inertia
123 RigidBodyInertia
124 to_kdl(urdf::InertialSharedPtr i)
125 {
126  Frame origin = to_kdl(i->origin);
127  // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
128  return origin.M
129  * RigidBodyInertia(i->mass,
130  origin.p,
131  RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
132 }
133 
134 // recursive function to walk through tree
135 bool
136 add_children_to_tree(urdf::LinkSharedPtr root, Tree &tree)
137 {
138  const std::vector<urdf::LinkSharedPtr> children = root->child_links;
139  //ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
140 
141  // constructs the optional inertia
142  RigidBodyInertia inert(0);
143  if (root->inertial)
144  inert = to_kdl(root->inertial);
145 
146  // constructs the kdl joint
147  Joint jnt = to_kdl(root->parent_joint);
148 
149  // construct the kdl segment
150  Segment sgm(root->name, jnt, to_kdl(root->parent_joint->parent_to_joint_origin_transform), inert);
151 
152  // add segment to tree
153  tree.addSegment(sgm, root->parent_joint->parent_link_name);
154 
155  // recursively add all children
156  for (size_t i = 0; i < children.size(); i++) {
157  if (!add_children_to_tree(children[i], tree))
158  return false;
159  }
160  return true;
161 }
162 
163 bool
164 tree_from_file(const string &file, Tree &tree)
165 {
166  TiXmlDocument urdf_xml;
167  urdf_xml.LoadFile(file);
168  return tree_from_xml(&urdf_xml, tree);
169 }
170 
171 bool
172 tree_from_string(const string &xml, Tree &tree)
173 {
174  TiXmlDocument urdf_xml;
175  urdf_xml.Parse(xml.c_str());
176  return tree_from_xml(&urdf_xml, tree);
177 }
178 
179 bool
180 tree_from_xml(TiXmlDocument *xml_doc, Tree &tree)
181 {
182  urdf::Model robot_model;
183  if (!robot_model.initXml(xml_doc)) {
184  throw KDLParserModelGenerationFailedException();
185  }
186  return tree_from_urdf_model(robot_model, tree);
187 }
188 
189 bool
190 tree_from_urdf_model(const urdf::ModelInterface &robot_model, Tree &tree)
191 {
192  tree = Tree(robot_model.getRoot()->name);
193 
194  // add all children
195  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++)
196  if (!add_children_to_tree(robot_model.getRoot()->child_links[i], tree))
197  return false;
198 
199  return true;
200 }
201 
202 } // namespace kdl_parser
203 
204 } // namespace fawkes
urdf::Model
Definition: model.h:72
fawkes
urdf::Model::initXml
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
Definition: model.cpp:128