OpenDESimpleSetup.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Ioan Sucan */
36 
37 #ifndef OMPL_EXTENSION_OPENDE_SIMPLE_SETUP_
38 #define OMPL_EXTENSION_OPENDE_SIMPLE_SETUP_
39 
40 #include "ompl/control/SimpleSetup.h"
41 #include "ompl/extensions/ode/OpenDEStateValidityChecker.h"
42 #include "ompl/extensions/ode/OpenDEStatePropagator.h"
43 #include "ompl/extensions/ode/OpenDEControlSpace.h"
44 
45 namespace ompl
46 {
47  namespace control
48  {
51  class OpenDESimpleSetup : public SimpleSetup
52  {
53  public:
55  explicit OpenDESimpleSetup(const ControlSpacePtr &space);
56 
58  explicit OpenDESimpleSetup(const base::StateSpacePtr &space);
59 
64  explicit OpenDESimpleSetup(const OpenDEEnvironmentPtr &env);
65 
66  ~OpenDESimpleSetup() override = default;
67 
70  {
71  return getStateSpace()->as<OpenDEStateSpace>()->getEnvironment();
72  }
73 
75  base::ScopedState<OpenDEStateSpace> getCurrentState() const;
76 
78  void setCurrentState(const base::ScopedState<> &state);
79 
81  void setCurrentState(const base::State *state);
82 
84  void setVolumeBounds(const base::RealVectorBounds &bounds)
85  {
86  getStateSpace()->as<OpenDEStateSpace>()->setVolumeBounds(bounds);
87  }
88 
90  void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
91  {
92  getStateSpace()->as<OpenDEStateSpace>()->setLinearVelocityBounds(bounds);
93  }
94 
96  void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
97  {
98  getStateSpace()->as<OpenDEStateSpace>()->setAngularVelocityBounds(bounds);
99  }
100 
105  void playPath(const base::PathPtr &path, double timeFactor = 1.0) const;
106 
108  void playSolutionPath(double timeFactor = 1.0) const;
109 
113  base::PathPtr simulateControl(const double *control, unsigned int steps) const;
114 
118  base::PathPtr simulateControl(const Control *control, unsigned int steps) const;
119 
124  base::PathPtr simulate(unsigned int steps) const;
125 
126  void setup() override;
127 
128  private:
129  void useEnvParams();
130  };
131  }
132 }
133 #endif
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
void setCurrentState(const base::ScopedState<> &state)
Set the current OpenDE state (set parameters for OpenDE bodies)
base::ScopedState< OpenDEStateSpace > getCurrentState() const
Get the current OpenDE state (read parameters from OpenDE bodies)
A shared pointer wrapper for ompl::control::ControlSpace.
void setVolumeBounds(const base::RealVectorBounds &bounds)
Set the bounds for the planning volume.
base::PathPtr simulate(unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the null control (ompl::con...
A shared pointer wrapper for ompl::control::OpenDEEnvironment.
OpenDESimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space needed for planning.
void playSolutionPath(double timeFactor=1.0) const
Call playPath() on the solution path, if one is available.
base::PathPtr simulateControl(const double *control, unsigned int steps) const
Simulate the OpenDE environment forward for steps simulation steps, using the control control....
void setAngularVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for the angular velocity.
const OpenDEEnvironmentPtr & getEnvironment() const
Get the OpenDE environment associated to the state and control spaces.
void playPath(const base::PathPtr &path, double timeFactor=1.0) const
Set the OpenDE world to the states that are contained in a given path, sequentially....
void setLinearVelocityBounds(const base::RealVectorBounds &bounds)
Set the bounds for the linear velocity.
Main namespace. Contains everything in this library.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156