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