moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <ament_index_cpp/get_package_share_directory.hpp>
38 #include <boost/algorithm/string_regex.hpp>
39 #include <filesystem>
40 #include <geometry_msgs/msg/pose.hpp>
42 #include <rclcpp/logger.hpp>
43 #include <rclcpp/logging.hpp>
44 #include <urdf_parser/urdf_parser.h>
45 
46 #include <pluginlib/class_loader.hpp>
47 
49 
50 namespace moveit
51 {
52 namespace core
53 {
54 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_utils.robot_model_test_utils");
55 
56 moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
57 {
58  urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name);
59  srdf::ModelSharedPtr srdf = loadSRDFModel(robot_name);
60  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
61  return robot_model;
62 }
63 
64 urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
65 {
66  const std::string package_name = "moveit_resources_" + robot_name + "_description";
67  std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
68  std::string urdf_path;
69  if (robot_name == "pr2")
70  {
71  urdf_path = (res_path / "urdf/robot.xml").string();
72  }
73  else
74  {
75  urdf_path = (res_path / "urdf" / (robot_name + ".urdf")).string();
76  }
77  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
78  if (urdf_model == nullptr)
79  {
80  RCLCPP_ERROR(LOGGER, "Cannot find URDF for %s. Make sure moveit_resources_<your_robot_description> is installed",
81  robot_name.c_str());
82  }
83  return urdf_model;
84 }
85 
86 srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
87 {
88  urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name);
89  auto srdf_model = std::make_shared<srdf::Model>();
90  std::string srdf_path;
91  if (robot_name == "pr2")
92  {
93  const std::string package_name = "moveit_resources_" + robot_name + "_description";
94  std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
95  srdf_path = (res_path / "srdf/robot.xml").string();
96  }
97  else
98  {
99  const std::string package_name = "moveit_resources_" + robot_name + "_moveit_config";
100  std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
101  srdf_path = (res_path / "config" / (robot_name + ".srdf")).string();
102  }
103  srdf_model->initFile(*urdf_model, srdf_path);
104  return srdf_model;
105 }
106 
107 void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup* jmg, const std::string& base_link,
108  const std::string& tip_link, std::string plugin, double timeout)
109 {
110  using LoaderType = pluginlib::ClassLoader<kinematics::KinematicsBase>;
111  static std::weak_ptr<LoaderType> cached_loader;
112  std::shared_ptr<LoaderType> loader = cached_loader.lock();
113  if (!loader)
114  {
115  loader = std::make_shared<LoaderType>("moveit_core", "kinematics::KinematicsBase");
116  cached_loader = loader;
117  }
118 
119  // translate short to long names
120  if (plugin == "KDL")
121  plugin = "kdl_kinematics_plugin/KDLKinematicsPlugin";
122 
123  jmg->setSolverAllocators(
124  [=](const JointModelGroup* jmg) -> kinematics::KinematicsBasePtr {
125  kinematics::KinematicsBasePtr result = loader->createUniqueInstance(plugin);
126  result->initialize(node, jmg->getParentModel(), jmg->getName(), base_link, { tip_link }, 0.0);
127  result->setDefaultTimeout(timeout);
128  return result;
129  },
131 }
132 
133 RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name)
134  : urdf_model_(std::make_shared<urdf::ModelInterface>()), srdf_writer_(std::make_shared<srdf::SRDFWriter>())
135 {
136  urdf_model_->clear();
137  urdf_model_->name_ = name;
138 
139  auto base_link = std::make_shared<urdf::Link>();
140  base_link->name = base_link_name;
141  urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
142 
143  srdf_writer_->robot_name_ = name;
144 }
145 
146 void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
147  const std::vector<geometry_msgs::msg::Pose>& joint_origins, urdf::Vector3 joint_axis)
148 {
149  std::vector<std::string> link_names;
150  boost::split_regex(link_names, section, boost::regex("->"));
151  if (link_names.empty())
152  {
153  RCLCPP_ERROR(LOGGER, "No links specified (empty section?)");
154  is_valid_ = false;
155  return;
156  }
157  // First link should already be added.
158  if (!urdf_model_->getLink(link_names[0]))
159  {
160  RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_names[0].c_str());
161  is_valid_ = false;
162  return;
163  }
164 
165  if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
166  {
167  RCLCPP_ERROR(LOGGER, "There should be one more link (%zu) than there are joint origins (%zu)", link_names.size(),
168  joint_origins.size());
169  is_valid_ = false;
170  return;
171  }
172 
173  // Iterate through each link.
174  for (size_t i = 1; i < link_names.size(); ++i)
175  {
176  // These links shouldn't be present already.
177  if (urdf_model_->getLink(link_names[i]))
178  {
179  RCLCPP_ERROR(LOGGER, "Link %s is already specified", link_names[i].c_str());
180  is_valid_ = false;
181  return;
182  }
183  auto link = std::make_shared<urdf::Link>();
184  link->name = link_names[i];
185  urdf_model_->links_.insert(std::make_pair(link_names[i], link));
186  auto joint = std::make_shared<urdf::Joint>();
187  joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint";
188  // Default to Identity transform for origins.
189  joint->parent_to_joint_origin_transform.clear();
190  if (!joint_origins.empty())
191  {
192  geometry_msgs::msg::Pose o = joint_origins[i - 1];
193  joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
194  joint->parent_to_joint_origin_transform.rotation =
195  urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
196  }
197 
198  joint->parent_link_name = link_names[i - 1];
199  joint->child_link_name = link_names[i];
200  if (type == "planar")
201  {
202  joint->type = urdf::Joint::PLANAR;
203  }
204  else if (type == "floating")
205  {
206  joint->type = urdf::Joint::FLOATING;
207  }
208  else if (type == "revolute")
209  {
210  joint->type = urdf::Joint::REVOLUTE;
211  }
212  else if (type == "continuous")
213  {
214  joint->type = urdf::Joint::CONTINUOUS;
215  }
216  else if (type == "prismatic")
217  {
218  joint->type = urdf::Joint::PRISMATIC;
219  }
220  else if (type == "fixed")
221  {
222  joint->type = urdf::Joint::FIXED;
223  }
224  else
225  {
226  RCLCPP_ERROR(LOGGER, "No such joint type as %s", type.c_str());
227  is_valid_ = false;
228  return;
229  }
230 
231  joint->axis = joint_axis;
232  if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
233  {
234  auto limits = std::make_shared<urdf::JointLimits>();
235  limits->lower = -M_PI;
236  limits->upper = M_PI;
237 
238  joint->limits = limits;
239  }
240  urdf_model_->joints_.insert(std::make_pair(joint->name, joint));
241  }
242 }
243 
244 void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin,
245  double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
246 {
247  if (!urdf_model_->getLink(link_name))
248  {
249  RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str());
250  is_valid_ = false;
251  return;
252  }
253 
254  auto inertial = std::make_shared<urdf::Inertial>();
255  inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
256  inertial->origin.rotation =
257  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
258  inertial->mass = mass;
259  inertial->ixx = ixx;
260  inertial->ixy = ixy;
261  inertial->ixz = ixz;
262  inertial->iyy = iyy;
263  inertial->iyz = iyz;
264  inertial->izz = izz;
265 
266  urdf::LinkSharedPtr link;
267  urdf_model_->getLink(link_name, link);
268  link->inertial = inertial;
269 }
270 
271 void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
272  geometry_msgs::msg::Pose origin)
273 {
274  auto vis = std::make_shared<urdf::Visual>();
275  auto geometry = std::make_shared<urdf::Box>();
276  geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
277  vis->geometry = geometry;
278  addLinkVisual(link_name, vis, origin);
279 }
280 
281 void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std::vector<double>& dims,
282  geometry_msgs::msg::Pose origin)
283 {
284  if (dims.size() != 3)
285  {
286  RCLCPP_ERROR(LOGGER, "There can only be 3 dimensions of a box (given %zu!)", dims.size());
287  is_valid_ = false;
288  return;
289  }
290  auto coll = std::make_shared<urdf::Collision>();
291  auto geometry = std::make_shared<urdf::Box>();
292  geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
293  coll->geometry = geometry;
294  addLinkCollision(link_name, coll, origin);
295 }
296 
297 void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
298  geometry_msgs::msg::Pose origin)
299 {
300  auto coll = std::make_shared<urdf::Collision>();
301  auto geometry = std::make_shared<urdf::Mesh>();
302  geometry->filename = filename;
303  coll->geometry = geometry;
304  addLinkCollision(link_name, coll, origin);
305 }
306 
307 void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision,
308  geometry_msgs::msg::Pose origin)
309 {
310  if (!urdf_model_->getLink(link_name))
311  {
312  RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str());
313  is_valid_ = false;
314  return;
315  }
316  collision->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
317  collision->origin.rotation =
318  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
319 
320  urdf::LinkSharedPtr link;
321  urdf_model_->getLink(link_name, link);
322  link->collision_array.push_back(collision);
323 }
324 
325 void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis,
326  geometry_msgs::msg::Pose origin)
327 {
328  if (!urdf_model_->getLink(link_name))
329  {
330  RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str());
331  is_valid_ = false;
332  return;
333  }
334  vis->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
335  vis->origin.rotation =
336  urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
337 
338  urdf::LinkSharedPtr link;
339  urdf_model_->getLink(link_name, link);
340  if (!link->visual_array.empty())
341  {
342  link->visual_array.push_back(vis);
343  }
344  else if (link->visual)
345  {
346  link->visual_array.push_back(link->visual);
347  link->visual.reset();
348  link->visual_array.push_back(vis);
349  }
350  else
351  {
352  link->visual = vis;
353  }
354 }
355 
356 void RobotModelBuilder::addVirtualJoint(const std::string& parent_frame, const std::string& child_link,
357  const std::string& type, const std::string& name)
358 {
359  srdf::Model::VirtualJoint new_virtual_joint;
360  if (name.empty())
361  {
362  new_virtual_joint.name_ = parent_frame + "-" + child_link + "-virtual_joint";
363  }
364  else
365  {
366  new_virtual_joint.name_ = name;
367  }
368  new_virtual_joint.type_ = type;
369  new_virtual_joint.parent_frame_ = parent_frame;
370  new_virtual_joint.child_link_ = child_link;
371  srdf_writer_->virtual_joints_.push_back(new_virtual_joint);
372 }
373 
374 void RobotModelBuilder::addGroupChain(const std::string& base_link, const std::string& tip_link, const std::string& name)
375 {
376  srdf::Model::Group new_group;
377  if (name.empty())
378  {
379  new_group.name_ = base_link + "-" + tip_link + "-chain-group";
380  }
381  else
382  {
383  new_group.name_ = name;
384  }
385  new_group.chains_.push_back(std::make_pair(base_link, tip_link));
386  srdf_writer_->groups_.push_back(new_group);
387 }
388 
389 void RobotModelBuilder::addGroup(const std::vector<std::string>& links, const std::vector<std::string>& joints,
390  const std::string& name)
391 {
392  srdf::Model::Group new_group;
393  new_group.name_ = name;
394  new_group.links_ = links;
395  new_group.joints_ = joints;
396  srdf_writer_->groups_.push_back(new_group);
397 }
398 
399 void RobotModelBuilder::addEndEffector(const std::string& name, const std::string& parent_link,
400  const std::string& parent_group, const std::string& component_group)
401 {
402  srdf::Model::EndEffector eef;
403  eef.name_ = name;
404  eef.parent_link_ = parent_link;
405  eef.parent_group_ = parent_group;
406  eef.component_group_ = component_group;
407  srdf_writer_->end_effectors_.push_back(eef);
408 }
409 
410 void RobotModelBuilder::addJointProperty(const std::string& joint_name, const std::string& property_name,
411  const std::string& value)
412 {
413  srdf_writer_->joint_properties_[joint_name].push_back({ joint_name, property_name, value });
414 }
415 
417 {
418  return is_valid_;
419 }
420 
421 moveit::core::RobotModelPtr RobotModelBuilder::build()
422 {
423  moveit::core::RobotModelPtr robot_model;
424  std::map<std::string, std::string> parent_link_tree;
425  parent_link_tree.clear();
426 
427  try
428  {
429  urdf_model_->initTree(parent_link_tree);
430  }
431  catch (urdf::ParseError& e)
432  {
433  RCLCPP_ERROR(LOGGER, "Failed to build tree: %s", e.what());
434  return robot_model;
435  }
436 
437  // find the root link
438  try
439  {
440  urdf_model_->initRoot(parent_link_tree);
441  }
442  catch (urdf::ParseError& e)
443  {
444  RCLCPP_ERROR(LOGGER, "Failed to find root link: %s", e.what());
445  return robot_model;
446  }
447  srdf_writer_->updateSRDFModel(*urdf_model_);
448  robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_writer_->srdf_model_);
449  return robot_model;
450 }
451 } // namespace core
452 } // namespace moveit
const std::string & getName() const
Get the name of the joint group.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
void addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::msg::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
void addInertial(const std::string &link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
void addEndEffector(const std::string &name, const std::string &parent_link, const std::string &parent_group="", const std::string &component_group="")
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::msg::Pose origin)
Adds a collision box to a specific link.
void addJointProperty(const std::string &joint_name, const std::string &property_name, const std::string &value)
Adds a new joint property.
bool isValid()
Returns true if the building process so far has been valid.
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.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
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.
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 addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::msg::Pose origin)
Adds a collision mesh to a specific link.
RobotModelBuilder(const std::string &name, const std::string &base_link_name)
Constructor, takes the names of the robot and the base link.
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::msg::Pose origin)
Adds a visual box to a specific link.
std::map< const JointModelGroup *, SolverAllocatorFn > SolverAllocatorMapFn
Map from group instances to allocator functions & bijections.
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
srdf::ModelSharedPtr loadSRDFModel(const std::string &robot_name)
Loads an SRDF Model from moveit_resources.
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup *jmg, const std::string &base_link, const std::string &tip_link, std::string plugin="KDL", double timeout=0.1)
Load an IK solver plugin for the given joint model group.
Main namespace for MoveIt.
Definition: exceptions.h:43
string package_name
Definition: setup.py:4
name
Definition: setup.py:7