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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33