moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
constraint_sampler_manager.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36
40#include <rclcpp/logger.hpp>
41#include <rclcpp/logging.hpp>
42#include <sstream>
44
45namespace constraint_samplers
46{
47namespace
48{
49rclcpp::Logger getLogger()
50{
51 return moveit::getLogger("moveit.core.constraint_sampler_manager");
52}
53} // namespace
54
55ConstraintSamplerPtr ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene,
56 const std::string& group_name,
57 const moveit_msgs::msg::Constraints& constr) const
58{
59 for (const ConstraintSamplerAllocatorPtr& sampler : sampler_alloc_)
60 {
61 if (sampler->canService(scene, group_name, constr))
62 return sampler->alloc(scene, group_name, constr);
63 }
64
65 // if no default sampler was used, try a default one
66 return selectDefaultSampler(scene, group_name, constr);
67}
68
69ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr& scene,
70 const std::string& group_name,
71 const moveit_msgs::msg::Constraints& constr)
72{
73 const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name);
74 if (!jmg)
75 return ConstraintSamplerPtr();
76 std::stringstream ss;
77 ss << constr.name;
78 RCLCPP_DEBUG(getLogger(),
79 "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
80 jmg->getName().c_str(), ss.str().c_str());
81
82 ConstraintSamplerPtr joint_sampler; // location to put chosen joint sampler if needed
83 // if there are joint constraints, we could possibly get a sampler from those
84 if (!constr.joint_constraints.empty())
85 {
86 RCLCPP_DEBUG(getLogger(),
87 "There are joint constraints specified. "
88 "Attempting to construct a JointConstraintSampler for group '%s'",
89 jmg->getName().c_str());
90
91 std::map<std::string, bool> joint_coverage;
92 for (const std::string& joint : jmg->getVariableNames())
93 joint_coverage[joint] = false;
94
95 // construct the constraints
96 std::vector<kinematic_constraints::JointConstraint> jc;
97 for (const moveit_msgs::msg::JointConstraint& joint_constraint : constr.joint_constraints)
98 {
99 kinematic_constraints::JointConstraint j(scene->getRobotModel());
100 if (j.configure(joint_constraint))
101 {
102 if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
103 {
104 joint_coverage[j.getJointVariableName()] = true;
105 jc.push_back(j);
106 }
107 }
108 }
109
110 // check if every joint is covered (constrained) by just joint samplers
111 bool full_coverage = true;
112 for (const std::pair<const std::string, bool>& it : joint_coverage)
113 {
114 if (!it.second)
115 {
116 full_coverage = false;
117 break;
118 }
119 }
120
121 // if we have constrained every joint, then we just use a sampler using these constraints
122 if (full_coverage)
123 {
124 JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->getName());
125 if (sampler->configure(jc))
126 {
127 RCLCPP_DEBUG(getLogger(), "Allocated a sampler satisfying joint constraints for group '%s'",
128 jmg->getName().c_str());
129 return sampler;
130 }
131 }
132 else
133 // if a smaller set of joints has been specified, keep the constraint sampler around, but use it only if no IK
134 // sampler has been specified.
135 if (!jc.empty())
136 {
137 JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->getName());
138 if (sampler->configure(jc))
139 {
140 RCLCPP_DEBUG(getLogger(),
141 "Temporary sampler satisfying joint constraints for group '%s' allocated. "
142 "Looking for different types of constraints before returning though.",
143 jmg->getName().c_str());
144 joint_sampler = sampler;
145 }
146 }
147 }
148
149 std::vector<ConstraintSamplerPtr> samplers;
150 if (joint_sampler) // Start making a union of constraint samplers
151 samplers.push_back(joint_sampler);
152
153 // read the ik allocators, if any
155 const moveit::core::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second;
156
157 // if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints
158 // should be used
159 if (ik_alloc)
160 {
161 RCLCPP_DEBUG(getLogger(),
162 "There is an IK allocator for '%s'. "
163 "Checking for corresponding position and/or orientation constraints",
164 jmg->getName().c_str());
165
166 // keep track of which links we constrained
167 std::map<std::string, IKConstraintSamplerPtr> used_l;
168
169 // if we have position and/or orientation constraints on links that we can perform IK for,
170 // we will use a sampleable goal region that employs IK to sample goals;
171 // if there are multiple constraints for the same link, we keep the one with the smallest
172 // volume for sampling
173 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
174 {
175 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
176 {
177 if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
178 {
179 kinematic_constraints::PositionConstraintPtr pc(
180 new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
181 kinematic_constraints::OrientationConstraintPtr oc(
182 new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
183 if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
184 oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
185 {
186 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
187 if (iks->configure(IKSamplingPose(pc, oc)))
188 {
189 bool use = true;
190 // Check if there already is a constraint on this link
191 if (used_l.find(constr.position_constraints[p].link_name) != used_l.end())
192 {
193 // If there is, check if the previous one has a smaller volume for sampling
194 if (used_l[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
195 use = false; // only use new constraint if it has a smaller sampling volume
196 }
197 if (use)
198 {
199 // assign the link to a new constraint sampler
200 used_l[constr.position_constraints[p].link_name] = iks;
201 RCLCPP_DEBUG(getLogger(),
202 "Allocated an IK-based sampler for group '%s' "
203 "satisfying position and orientation constraints on link '%s'",
204 jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
205 }
206 }
207 }
208 }
209 }
210 }
211
212 // keep track of links constrained with a full pose
213 std::map<std::string, IKConstraintSamplerPtr> used_l_full_pose = used_l;
214
215 for (const moveit_msgs::msg::PositionConstraint& position_constraint : constr.position_constraints)
216 {
217 // if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint
218 // only
219 if (used_l_full_pose.find(position_constraint.link_name) != used_l_full_pose.end())
220 continue;
221
222 kinematic_constraints::PositionConstraintPtr pc(
223 new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
224 if (pc->configure(position_constraint, scene->getTransforms()))
225 {
226 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
227 if (iks->configure(IKSamplingPose(pc)))
228 {
229 bool use = true;
230 if (used_l.find(position_constraint.link_name) != used_l.end())
231 {
232 if (used_l[position_constraint.link_name]->getSamplingVolume() < iks->getSamplingVolume())
233 use = false;
234 }
235 if (use)
236 {
237 used_l[position_constraint.link_name] = iks;
238 RCLCPP_DEBUG(getLogger(),
239 "Allocated an IK-based sampler for group '%s' "
240 "satisfying position constraints on link '%s'",
241 jmg->getName().c_str(), position_constraint.link_name.c_str());
242 }
243 }
244 }
245 }
246
247 for (const moveit_msgs::msg::OrientationConstraint& orientation_constraint : constr.orientation_constraints)
248 {
249 // if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation
250 // constraint only
251 if (used_l_full_pose.find(orientation_constraint.link_name) != used_l_full_pose.end())
252 continue;
253
254 kinematic_constraints::OrientationConstraintPtr oc(
255 new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
256 if (oc->configure(orientation_constraint, scene->getTransforms()))
257 {
258 IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
259 if (iks->configure(IKSamplingPose(oc)))
260 {
261 bool use = true;
262 if (used_l.find(orientation_constraint.link_name) != used_l.end())
263 {
264 if (used_l[orientation_constraint.link_name]->getSamplingVolume() < iks->getSamplingVolume())
265 use = false;
266 }
267 if (use)
268 {
269 used_l[orientation_constraint.link_name] = iks;
270 RCLCPP_DEBUG(getLogger(),
271 "Allocated an IK-based sampler for group '%s' "
272 "satisfying orientation constraints on link '%s'",
273 jmg->getName().c_str(), orientation_constraint.link_name.c_str());
274 }
275 }
276 }
277 }
278
279 if (used_l.size() == 1)
280 {
281 if (samplers.empty())
282 {
283 return used_l.begin()->second;
284 }
285 else
286 {
287 samplers.push_back(used_l.begin()->second);
288 return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
289 }
290 }
291 else if (used_l.size() > 1)
292 {
293 RCLCPP_DEBUG(getLogger(),
294 "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
295 jmg->getName().c_str());
296 // find the sampler with the smallest sampling volume; delete the rest
297 IKConstraintSamplerPtr iks = used_l.begin()->second;
298 double msv = iks->getSamplingVolume();
299 for (std::map<std::string, IKConstraintSamplerPtr>::const_iterator it = ++used_l.begin(); it != used_l.end(); ++it)
300 {
301 double v = it->second->getSamplingVolume();
302 if (v < msv)
303 {
304 iks = it->second;
305 msv = v;
306 }
307 }
308 if (samplers.empty())
309 {
310 return iks;
311 }
312 else
313 {
314 samplers.push_back(iks);
315 return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
316 }
317 }
318 }
319
320 // if we got to this point, we have not decided on a sampler.
321 // we now check to see if we can use samplers from subgroups
322 if (!ik_subgroup_alloc.empty())
323 {
324 RCLCPP_DEBUG(getLogger(),
325 "There are IK allocators for subgroups of group '%s'. "
326 "Checking for corresponding position and/or orientation constraints",
327 jmg->getName().c_str());
328
329 bool some_sampler_valid = false;
330
331 std::set<std::size_t> used_p, used_o;
332 for (moveit::core::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin();
333 it != ik_subgroup_alloc.end(); ++it)
334 {
335 // construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator
336 moveit_msgs::msg::Constraints sub_constr;
337 for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
338 {
339 if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
340 {
341 if (used_p.find(p) == used_p.end())
342 {
343 sub_constr.position_constraints.push_back(constr.position_constraints[p]);
344 used_p.insert(p);
345 }
346 }
347 }
348
349 for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
350 {
351 if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
352 {
353 if (used_o.find(o) == used_o.end())
354 {
355 sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
356 used_o.insert(o);
357 }
358 }
359 }
360
361 // if some matching constraints were found, construct the allocator
362 if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
363 {
364 RCLCPP_DEBUG(getLogger(), "Attempting to construct a sampler for the '%s' subgroup of '%s'",
365 it->first->getName().c_str(), jmg->getName().c_str());
366 ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
367 if (cs)
368 {
369 RCLCPP_DEBUG(getLogger(),
370 "Constructed a sampler for the joints corresponding to group '%s', "
371 "but part of group '%s'",
372 it->first->getName().c_str(), jmg->getName().c_str());
373 some_sampler_valid = true;
374 samplers.push_back(cs);
375 }
376 }
377 }
378 if (some_sampler_valid)
379 {
380 RCLCPP_DEBUG(getLogger(), "Constructing sampler for group '%s' as a union of %zu samplers",
381 jmg->getName().c_str(), samplers.size());
382 return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
383 }
384 }
385
386 // if we've gotten here, just return joint sampler
387 if (joint_sampler)
388 {
389 RCLCPP_DEBUG(getLogger(), "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
390 return joint_sampler;
391 }
392
393 RCLCPP_DEBUG(getLogger(), "No constraints sampler allocated for group '%s'", jmg->getName().c_str());
394
395 return ConstraintSamplerPtr();
396}
397} // namespace constraint_samplers
static ConstraintSamplerPtr selectDefaultSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::msg::Constraints &constr)
Default logic to select a ConstraintSampler given a constraints message.
ConstraintSamplerPtr selectSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const moveit_msgs::msg::Constraints &constr) const
Selects among the potential sampler allocators.
Class for handling single DOF joint constraints.
const std::string & getJointVariableName() const
Gets the joint variable name, as known to the moveit::core::RobotModel.
bool configure(const moveit_msgs::msg::JointConstraint &jc)
Configure the constraint based on a moveit_msgs::msg::JointConstraint.
Class for constraints on the orientation of a link.
Class for constraints on the XYZ position of a link.
const std::string & getName() const
Get the name of the joint group.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
A structure for potentially holding a position constraint and an orientation constraint for use durin...