kdl_parser.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
38 
39 #include <string>
40 #include <vector>
41 
42 #include <urdf_model/model.h>
43 #include <urdf_parser/urdf_parser.h>
44 
45 #include <kdl/frames_io.hpp>
46 
47 #ifdef HAS_ROS
48 #include <ros/console.h>
49 #else
50 // forward ROS warnings and errors to stderr
51 #define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__);
52 #define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__);
53 #define ROS_WARN(...) fprintf(stderr, __VA_ARGS__);
54 #endif
55 
56 #ifdef HAS_URDF
57 #include <urdf/model.h>
58 #include <urdf/urdfdom_compatibility.h>
59 #endif
60 
61 namespace kdl_parser
62 {
63 // construct vector
64 KDL::Vector toKdl(urdf::Vector3 v)
65 {
66  return KDL::Vector(v.x, v.y, v.z);
67 }
68 
69 // construct rotation
70 KDL::Rotation toKdl(urdf::Rotation r)
71 {
72  return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
73 }
74 
75 // construct pose
76 KDL::Frame toKdl(urdf::Pose p)
77 {
78  return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
79 }
80 
81 // construct joint
82 KDL::Joint toKdl(urdf::JointSharedPtr jnt)
83 {
84  KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
85 
86  switch (jnt->type) {
87  case urdf::Joint::FIXED: {
88  return KDL::Joint(jnt->name, KDL::Joint::None);
89  }
90  case urdf::Joint::REVOLUTE: {
91  KDL::Vector axis = toKdl(jnt->axis);
92  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
93  }
94  case urdf::Joint::CONTINUOUS: {
95  KDL::Vector axis = toKdl(jnt->axis);
96  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
97  }
98  case urdf::Joint::PRISMATIC: {
99  KDL::Vector axis = toKdl(jnt->axis);
100  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
101  }
102  default: {
103  ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
104  return KDL::Joint(jnt->name, KDL::Joint::None);
105  }
106  }
107  return KDL::Joint();
108 }
109 
110 // construct inertia
111 KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
112 {
113  KDL::Frame origin = toKdl(i->origin);
114 
115  // the mass is frame independent
116  double kdl_mass = i->mass;
117 
118  // kdl and urdf both specify the com position in the reference frame of the link
119  KDL::Vector kdl_com = origin.p;
120 
121  // kdl specifies the inertia matrix in the reference frame of the link,
122  // while the urdf specifies the inertia matrix in the inertia reference frame
123  KDL::RotationalInertia urdf_inertia =
124  KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
125 
126  // Rotation operators are not defined for rotational inertia,
127  // so we use the RigidBodyInertia operators (with com = 0) as a workaround
128  KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
129  origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia);
130 
131  // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
132  // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
133  // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
134  KDL::RotationalInertia kdl_inertia_wrt_com =
135  kdl_inertia_wrt_com_workaround.getRotationalInertia();
136 
137  return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);
138 }
139 
140 
141 // recursive function to walk through tree
142 bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
143 {
144  std::vector<urdf::LinkSharedPtr> children = root->child_links;
145  ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
146 
147  // constructs the optional inertia
148  KDL::RigidBodyInertia inert(0);
149  if (root->inertial) {
150  inert = toKdl(root->inertial);
151  }
152 
153  // constructs the kdl joint
154  KDL::Joint jnt = toKdl(root->parent_joint);
155 
156  // construct the kdl segment
157  KDL::Segment sgm(root->name, jnt, toKdl(
158  root->parent_joint->parent_to_joint_origin_transform), inert);
159 
160  // add segment to tree
161  tree.addSegment(sgm, root->parent_joint->parent_link_name);
162 
163  // recurslively add all children
164  for (size_t i = 0; i < children.size(); i++) {
165  if (!addChildrenToTree(children[i], tree)) {
166  return false;
167  }
168  }
169  return true;
170 }
171 
172 bool treeFromFile(const std::string & file, KDL::Tree & tree)
173 {
174  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file);
175  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
176 }
177 
178 
179 bool treeFromParam(const std::string & param, KDL::Tree & tree)
180 {
181 #if defined(HAS_ROS) && defined(HAS_URDF)
182  urdf::Model robot_model;
183  if (!robot_model.initParam(param)){
184  ROS_ERROR("Could not generate robot model");
185  return false;
186  }
187  return treeFromUrdfModel(robot_model, tree);
188 #else
189  return false;
190 #endif
191 }
192 
193 
194 bool treeFromString(const std::string & xml, KDL::Tree & tree)
195 {
196  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml);
197  if (!robot_model) {
198  ROS_ERROR("Could not generate robot model");
199  return false;
200  }
201  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
202 }
203 
204 bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree)
205 {
206  if (!xml_doc) {
207  ROS_ERROR("Could not parse the xml document");
208  return false;
209  }
210 
211  tinyxml2::XMLPrinter printer;
212  xml_doc->Print(&printer);
213 
214  return treeFromString(printer.CStr(), tree);
215 }
216 
217 bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
218 {
219  if (!xml_doc) {
220  ROS_ERROR("Could not parse the xml document");
221  return false;
222  }
223 
224  std::stringstream ss;
225  ss << *xml_doc;
226 
227  return treeFromString(ss.str(), tree);
228 }
229 
230 bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
231 {
232  if (!robot_model.getRoot()) {
233  return false;
234  }
235 
236  tree = KDL::Tree(robot_model.getRoot()->name);
237 
238  // warn if root link has inertia. KDL does not support this
239  if (robot_model.getRoot()->inertial) {
240  ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not "
241  "support a root link with an inertia. As a workaround, you can add an extra "
242  "dummy link to your URDF.", robot_model.getRoot()->name.c_str());
243  }
244 
245  // add all children
246  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
247  if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
248  return false;
249  }
250  }
251 
252  return true;
253 }
254 
255 } // namespace kdl_parser
kdl_parser::treeFromFile
KDL_PARSER_PUBLIC bool treeFromFile(const std::string &file, KDL::Tree &tree)
Definition: kdl_parser.cpp:172
kdl_parser::treeFromXml
KDL_PARSER_PUBLIC bool treeFromXml(const tinyxml2::XMLDocument *xml_doc, KDL::Tree &tree)
Definition: kdl_parser.cpp:204
kdl_parser::addChildrenToTree
bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree &tree)
Definition: kdl_parser.cpp:142
kdl_parser::toKdl
KDL::Vector toKdl(urdf::Vector3 v)
Definition: kdl_parser.cpp:64
kdl_parser::treeFromString
KDL_PARSER_PUBLIC bool treeFromString(const std::string &xml, KDL::Tree &tree)
Definition: kdl_parser.cpp:194
ROS_DEBUG
#define ROS_DEBUG(...)
Definition: kdl_parser.cpp:51
urdf::Model
kdl_parser::treeFromUrdfModel
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
Definition: kdl_parser.cpp:230
console.h
model.h
kdl_parser.hpp
ROS_ERROR
#define ROS_ERROR(...)
Definition: kdl_parser.cpp:52
kdl_parser::treeFromParam
KDL_PARSER_PUBLIC bool treeFromParam(const std::string &param, KDL::Tree &tree)
Definition: kdl_parser.cpp:179
ROS_WARN
#define ROS_WARN(...)
Definition: kdl_parser.cpp:53
urdf::Model::initParam
URDF_EXPORT bool initParam(const std::string &param)
param
T param(const std::string &param_name, const T &default_val)
kdl_parser
Definition: kdl_parser.hpp:48


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Fri Apr 15 2022 02:21:42