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 
43 #include <ros/package.h>
44 
45 namespace moveit
46 {
47 namespace core
48 {
49 const std::string LOGNAME = "robot_model_builder";
50 
51 moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
52 {
53  urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name);
55  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf, srdf));
56  return robot_model;
57 }
58 
59 urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
60 {
61  boost::filesystem::path res_path(ros::package::getPath("moveit_resources"));
62  std::string urdf_path;
63  if (robot_name == "pr2")
64  {
65  urdf_path = (res_path / "pr2_description/urdf/robot.xml").string();
66  }
67  else
68  {
69  urdf_path = (res_path / (robot_name + "_description") / "urdf" / (robot_name + ".urdf")).string();
70  }
71  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
72  if (urdf_model == nullptr)
73  {
74  ROS_ERROR_NAMED(LOGNAME, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
75  robot_name.c_str());
76  }
77  return urdf_model;
78 }
79 
80 srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
81 {
82  boost::filesystem::path res_path(ros::package::getPath("moveit_resources"));
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 = (res_path / "pr2_description/srdf/robot.xml").string();
89  }
90  else
91  {
92  srdf_path = (res_path / (robot_name + "_moveit_config") / "config" / (robot_name + ".srdf")).string();
93  }
94  srdf_model->initFile(*urdf_model, srdf_path);
95  return srdf_model;
96 }
97 
98 RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name)
99  : urdf_model_(new urdf::ModelInterface()), srdf_writer_(new srdf::SRDFWriter())
100 {
101  urdf_model_->clear();
102  urdf_model_->name_ = name;
103 
104  urdf::LinkSharedPtr base_link(new urdf::Link);
105  base_link->name = base_link_name;
106  urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
107 
108  srdf_writer_->robot_name_ = name;
109 }
110 
111 void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
112  const std::vector<geometry_msgs::Pose>& joint_origins)
113 {
114  std::vector<std::string> link_names;
115  boost::split_regex(link_names, section, boost::regex("->"));
116  if (link_names.empty())
117  {
118  ROS_ERROR_NAMED(LOGNAME, "No links specified (empty section?)");
119  is_valid_ = false;
120  return;
121  }
122  // First link should already be added.
123  if (!urdf_model_->getLink(link_names[0]))
124  {
125  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str());
126  is_valid_ = false;
127  return;
128  }
129 
130  if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
131  {
132  ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)",
133  link_names.size(), joint_origins.size());
134  is_valid_ = false;
135  return;
136  }
137 
138  // Iterate through each link.
139  for (size_t i = 1; i < link_names.size(); ++i)
140  {
141  // These links shouldn't be present already.
142  if (urdf_model_->getLink(link_names[i]))
143  {
144  ROS_ERROR_NAMED(LOGNAME, "Link %s is already specified", link_names[i].c_str());
145  is_valid_ = false;
146  return;
147  }
148  urdf::LinkSharedPtr link(new urdf::Link);
149  link->name = link_names[i];
150  urdf_model_->links_.insert(std::make_pair(link_names[i], link));
151  urdf::JointSharedPtr joint(new urdf::Joint);
152  joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint";
153  // Default to Identity transform for origins.
154  joint->parent_to_joint_origin_transform.clear();
155  if (!joint_origins.empty())
156  {
157  geometry_msgs::Pose o = joint_origins[i - 1];
158  joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
159  joint->parent_to_joint_origin_transform.rotation =
160  urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
161  }
162 
163  joint->parent_link_name = link_names[i - 1];
164  joint->child_link_name = link_names[i];
165  if (type == "planar")
166  joint->type = urdf::Joint::PLANAR;
167  else if (type == "floating")
168  joint->type = urdf::Joint::FLOATING;
169  else if (type == "revolute")
170  joint->type = urdf::Joint::REVOLUTE;
171  else if (type == "continuous")
172  joint->type = urdf::Joint::CONTINUOUS;
173  else if (type == "prismatic")
174  joint->type = urdf::Joint::PRISMATIC;
175  else if (type == "fixed")
176  joint->type = urdf::Joint::FIXED;
177  else
178  {
179  ROS_ERROR_NAMED(LOGNAME, "No such joint type as %s", type.c_str());
180  is_valid_ = false;
181  return;
182  }
183 
184  joint->axis = urdf::Vector3(1.0, 0.0, 0.0);
185  if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
186  {
187  urdf::JointLimitsSharedPtr limits(new urdf::JointLimits);
188  limits->lower = -boost::math::constants::pi<double>();
189  limits->upper = boost::math::constants::pi<double>();
190 
191  joint->limits = limits;
192  }
193  urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
194  }
195 }
196 
197 void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx,
198  double ixy, double ixz, double iyy, double iyz, double izz)
199 {
200  if (!urdf_model_->getLink(link_name))
201  {
202  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
203  is_valid_ = false;
204  return;
205  }
206 
207  urdf::InertialSharedPtr inertial(new urdf::Inertial);
208  inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
209  inertial->origin.rotation =
210  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
211  inertial->mass = mass;
212  inertial->ixx = ixx;
213  inertial->ixy = ixy;
214  inertial->ixz = ixz;
215  inertial->iyy = iyy;
216  inertial->iyz = iyz;
217  inertial->izz = izz;
218 
219  urdf::LinkSharedPtr link;
220  urdf_model_->getLink(link_name, link);
221  link->inertial = inertial;
222 }
223 
224 void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
225  geometry_msgs::Pose origin)
226 {
227  urdf::VisualSharedPtr vis(new urdf::Visual);
228  urdf::BoxSharedPtr geometry(new urdf::Box);
229  geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
230  vis->geometry = geometry;
231  addLinkVisual(link_name, vis, origin);
232 }
233 
234 void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std::vector<double>& dims,
235  geometry_msgs::Pose origin)
236 {
237  if (dims.size() != 3)
238  {
239  ROS_ERROR("There can only be 3 dimensions of a box (given %zu!)", dims.size());
240  is_valid_ = false;
241  return;
242  }
243  urdf::CollisionSharedPtr coll(new urdf::Collision);
244  urdf::BoxSharedPtr geometry(new urdf::Box);
245  geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
246  coll->geometry = geometry;
247  addLinkCollision(link_name, coll, origin);
248 }
249 
250 void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
251  geometry_msgs::Pose origin)
252 {
253  urdf::CollisionSharedPtr coll(new urdf::Collision);
254  urdf::MeshSharedPtr geometry(new urdf::Mesh);
255  geometry->filename = filename;
256  coll->geometry = geometry;
257  addLinkCollision(link_name, coll, origin);
258 }
259 
260 void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision,
261  geometry_msgs::Pose origin)
262 {
263  if (!urdf_model_->getLink(link_name))
264  {
265  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
266  is_valid_ = false;
267  return;
268  }
269  collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
270  collision->origin.rotation =
271  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
272 
273  urdf::LinkSharedPtr link;
274  urdf_model_->getLink(link_name, link);
275  link->collision_array.push_back(collision);
276 }
277 
278 void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis,
279  geometry_msgs::Pose origin)
280 {
281  if (!urdf_model_->getLink(link_name))
282  {
283  ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
284  is_valid_ = false;
285  return;
286  }
287  vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
288  vis->origin.rotation =
289  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
290 
291  urdf::LinkSharedPtr link;
292  urdf_model_->getLink(link_name, link);
293  if (!link->visual_array.empty())
294  {
295  link->visual_array.push_back(vis);
296  }
297  else if (link->visual)
298  {
299  link->visual_array.push_back(link->visual);
300  link->visual.reset();
301  link->visual_array.push_back(vis);
302  }
303  else
304  {
305  link->visual = vis;
306  }
307 }
308 
309 void RobotModelBuilder::addVirtualJoint(const std::string& parent_frame, const std::string& child_link,
310  const std::string& type, const std::string& name)
311 {
312  srdf::Model::VirtualJoint new_virtual_joint;
313  if (name.empty())
314  new_virtual_joint.name_ = parent_frame + "-" + child_link + "-virtual_joint";
315  else
316  new_virtual_joint.name_ = name;
317  new_virtual_joint.type_ = type;
318  new_virtual_joint.parent_frame_ = parent_frame;
319  new_virtual_joint.child_link_ = child_link;
320  srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
321 }
322 
323 void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link,
324  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 Sat Jul 11 2020 03:51:21