robot_model_test_utils.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Bryce Willey.
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 MoveIt! 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: Bryce Willey */
36 
37 #include <ros/ros.h>
38 #include <boost/algorithm/string_regex.hpp>
39 #include <boost/math/constants/constants.hpp>
40 #include <geometry_msgs/Pose.h>
41 
42 #include <urdf_parser/urdf_parser.h>
44 #include <ros/package.h>
45 
46 namespace moveit
47 {
48 namespace core
49 {
50 const std::string LOGNAME = "robot_model_builder";
51 
52 moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
53 {
54  urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name);
56  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf, srdf));
57  return robot_model;
58 }
59 
60 urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
61 {
62  std::string urdf_path;
63  if (robot_name == "pr2")
64  {
65  urdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/urdf/robot.xml";
66  }
67  else
68  {
69  urdf_path =
70  ros::package::getPath("moveit_resources_" + robot_name + "_description") + "/urdf/" + robot_name + ".urdf";
71  }
72  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
73  if (urdf_model == nullptr)
74  {
75  ROS_ERROR_NAMED(LOGNAME, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
76  robot_name.c_str());
77  }
78  return urdf_model;
79 }
80 
81 srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
82 {
83  urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name);
84  srdf::ModelSharedPtr srdf_model(new srdf::Model());
85  std::string srdf_path;
86  if (robot_name == "pr2")
87  {
88  srdf_path = ros::package::getPath("moveit_resources_pr2_description") + "/srdf/robot.xml";
89  }
90  else
91  {
92  srdf_path =
93  ros::package::getPath("moveit_resources_" + robot_name + "_moveit_config") + "/config/" + robot_name + ".srdf";
94  }
95  srdf_model->initFile(*urdf_model, srdf_path);
96  return srdf_model;
97 }
98 
99 RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name)
100  : urdf_model_(new urdf::ModelInterface()), srdf_writer_(new srdf::SRDFWriter())
101 {
102  urdf_model_->clear();
103  urdf_model_->name_ = name;
104 
105  urdf::LinkSharedPtr base_link(new urdf::Link);
106  base_link->name = base_link_name;
107  urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
108 
109  srdf_writer_->robot_name_ = name;
110 }
111 
112 void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
113  const std::vector<geometry_msgs::Pose>& joint_origins)
114 {
115  std::vector<std::string> link_names;
116  boost::split_regex(link_names, section, boost::regex("->"));
117  if (link_names.empty())
118  {
119  ROS_ERROR_NAMED(LOGNAME, "No links specified (empty section?)");
120  is_valid_ = false;
121  return;
122  }
123  // First link should already be added.
124  if (!urdf_model_->getLink(link_names[0]))
125  {
126  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str());
127  is_valid_ = false;
128  return;
129  }
130 
131  if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
132  {
133  ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)",
134  link_names.size(), joint_origins.size());
135  is_valid_ = false;
136  return;
137  }
138 
139  // Iterate through each link.
140  for (size_t i = 1; i < link_names.size(); ++i)
141  {
142  // These links shouldn't be present already.
143  if (urdf_model_->getLink(link_names[i]))
144  {
145  ROS_ERROR_NAMED(LOGNAME, "Link %s is already specified", link_names[i].c_str());
146  is_valid_ = false;
147  return;
148  }
149  urdf::LinkSharedPtr link(new urdf::Link);
150  link->name = link_names[i];
151  urdf_model_->links_.insert(std::make_pair(link_names[i], link));
152  urdf::JointSharedPtr joint(new urdf::Joint);
153  joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint";
154  // Default to Identity transform for origins.
155  joint->parent_to_joint_origin_transform.clear();
156  if (!joint_origins.empty())
157  {
158  geometry_msgs::Pose o = joint_origins[i - 1];
159  joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
160  joint->parent_to_joint_origin_transform.rotation =
161  urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
162  }
163 
164  joint->parent_link_name = link_names[i - 1];
165  joint->child_link_name = link_names[i];
166  if (type == "planar")
167  joint->type = urdf::Joint::PLANAR;
168  else if (type == "floating")
169  joint->type = urdf::Joint::FLOATING;
170  else if (type == "revolute")
171  joint->type = urdf::Joint::REVOLUTE;
172  else if (type == "continuous")
173  joint->type = urdf::Joint::CONTINUOUS;
174  else if (type == "prismatic")
175  joint->type = urdf::Joint::PRISMATIC;
176  else if (type == "fixed")
177  joint->type = urdf::Joint::FIXED;
178  else
179  {
180  ROS_ERROR_NAMED(LOGNAME, "No such joint type as %s", type.c_str());
181  is_valid_ = false;
182  return;
183  }
184 
185  joint->axis = urdf::Vector3(1.0, 0.0, 0.0);
186  if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
187  {
188  urdf::JointLimitsSharedPtr limits(new urdf::JointLimits);
189  limits->lower = -boost::math::constants::pi<double>();
190  limits->upper = boost::math::constants::pi<double>();
191 
192  joint->limits = limits;
193  }
194  urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
195  }
196 }
197 
198 void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx,
199  double ixy, double ixz, double iyy, double iyz, double izz)
200 {
201  if (!urdf_model_->getLink(link_name))
202  {
203  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
204  is_valid_ = false;
205  return;
206  }
207 
208  urdf::InertialSharedPtr inertial(new urdf::Inertial);
209  inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
210  inertial->origin.rotation =
211  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
212  inertial->mass = mass;
213  inertial->ixx = ixx;
214  inertial->ixy = ixy;
215  inertial->ixz = ixz;
216  inertial->iyy = iyy;
217  inertial->iyz = iyz;
218  inertial->izz = izz;
219 
220  urdf::LinkSharedPtr link;
221  urdf_model_->getLink(link_name, link);
222  link->inertial = inertial;
223 }
224 
225 void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
226  geometry_msgs::Pose origin)
227 {
228  urdf::VisualSharedPtr vis(new urdf::Visual);
229  urdf::BoxSharedPtr geometry(new urdf::Box);
230  geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
231  vis->geometry = geometry;
232  addLinkVisual(link_name, vis, origin);
233 }
234 
235 void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std::vector<double>& dims,
236  geometry_msgs::Pose origin)
237 {
238  if (dims.size() != 3)
239  {
240  ROS_ERROR("There can only be 3 dimensions of a box (given %zu!)", dims.size());
241  is_valid_ = false;
242  return;
243  }
244  urdf::CollisionSharedPtr coll(new urdf::Collision);
245  urdf::BoxSharedPtr geometry(new urdf::Box);
246  geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
247  coll->geometry = geometry;
248  addLinkCollision(link_name, coll, origin);
249 }
250 
251 void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
252  geometry_msgs::Pose origin)
253 {
254  urdf::CollisionSharedPtr coll(new urdf::Collision);
255  urdf::MeshSharedPtr geometry(new urdf::Mesh);
256  geometry->filename = filename;
257  coll->geometry = geometry;
258  addLinkCollision(link_name, coll, origin);
259 }
260 
261 void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision,
262  geometry_msgs::Pose origin)
263 {
264  if (!urdf_model_->getLink(link_name))
265  {
266  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
267  is_valid_ = false;
268  return;
269  }
270  collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
271  collision->origin.rotation =
272  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
273 
274  urdf::LinkSharedPtr link;
275  urdf_model_->getLink(link_name, link);
276  link->collision_array.push_back(collision);
277 }
278 
279 void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis,
280  geometry_msgs::Pose origin)
281 {
282  if (!urdf_model_->getLink(link_name))
283  {
284  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
285  is_valid_ = false;
286  return;
287  }
288  vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
289  vis->origin.rotation =
290  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
291 
292  urdf::LinkSharedPtr link;
293  urdf_model_->getLink(link_name, link);
294  if (!link->visual_array.empty())
295  {
296  link->visual_array.push_back(vis);
297  }
298  else if (link->visual)
299  {
300  link->visual_array.push_back(link->visual);
301  link->visual.reset();
302  link->visual_array.push_back(vis);
303  }
304  else
305  {
306  link->visual = vis;
307  }
308 }
309 
310 void RobotModelBuilder::addVirtualJoint(const std::string& parent_frame, const std::string& child_link,
311  const std::string& type, const std::string& name)
312 {
313  srdf::Model::VirtualJoint new_virtual_joint;
314  if (name.empty())
315  new_virtual_joint.name_ = parent_frame + "-" + child_link + "-virtual_joint";
316  else
317  new_virtual_joint.name_ = name;
318  new_virtual_joint.type_ = type;
319  new_virtual_joint.parent_frame_ = parent_frame;
320  new_virtual_joint.child_link_ = child_link;
321  srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
322 }
323 
324 void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name)
325 {
326  srdf::Model::Group new_group;
327  if (name.empty())
328  new_group.name_ = base_link + "-" + tip_link + "-chain-group";
329  else
330  new_group.name_ = name;
331  new_group.chains_.push_back(std::make_pair(base_link, tip_link));
332  srdf_writer_->groups_.push_back(new_group);
333 }
334 
335 void RobotModelBuilder::addGroup(const std::vector<std::string>& links, const std::vector<std::string>& joints,
336  const std::string& name)
337 {
338  srdf::Model::Group new_group;
339  new_group.name_ = name;
340  new_group.links_ = links;
341  new_group.joints_ = joints;
342  srdf_writer_->groups_.push_back(new_group);
343 }
344 
346 {
347  return is_valid_;
348 }
349 
350 moveit::core::RobotModelPtr RobotModelBuilder::build()
351 {
352  moveit::core::RobotModelPtr robot_model;
353  std::map<std::string, std::string> parent_link_tree;
354  parent_link_tree.clear();
355 
356  try
357  {
358  urdf_model_->initTree(parent_link_tree);
359  }
360  catch (urdf::ParseError& e)
361  {
362  ROS_ERROR_NAMED(LOGNAME, "Failed to build tree: %s", e.what());
363  return robot_model;
364  }
365 
366  // find the root link
367  try
368  {
369  urdf_model_->initRoot(parent_link_tree);
370  }
371  catch (urdf::ParseError& e)
372  {
373  ROS_ERROR_NAMED(LOGNAME, "Failed to find root link: %s", e.what());
374  return robot_model;
375  }
376  srdf_writer_->updateSRDFModel(*urdf_model_);
377  robot_model.reset(new moveit::core::RobotModel(urdf_model_, srdf_writer_->srdf_model_));
378  return robot_model;
379 }
380 } // namespace core
381 } // namespace moveit
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
bool is_valid_
Whether the current builder state is valid. If any &#39;add&#39; method fails, this becomes false...
Core components of MoveIt!
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
void addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::Pose > &joint_origins={})
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
std::vector< std::pair< std::string, std::string > > chains_
RobotModelBuilder(const std::string &name, const std::string &base_link_name)
Constructor, takes the names of the robot and the base link.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
#define ROS_ERROR(...)
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
urdf::ModelInterfaceSharedPtr urdf_model_
The URDF model, holds all of the URDF components of the robot added so far.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
std::string name_
srdf::SRDFWriterPtr srdf_writer_
The SRDF model, holds all of the SRDF components of the robot added so far.
ROSLIB_DECL std::string getPath(const std::string &package_name)
std::vector< std::string > links_
const std::string LOGNAME
bool isValid()
Returns true if the building process so far has been valid.
std::shared_ptr< Model > ModelSharedPtr
void addLinkCollision(const std::string &link_name, const urdf::CollisionSharedPtr &coll, geometry_msgs::Pose origin)
Adds different collision geometries to a link.
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::Pose origin)
Adds a visual box to a specific link.
std::vector< std::string > joints_
#define ROS_ERROR_NAMED(name,...)
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
srdf::ModelSharedPtr loadSRDFModel(const std::string &robot_name)
Loads an SRDF Model from moveit_resources.
Main namespace for MoveIt!
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::Pose origin)
Adds a collision box to a specific link.
void addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::Pose origin)
Adds a collision mesh to a specific link.
void addInertial(const std::string &link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
void addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain...
void addLinkVisual(const std::string &link_name, const urdf::VisualSharedPtr &vis, geometry_msgs::Pose origin)
Adds different visual geometries to a link.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:13:14