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
64using namespace std;
65using namespace KDL;
66
67namespace fawkes {
68
69namespace kdl_parser {
70
71// construct vector
72Vector
73to_kdl(urdf::Vector3 v)
74{
75 return Vector(v.x, v.y, v.z);
76}
77
78// construct rotation
79Rotation
80to_kdl(urdf::Rotation r)
81{
82 return Rotation::Quaternion(r.x, r.y, r.z, r.w);
83}
84
85// construct pose
86Frame
87to_kdl(urdf::Pose p)
88{
89 return Frame(to_kdl(p.rotation), to_kdl(p.position));
90}
91
92// construct joint
93Joint
94to_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
123RigidBodyInertia
124to_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
135bool
136add_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
163bool
164tree_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
171bool
172tree_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
179bool
180tree_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
189bool
190tree_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
This class represents an URDF model.
Definition: model.h:74
bool initXml(TiXmlElement *xml)
Initialize the model using a XML Element.
Definition: model.cpp:129
Fawkes library namespace.