moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
planning_groups.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021, PickNik Robotics
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 PickNik Robotics 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: David V. Lu!! */
36
39#include <pluginlib/class_loader.hpp> // for loading all avail kinematic planners
40
42#include <boost/graph/adjacency_list.hpp>
43#include <boost/graph/depth_first_search.hpp>
44
45namespace moveit_setup
46{
47namespace srdf_setup
48{
49// Used for checking for cycles in a subgroup hierarchy
50struct CycleDetector : public boost::dfs_visitor<>
51{
52 CycleDetector(bool& has_cycle) : m_has_cycle_(has_cycle)
53 {
54 }
55
56 template <class Edge, class Graph>
57 void backEdge(Edge /*unused*/, Graph& /*unused*/)
58 {
59 m_has_cycle_ = true;
60 }
61
62protected:
64};
65
67{
69 config_data_->registerType("group_meta", "moveit_setup::srdf_setup::GroupMetaConfig");
71}
72
73void PlanningGroups::renameGroup(const std::string& old_group_name, const std::string& new_group_name)
74{
75 // Rename the actual group
76 rename(old_group_name, new_group_name);
77
78 long changes = 0L;
79
80 // Change all references to this group name in other subgroups
81 // Loop through every group
82 for (srdf::Model::Group& group : srdf_config_->getGroups())
83 {
84 // Loop through every subgroup
85 for (std::string& subgroup : group.subgroups_)
86 {
87 // Check if that subgroup references old group name. if so, update it
88 if (subgroup == old_group_name) // same name
89 {
90 subgroup.assign(new_group_name); // updated
91 changes |= GROUP_CONTENTS;
92 }
93 }
94 }
95
96 // Change all references to this group name in the end effectors screen
97 for (srdf::Model::EndEffector& eef : srdf_config_->getEndEffectors())
98 {
99 // Check if this eef's parent group references old group name. if so, update it
100 if (eef.parent_group_ == old_group_name) // same name
101 {
102 RCLCPP_DEBUG_STREAM(*logger_, "Changed eef '" << eef.name_ << "' to new parent group name " << new_group_name);
103 eef.parent_group_ = new_group_name; // updated
104 changes |= END_EFFECTORS;
105 }
106
107 // Check if this eef's group references old group name. if so, update it
108 if (eef.component_group_.compare(old_group_name) == 0) // same name
109 {
110 RCLCPP_DEBUG_STREAM(*logger_, "Changed eef '" << eef.name_ << "' to new group name " << new_group_name);
111 eef.component_group_ = new_group_name; // updated
112 changes |= END_EFFECTORS;
113 }
114 }
115
116 // Change all references to this group name in the robot poses screen
117 for (srdf::Model::GroupState& gs : srdf_config_->getGroupStates())
118 {
119 // Check if this eef's parent group references old group name. if so, update it
120 if (gs.group_ == old_group_name) // same name
121 {
122 RCLCPP_DEBUG_STREAM(*logger_, "Changed group state group '" << gs.group_ << "' to new parent group name "
123 << new_group_name);
124 gs.group_ = new_group_name; // updated
125 changes |= POSES;
126 }
127 }
128
129 group_meta_config_->renameGroup(old_group_name, new_group_name);
130 changes |= GROUPS;
131
132 // Now update the robot model based on our changed to the SRDF
133 srdf_config_->updateRobotModel(changes);
134}
135
136void PlanningGroups::deleteGroup(const std::string& group_name)
137{
138 long changes = 0L;
139
140 // Remove poses in this group
141 for (const std::string& pose_name : getPosesByGroup(group_name))
142 {
143 srdf_config_->removePoseByName(pose_name, group_name);
144 }
145
146 // Remove end effectors in this group
147 auto& eefs = srdf_config_->getEndEffectors();
148 auto it = eefs.begin();
149 while (it != eefs.end())
150 {
151 if (it->component_group_ == group_name)
152 {
153 it = eefs.erase(it);
154 changes |= END_EFFECTORS;
155 }
156 else
157 {
158 it++;
159 }
160 }
161
162 // delete actual group
163 remove(group_name);
164
165 // Delete references in subgroups
166 for (srdf::Model::Group& group : srdf_config_->getGroups())
167 {
168 auto& subgroups = group.subgroups_;
169 std::vector<std::string>::iterator subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name);
170 while (subgroup_it != subgroups.end())
171 {
172 subgroups.erase(subgroup_it);
173 changes |= GROUP_CONTENTS;
174 subgroup_it = std::find(subgroups.begin(), subgroups.end(), group_name);
175 }
176 }
177
178 group_meta_config_->deleteGroup(group_name);
179
180 srdf_config_->updateRobotModel(changes);
181}
182
183void PlanningGroups::setJoints(const std::string& group_name, const std::vector<std::string>& joint_names)
184{
185 // Find the group we are editing based on the group name string
186 srdf::Model::Group* searched_group = find(group_name);
187
188 // save the data
189 searched_group->joints_ = joint_names;
190
191 // Update the kinematic model with changes
192 srdf_config_->updateRobotModel(GROUP_CONTENTS);
193}
194
195void PlanningGroups::setLinks(const std::string& group_name, const std::vector<std::string>& link_names)
196{
197 // Find the group we are editing based on the group name string
198 srdf::Model::Group* searched_group = find(group_name);
199
200 // save the data
201 searched_group->links_ = link_names;
202
203 // Update the kinematic model with changes
204 srdf_config_->updateRobotModel(GROUP_CONTENTS);
205}
206
207void PlanningGroups::setChain(const std::string& group_name, const std::string& base, const std::string& tip)
208{
209 // Check that box the tip and base, or neither, have text
210 if ((!tip.empty() && base.empty()) || (tip.empty() && !base.empty()))
211 {
212 throw std::runtime_error("You must specify a link for both the base and tip, or leave both "
213 "blank.");
214 }
215
216 // Check that both given links are valid links, unless they are both blank
217 if (!tip.empty() && !base.empty())
218 {
219 // Check that they are not the same link
220 if (tip.compare(base) == 0) // they are same
221 {
222 throw std::runtime_error("Tip and base link cannot be the same link.");
223 }
224
225 bool found_tip = false;
226 bool found_base = false;
227 const std::vector<std::string>& links = getLinkNames();
228
229 for (const std::string& link : links)
230 {
231 // Check if string matches either of user specified links
232 if (link.compare(tip) == 0)
233 { // they are same
234 found_tip = true;
235 }
236 else if (link.compare(base) == 0)
237 { // they are same
238 found_base = true;
239 }
240
241 // Check if we are done searching
242 if (found_tip && found_base)
243 break;
244 }
245
246 // Check if we found both links
247 if (!found_tip || !found_base)
248 {
249 throw std::runtime_error("Tip or base link(s) were not found in kinematic chain.");
250 }
251 }
252
253 // Find the group we are editing based on the group name string
254 srdf::Model::Group* searched_group = find(group_name);
255
256 // clear the old data
257 searched_group->chains_.clear();
258
259 // Save the data if there is data to save
260 if (!tip.empty() && !base.empty())
261 {
262 searched_group->chains_.push_back(std::pair<std::string, std::string>(base, tip));
263 }
264
265 // Update the kinematic model with changes
266 srdf_config_->updateRobotModel(GROUP_CONTENTS);
267}
268
269void PlanningGroups::setSubgroups(const std::string& selected_group_name, const std::vector<std::string>& subgroups)
270{
271 // Check for cycles -------------------------------
272
273 // Create vector index of all nodes
274 std::map<std::string, int> group_nodes;
275
276 // Create vector of all nodes for use as id's
277 int node_id = 0;
278 for (const std::string& group_name : getGroupNames())
279 {
280 // Add string to vector
281 group_nodes.insert(std::pair<std::string, int>(group_name, node_id));
282 ++node_id;
283 }
284
285 // Create the empty graph
286 typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS> Graph;
287 Graph g(group_nodes.size());
288
289 // Traverse the group list again, this time inserting subgroups into graph
290 int from_id = 0; // track the outer node we are on to reduce searches performed
291 for (srdf::Model::Group& group : srdf_config_->getGroups())
292 {
293 // Check if group_it is same as current group
294 if (group.name_ == selected_group_name) // yes, same group
295 {
296 // add new subgroup list from widget, not old one. this way we can check for new cycles
297 for (const std::string& to_string : subgroups)
298 {
299 // convert subgroup string to associated id
300 int to_id = group_nodes[to_string];
301
302 // Add edge from from_id to to_id
303 add_edge(from_id, to_id, g);
304 }
305 }
306 else // this group is not the group we are editing, so just add subgroups from memory
307 {
308 // add new subgroup list from widget, not old one. this way we can check for new cycles
309 for (const std::string& to_string : group.subgroups_)
310 {
311 // Get std::string of subgroup
312 // convert subgroup string to associated id
313 int to_id = group_nodes[to_string];
314
315 // Add edge from from_id to to_id
316 add_edge(from_id, to_id, g);
317 }
318 }
319
320 ++from_id;
321 }
322
323 // Check for cycles
324 bool has_cycle = false;
325 CycleDetector vis(has_cycle);
326 boost::depth_first_search(g, visitor(vis));
327
328 if (has_cycle)
329 {
330 throw std::runtime_error("Depth first search reveals a cycle in the subgroups");
331 }
332
333 // Find the group we are editing based on the group name string
334 srdf::Model::Group* searched_group = find(selected_group_name);
335
336 // save the data
337 searched_group->subgroups_ = subgroups;
338
339 // Update the kinematic model with changes
340 srdf_config_->updateRobotModel(GROUP_CONTENTS);
341}
342
343std::string PlanningGroups::getChildOfJoint(const std::string& joint_name) const
344{
345 return srdf_config_->getChildOfJoint(joint_name);
346}
347
348std::string PlanningGroups::getJointType(const std::string& joint_name) const
349{
350 const moveit::core::JointModel* joint_model = srdf_config_->getRobotModel()->getJointModel(joint_name);
351 if (!joint_model)
352 {
353 return "";
354 }
355 return joint_model->getTypeName();
356}
357
359{
360 const moveit::core::JointModel* root_joint = srdf_config_->getRobotModel()->getRootJoint();
361 return buildLinkNameTree(root_joint->getChildLinkModel());
362}
363
364std::vector<std::string> PlanningGroups::getPosesByGroup(const std::string& group_name) const
365{
366 std::vector<std::string> pose_names;
367 for (const srdf::Model::GroupState& pose : srdf_config_->getGroupStates())
368 {
369 if (pose.group_ == group_name)
370 {
371 pose_names.push_back(pose.name_);
372 }
373 }
374 return pose_names;
375}
376
377std::vector<std::string> PlanningGroups::getEndEffectorsByGroup(const std::string& group_name) const
378{
379 std::vector<std::string> eef_names;
380 for (const srdf::Model::EndEffector& eef : srdf_config_->getEndEffectors())
381 {
382 if (eef.component_group_ == group_name)
383 {
384 eef_names.push_back(eef.name_);
385 }
386 }
387 return eef_names;
388}
389
390std::vector<std::string> PlanningGroups::getKinematicPlanners() const
391{
392 // load all avail kin planners
393 std::unique_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase>> loader;
394 try
395 {
396 loader = std::make_unique<pluginlib::ClassLoader<kinematics::KinematicsBase>>("moveit_core",
397 "kinematics::KinematicsBase");
398 }
399 catch (pluginlib::PluginlibException& ex)
400 {
401 throw std::runtime_error(std::string("Exception while creating class loader for kinematic "
402 "solver plugins: ") +
403 ex.what());
404 }
405
406 std::vector<std::string> planners(loader->getDeclaredClasses());
407
408 // Warn if no plugins are found
409 if (planners.empty())
410 {
411 throw std::runtime_error("No MoveIt-compatible kinematics solvers found. Try "
412 "installing moveit_kinematics (sudo apt-get install "
413 "ros-${ROS_DISTRO}-moveit-kinematics)");
414 }
415 return planners;
416}
417
418std::vector<std::string> PlanningGroups::getOMPLPlanners() const
419{
420 std::vector<std::string> planner_names;
421 // TODO: This should call ompl_interface::PlanningContextManager::getRegisteredPlannerAllocators to load the
422 // names dynamically
423 planner_names.push_back("AnytimePathShortening");
424 planner_names.push_back("SBL");
425 planner_names.push_back("EST");
426 planner_names.push_back("LBKPIECE");
427 planner_names.push_back("BKPIECE");
428 planner_names.push_back("KPIECE");
429 planner_names.push_back("RRT");
430 planner_names.push_back("RRTConnect");
431 planner_names.push_back("RRTstar");
432 planner_names.push_back("TRRT");
433 planner_names.push_back("PRM");
434 planner_names.push_back("PRMstar");
435 planner_names.push_back("FMT");
436 planner_names.push_back("BFMT");
437 planner_names.push_back("PDST");
438 planner_names.push_back("STRIDE");
439 planner_names.push_back("BiTRRT");
440 planner_names.push_back("LBTRRT");
441 planner_names.push_back("BiEST");
442 planner_names.push_back("ProjEST");
443 planner_names.push_back("LazyPRM");
444 planner_names.push_back("LazyPRMstar");
445 planner_names.push_back("SPARS");
446 planner_names.push_back("SPARStwo");
447
448 return planner_names;
449}
450
451} // namespace srdf_setup
452} // namespace moveit_setup
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
std::string getTypeName() const
Get the type of joint as a string.
DataWarehousePtr config_data_
virtual void onInit()
Overridable initialization method.
std::shared_ptr< rclcpp::Logger > logger_
void setChain(const std::string &group_name, const std::string &base, const std::string &tip)
Set the specified group's kinematic chain.
void onInit() override
Overridable initialization method.
std::vector< std::string > getGroupNames() const
void setLinks(const std::string &group_name, const std::vector< std::string > &link_names)
Set the specified group's link names.
void setSubgroups(const std::string &selected_group_name, const std::vector< std::string > &subgroups)
Set the specified group's subgroups.
void renameGroup(const std::string &old_group_name, const std::string &new_group_name)
std::string getChildOfJoint(const std::string &joint_name) const
void deleteGroup(const std::string &group_name)
std::vector< std::string > getPosesByGroup(const std::string &group_name) const
std::string getJointType(const std::string &joint_name) const
std::shared_ptr< GroupMetaConfig > group_meta_config_
std::vector< std::string > getKinematicPlanners() const
std::vector< std::string > getOMPLPlanners() const
const std::vector< std::string > & getLinkNames() const
std::vector< std::string > getEndEffectorsByGroup(const std::string &group_name) const
void setJoints(const std::string &group_name, const std::vector< std::string > &joint_names)
Set the specified group's joint names.
std::shared_ptr< SRDFConfig > srdf_config_
Definition srdf_step.hpp:67
srdf::Model::Group * find(const std::string &name)
Return a pointer to an item with the given name if it exists, otherwise null.
Definition srdf_step.hpp:98
srdf::Model::Group * rename(const std::string &old_name, const std::string &new_name)
Renames an item and returns a pointer to the item.
bool remove(const std::string &name)
Delete an item with the given name from the list.
LinkNameTree buildLinkNameTree(const moveit::core::LinkModel *link)