union_constraint_sampler.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 
39 #include <algorithm>
40 
41 namespace constraint_samplers
42 {
44 {
45  bool operator()(const ConstraintSamplerPtr& a, const ConstraintSamplerPtr& b) const
46  {
47  const std::vector<std::string>& alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
48  const std::vector<std::string>& blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
49  std::set<std::string> a_updates(alinks.begin(), alinks.end());
50  std::set<std::string> b_updates(blinks.begin(), blinks.end());
51 
52  bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(), b_updates.begin(), b_updates.end());
53 
54  bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(), a_updates.begin(), a_updates.end());
55 
56  // a contains b and sets are not equal
57  if (a_contains_b && !b_contains_a)
58  return true;
59  if (b_contains_a && !a_contains_b)
60  return false;
61 
62  // sets are equal or disjoint
63  bool a_depends_on_b = false;
64  bool b_depends_on_a = false;
65  const std::vector<std::string>& fda = a->getFrameDependency();
66  const std::vector<std::string>& fdb = b->getFrameDependency();
67  for (std::size_t i = 0; i < fda.size() && !a_depends_on_b; ++i)
68  for (std::size_t j = 0; j < blinks.size(); ++j)
69  if (blinks[j] == fda[i])
70  {
71  a_depends_on_b = true;
72  break;
73  }
74  for (std::size_t i = 0; i < fdb.size() && !b_depends_on_a; ++i)
75  for (std::size_t j = 0; j < alinks.size(); ++j)
76  if (alinks[j] == fdb[i])
77  {
78  b_depends_on_a = true;
79  break;
80  }
81  if (b_depends_on_a && a_depends_on_b)
82  {
83  ROS_WARN_NAMED("constraint_samplers",
84  "Circular frame dependency! "
85  "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
86  a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
87  return true;
88  }
89  if (b_depends_on_a && !a_depends_on_b)
90  return true;
91  if (a_depends_on_b && !b_depends_on_a)
92  return false;
93 
94  // prefer sampling JointConstraints first
95  JointConstraintSampler* ja = dynamic_cast<JointConstraintSampler*>(a.get());
96  JointConstraintSampler* jb = dynamic_cast<JointConstraintSampler*>(b.get());
97  if (ja && jb == nullptr)
98  return true;
99  if (jb && ja == nullptr)
100  return false;
101 
102  // neither depends on either, so break ties based on group name
103  return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
104  }
105 };
106 
107 UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene,
108  const std::string& group_name,
109  const std::vector<ConstraintSamplerPtr>& samplers)
110  : ConstraintSampler(scene, group_name), samplers_(samplers)
111 {
112  // using stable sort to preserve order of equivalents
113  std::stable_sort(samplers_.begin(), samplers_.end(), OrderSamplers());
114 
115  for (std::size_t i = 0; i < samplers_.size(); ++i)
116  {
117  const std::vector<std::string>& fd = samplers_[i]->getFrameDependency();
118  for (std::size_t j = 0; j < fd.size(); ++j)
119  frame_depends_.push_back(fd[j]);
120 
121  ROS_DEBUG_NAMED("constraint_samplers", "Union sampler for group '%s' includes sampler for group '%s'",
122  jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str());
123  }
124 }
125 
127  unsigned int max_attempts)
128 {
129  state = reference_state;
130  state.setToRandomPositions(jmg_);
131 
132  if (!samplers_.empty())
133  {
134  if (!samplers_[0]->sample(state, reference_state, max_attempts))
135  return false;
136  }
137 
138  for (std::size_t i = 1; i < samplers_.size(); ++i)
139  {
140  // ConstraintSampler::sample returns states with dirty link transforms (because it only writes values)
141  // but requires a state with clean link transforms as input. This means that we need to clean the link
142  // transforms between calls to ConstraintSampler::sample.
143  state.updateLinkTransforms();
144  if (!samplers_[i]->sample(state, state, max_attempts))
145  return false;
146  }
147  return true;
148 }
149 
150 bool UnionConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
151 {
152  for (std::size_t i = 0; i < samplers_.size(); ++i)
153  {
154  // ConstraintSampler::project returns states with dirty link transforms (because it only writes values)
155  // but requires a state with clean link transforms as input. This means that we need to clean the link
156  // transforms between calls to ConstraintSampler::sample.
157  state.updateLinkTransforms();
158  if (!samplers_[i]->project(state, max_attempts))
159  return false;
160  }
161  return true;
162 }
163 
164 } // end of namespace constraint_samplers
#define ROS_WARN_NAMED(name,...)
const std::string & getName() const
Get the name of the joint group.
UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::vector< ConstraintSamplerPtr > &samplers)
Constructor, which will re-order its internal list of samplers on construction.
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces a sample from all configured samplers.
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
#define ROS_DEBUG_NAMED(name,...)
bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const
std::vector< ConstraintSamplerPtr > samplers_
Holder for sorted internal list of samplers.
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn...
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05