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