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
bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const
#define ROS_WARN_NAMED(name,...)
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.
bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts) override
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,...)
const std::string & getName() const
Get the name of the joint group.
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...
bool project(robot_state::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
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:122
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue May 26 2020 03:51:50