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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Jun 21 2024 02:25:13