moveit2
The MoveIt Motion Planning Framework for ROS 2.
goal_union.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 
38 #include <ompl/base/goals/GoalLazySamples.h>
39 
40 namespace
41 {
42 ompl::base::SpaceInformationPtr getGoalsSI(const std::vector<ompl::base::GoalPtr>& goals)
43 {
44  if (goals.empty())
45  return ompl::base::SpaceInformationPtr();
46  for (const ompl::base::GoalPtr& goal : goals)
47  if (!goal->hasType(ompl::base::GOAL_SAMPLEABLE_REGION))
48  throw ompl::Exception("Multiplexed goals must be instances of GoalSampleableRegion");
49  for (const ompl::base::GoalPtr& goal : goals)
50  if (goal->getSpaceInformation() != goals[0]->getSpaceInformation())
51  throw ompl::Exception("The instance of SpaceInformation must be the same among the goals to be considered");
52  return goals[0]->getSpaceInformation();
53 }
54 } // namespace
55 
56 ompl_interface::GoalSampleableRegionMux::GoalSampleableRegionMux(const std::vector<ompl::base::GoalPtr>& goals)
57  : ompl::base::GoalSampleableRegion(getGoalsSI(goals)), goals_(goals), gindex_(0)
58 {
59 }
60 
62 {
63  for (ompl::base::GoalPtr& goal : goals_)
64  if (goal->hasType(ompl::base::GOAL_LAZY_SAMPLES))
65  static_cast<ompl::base::GoalLazySamples*>(goal.get())->startSampling();
66 }
67 
69 {
70  for (ompl::base::GoalPtr& goal : goals_)
71  if (goal->hasType(ompl::base::GOAL_LAZY_SAMPLES))
72  static_cast<ompl::base::GoalLazySamples*>(goal.get())->stopSampling();
73 }
74 
75 void ompl_interface::GoalSampleableRegionMux::sampleGoal(ompl::base::State* st) const
76 {
77  for (std::size_t i = 0; i < goals_.size(); ++i)
78  {
79  if (goals_[gindex_]->as<ompl::base::GoalSampleableRegion>()->maxSampleCount() > 0)
80  {
81  goals_[gindex_]->as<ompl::base::GoalSampleableRegion>()->sampleGoal(st);
82  return;
83  }
84  gindex_ = (gindex_ + 1) % goals_.size();
85  }
86  throw ompl::Exception("There are no states to sample");
87 }
88 
90 {
91  unsigned int sc = 0;
92  for (const ompl::base::GoalPtr& goal : goals_)
93  sc += goal->as<GoalSampleableRegion>()->maxSampleCount();
94  return sc;
95 }
96 
98 {
99  for (const ompl::base::GoalPtr& goal : goals_)
100  if (goal->as<ompl::base::GoalSampleableRegion>()->canSample())
101  return true;
102  return false;
103 }
104 
106 {
107  for (const ompl::base::GoalPtr& goal : goals_)
108  if (goal->as<ompl::base::GoalSampleableRegion>()->couldSample())
109  return true;
110  return false;
111 }
112 
113 bool ompl_interface::GoalSampleableRegionMux::isSatisfied(const ompl::base::State* st, double* distance) const
114 {
115  for (const ompl::base::GoalPtr& goal : goals_)
116  if (goal->isSatisfied(st, distance))
117  return true;
118  return false;
119 }
120 
121 double ompl_interface::GoalSampleableRegionMux::distanceGoal(const ompl::base::State* st) const
122 {
123  double min_d = std::numeric_limits<double>::infinity();
124  for (const ompl::base::GoalPtr& goal : goals_)
125  {
126  double d = goal->as<ompl::base::GoalRegion>()->distanceGoal(st);
127  if (d < min_d)
128  min_d = d;
129  }
130  return min_d;
131 }
132 
134 {
135  out << "MultiGoal [\n";
136  for (const ompl::base::GoalPtr& goal : goals_)
137  goal->print(out);
138  out << "]\n";
139 }
void startSampling()
If there are any member lazy samplers, start them.
Definition: goal_union.cpp:61
bool couldSample() const override
Query if sampler could find a sample in the future.
Definition: goal_union.cpp:105
double distanceGoal(const ompl::base::State *st) const override
Find the distance of this state from the goal.
Definition: goal_union.cpp:121
virtual bool canSample() const
Query if sampler can find any sample.
Definition: goal_union.cpp:97
unsigned int maxSampleCount() const override
Get the max sample count.
Definition: goal_union.cpp:89
GoalSampleableRegionMux(const std::vector< ompl::base::GoalPtr > &goals)
Constructor.
Definition: goal_union.cpp:56
void print(std::ostream &out=std::cout) const override
Pretty print goal information.
Definition: goal_union.cpp:133
void sampleGoal(ompl::base::State *st) const override
Sample a goal.
Definition: goal_union.cpp:75
bool isSatisfied(const ompl::base::State *st, double *distance) const override
Is the goal satisfied for this state (given a distance)
Definition: goal_union.cpp:113
void stopSampling()
If there are any member lazy samplers, stop them.
Definition: goal_union.cpp:68
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
d
Definition: setup.py:4