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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 23 2020 03:52:30