ProjectedStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2017, Rice University
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 the Rice University 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: Zachary Kingston */
36 
37 #include "ompl/base/spaces/constraint/ProjectedStateSpace.h"
38 
39 #include <eigen3/Eigen/Core>
40 #include <utility>
41 
43 
45 
47  : WrapperStateSampler(space, std::move(sampler)), constraint_(space->getConstraint())
48 {
49 }
50 
52 {
54  constraint_->project(state);
55  space_->enforceBounds(state);
56 }
57 
58 void ompl::base::ProjectedStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
59 {
60  WrapperStateSampler::sampleUniformNear(state, near, distance);
61  constraint_->project(state);
62  space_->enforceBounds(state);
63 }
64 
65 void ompl::base::ProjectedStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
66 {
67  WrapperStateSampler::sampleGaussian(state, mean, stdDev);
68  constraint_->project(state);
69  space_->enforceBounds(state);
70 }
71 
73 
75 
76 bool ompl::base::ProjectedStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
77  std::vector<State *> *geodesic) const
78 {
79  // Save a copy of the from state.
80  if (geodesic != nullptr)
81  {
82  geodesic->clear();
83  geodesic->push_back(cloneState(from));
84  }
85 
86  const double tolerance = delta_;
87 
88  // No need to traverse the manifold if we are already there.
89  double dist, step, total = 0;
90  if ((dist = distance(from, to)) <= tolerance)
91  return true;
92 
93  const double max = dist * lambda_;
94 
95  auto previous = cloneState(from);
96  auto scratch = allocState();
97 
98  auto &&svc = si_->getStateValidityChecker();
99 
100  do
101  {
102  WrapperStateSpace::interpolate(previous, to, delta_ / dist, scratch);
103 
104  // Project new state onto constraint manifold
105  if (!constraint_->project(scratch) // not on manifold
106  || !(interpolate || svc->isValid(scratch)) // not valid
107  || (step = distance(previous, scratch)) > lambda_ * delta_) // deviated
108  break;
109 
110  // Check if we have wandered too far
111  total += step;
112  if (total > max)
113  break;
114 
115  // Check if we are no closer than before
116  const double newDist = distance(scratch, to);
117  if (newDist >= dist)
118  break;
119 
120  dist = newDist;
121  copyState(previous, scratch);
122 
123  // Store the new state
124  if (geodesic != nullptr)
125  geodesic->push_back(cloneState(scratch));
126 
127  } while (dist >= tolerance);
128 
129  freeState(scratch);
130  freeState(previous);
131 
132  return dist <= tolerance;
133 }
A state sampler that wraps around another state sampler.
A shared pointer wrapper for ompl::base::StateSampler.
void sampleUniform(State *state) override
Sample a state uniformly in ambient space and project to the manifold. Return sample in state...
STL namespace.
void sampleUniform(State *state) override
Sample a state using underlying sampler.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
Definition of an abstract state.
Definition: State.h:49
ProjectedStateSampler(const ProjectedStateSpace *space, StateSamplerPtr sampler)
Constructor.
ConstrainedStateSpace encapsulating a projection-based methodology for planning with constraints...
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state uniformly from a normal distribution with given mean and stdDev in ambient space and p...
bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State *> *geodesic=nullptr) const override
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state uniformly from the ball with center near and radius distance in ambient space and proj...