KoulesDirectedControlSampler.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, 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: Beck Chen, Mark Moll */
36 
37 #include "KoulesDirectedControlSampler.h"
38 #include "KoulesStateSpace.h"
39 #include <ompl/base/spaces/RealVectorStateSpace.h>
40 
41 namespace ob = ompl::base;
42 namespace oc = ompl::control;
43 
44 unsigned int KoulesDirectedControlSampler::sampleTo(oc::Control *control, const ob::State *source, ob::State *dest)
45 {
46  const double* dstPos = dest->as<KoulesStateSpace::StateType>()->values;
47  double stepSize = si_->getPropagationStepSize();
48  unsigned int steps = propagateMax_ ? si_->getMaxControlDuration() :
50 
51  cs_.steer(control, source, dstPos[0], dstPos[1]);
52  // perform the first step of propagation
53  statePropagator_->propagate(source, control, stepSize, dest);
54  // if we reached the goal, we're done
55  if (goal_->isSatisfied(dest))
56  return 1;
57  // if we found a valid state after one step, we can go on
58  else if (si_->isValid(dest))
59  {
60  ob::State *temp1 = dest, *temp2 = si_->allocState(), *toDelete = temp2;
61  unsigned int r = steps;
62  for (unsigned int i = 1 ; i < steps ; ++i)
63  {
64  statePropagator_->propagate(temp1, control, stepSize, temp2);
65  if (goal_->isSatisfied(dest))
66  {
67  si_->copyState(dest, temp2);
68  si_->freeState(toDelete);
69  return i + 1;
70  }
71  else if (si_->isValid(temp2))
72  std::swap(temp1, temp2);
73  else
74  {
75  // the last valid state is temp1;
76  r = i;
77  break;
78  }
79  }
80  // if we finished the for-loop without finding an invalid state, the last valid state is temp1
81  // make sure dest contains that information
82  if (dest != temp1)
83  si_->copyState(dest, temp1);
84  si_->freeState(toDelete);
85  return r;
86  }
87  // if the first propagation step produced an invalid step, return 0 steps
88  // the last valid state is the starting one (assumed to be valid)
89  else
90  {
91  if (dest != source)
92  si_->copyState(dest, source);
93  return 0;
94  }
95 }
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
Definition of an abstract control.
Definition: Control.h:47
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
void freeState(State *state) const
Free the memory of a state.
bool isValid(const State *state) const
Check if a given state is valid or not.
State * allocState() const
Allocate memory for a state.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void copyState(State *destination, const State *source) const
Copy a state to another.
virtual unsigned int sampleStepCount(unsigned int minSteps, unsigned int maxSteps)
Sample a number of steps to execute a control for.
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
virtual unsigned int sampleTo(ompl::control::Control *control, const ompl::base::State *source, ompl::base::State *dest)
Sample a control given that it will be applied to state state and the intention is to reach state tar...
const SpaceInformation * si_
The space information this sampler operates on.