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.h>
43 #include <urdf/urdfdom_compatibility.h>
44 #include <kdl/frames_io.hpp>
45 #include <ros/console.h>
46 
47 namespace kdl_parser
48 {
49 
50 // construct vector
51 KDL::Vector toKdl(urdf::Vector3 v)
52 {
53  return KDL::Vector(v.x, v.y, v.z);
54 }
55 
56 // construct rotation
57 KDL::Rotation toKdl(urdf::Rotation r)
58 {
59  return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
60 }
61 
62 // construct pose
63 KDL::Frame toKdl(urdf::Pose p)
64 {
65  return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
66 }
67 
68 // construct joint
69 KDL::Joint toKdl(urdf::JointSharedPtr jnt)
70 {
71  KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
72 
73  switch (jnt->type) {
74  case urdf::Joint::FIXED: {
75  return KDL::Joint(jnt->name, KDL::Joint::None);
76  }
77  case urdf::Joint::REVOLUTE: {
78  KDL::Vector axis = toKdl(jnt->axis);
79  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
80  }
81  case urdf::Joint::CONTINUOUS: {
82  KDL::Vector axis = toKdl(jnt->axis);
83  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
84  }
85  case urdf::Joint::PRISMATIC: {
86  KDL::Vector axis = toKdl(jnt->axis);
87  return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
88  }
89  default: {
90  ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint",
91  jnt->name.c_str());
92  return KDL::Joint(jnt->name, KDL::Joint::None);
93  }
94  }
95  return KDL::Joint();
96 }
97 
98 // construct inertia
99 KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
100 {
101  KDL::Frame origin = toKdl(i->origin);
102 
103  // the mass is frame independent
104  double kdl_mass = i->mass;
105 
106  // kdl and urdf both specify the com position in the reference frame of the link
107  KDL::Vector kdl_com = origin.p;
108 
109  // kdl specifies the inertia matrix in the reference frame of the link,
110  // while the urdf specifies the inertia matrix in the inertia reference frame
111  KDL::RotationalInertia urdf_inertia =
112  KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
113 
114  // Rotation operators are not defined for rotational inertia,
115  // so we use the RigidBodyInertia operators (with com = 0) as a workaround
116  KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
117  origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia);
118 
119  // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
120  // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
121  // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
122  KDL::RotationalInertia kdl_inertia_wrt_com =
123  kdl_inertia_wrt_com_workaround.getRotationalInertia();
124 
125  return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);
126 }
127 
128 
129 // recursive function to walk through tree
130 bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
131 {
132  std::vector<urdf::LinkSharedPtr> children = root->child_links;
133  ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
134 
135  // constructs the optional inertia
136  KDL::RigidBodyInertia inert(0);
137  if (root->inertial) {
138  inert = toKdl(root->inertial);
139  }
140 
141  // constructs the kdl joint
142  KDL::Joint jnt = toKdl(root->parent_joint);
143 
144  // construct the kdl segment
145  KDL::Segment sgm(root->name, jnt, toKdl(
146  root->parent_joint->parent_to_joint_origin_transform), inert);
147 
148  // add segment to tree
149  tree.addSegment(sgm, root->parent_joint->parent_link_name);
150 
151  // recurslively add all children
152  for (size_t i = 0; i < children.size(); i++) {
153  if (!addChildrenToTree(children[i], tree)) {
154  return false;
155  }
156  }
157  return true;
158 }
159 
160 
161 bool treeFromFile(const std::string & file, KDL::Tree & tree)
162 {
163  TiXmlDocument urdf_xml;
164  urdf_xml.LoadFile(file);
165  return treeFromXml(&urdf_xml, tree);
166 }
167 
168 bool treeFromParam(const std::string & param, KDL::Tree & tree)
169 {
170  urdf::Model robot_model;
171  if (!robot_model.initParam(param)){
172  ROS_ERROR("Could not generate robot model");
173  return false;
174  }
175  return treeFromUrdfModel(robot_model, tree);
176 }
177 
178 bool treeFromString(const std::string & xml, KDL::Tree & tree)
179 {
180  TiXmlDocument urdf_xml;
181  urdf_xml.Parse(xml.c_str());
182  return treeFromXml(&urdf_xml, tree);
183 }
184 
185 bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
186 {
187  urdf::Model robot_model;
188  if (!robot_model.initXml(xml_doc)) {
189  ROS_ERROR("Could not generate robot model");
190  return false;
191  }
192  return treeFromUrdfModel(robot_model, tree);
193 }
194 
195 
196 bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
197 {
198  if (!robot_model.getRoot()) {
199  return false;
200  }
201 
202  tree = KDL::Tree(robot_model.getRoot()->name);
203 
204  // warn if root link has inertia. KDL does not support this
205  if (robot_model.getRoot()->inertial) {
206  ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not "
207  "support a root link with an inertia. As a workaround, you can add an extra "
208  "dummy link to your URDF.", robot_model.getRoot()->name.c_str());
209  }
210 
211  // add all children
212  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
213  if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
214  return false;
215  }
216  }
217 
218  return true;
219 }
220 
221 } // namespace kdl_parser
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
Definition: kdl_parser.cpp:196
KDL::Vector toKdl(urdf::Vector3 v)
Definition: kdl_parser.cpp:51
URDF_EXPORT bool initXml(TiXmlElement *xml)
static Rotation Quaternion(double x, double y, double z, double w)
#define ROS_WARN(...)
bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree &tree)
Definition: kdl_parser.cpp:130
Rotation M
KDL_PARSER_PUBLIC bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree &tree)
Definition: kdl_parser.cpp:185
KDL_PARSER_PUBLIC bool treeFromFile(const std::string &file, KDL::Tree &tree)
Definition: kdl_parser.cpp:161
KDL_PARSER_PUBLIC bool treeFromString(const std::string &xml, KDL::Tree &tree)
Definition: kdl_parser.cpp:178
URDF_EXPORT bool initParam(const std::string &param)
KDL_PARSER_PUBLIC bool treeFromParam(const std::string &param, KDL::Tree &tree)
Definition: kdl_parser.cpp:168
RotationalInertia getRotationalInertia() const
bool addSegment(const Segment &segment, const std::string &hook_name)
#define ROS_ERROR(...)
static Vector Zero()
#define ROS_DEBUG(...)


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Mon Jun 10 2019 13:42:36