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