KrisLibrary  1.0.0
KinodynamicSpace.h
1 #ifndef ROBOTICS_KINODYNAMIC_SPACE_H
2 #define ROBOTICS_KINODYNAMIC_SPACE_H
3 
4 #include <KrisLibrary/errors.h>
5 #include "ControlSpace.h"
6 #include "CSpaceHelpers.h"
7 
8 
23 {
24 public:
25  KinodynamicSpace(const std::shared_ptr<CSpace>& xspace,const std::shared_ptr<ControlSpace>& uspace);
26  virtual ~KinodynamicSpace();
27 
28  std::shared_ptr<ControlSpace> GetControlSpace() const { return controlSpace; }
29  std::shared_ptr<CSpace> GetStateSpace() const { return stateSpace; }
30  std::shared_ptr<CSet> GetControlSet(const Config& x) { return controlSpace->GetControlSet(x); }
31 
32  virtual EdgePlannerPtr PathChecker(const InterpolatorPtr& path) { FatalError("Visibility not checked using LocalPlanner for kinodynamic planning, use TrajectoryChecker instead"); return NULL; }
33 
36  virtual EdgePlannerPtr TrajectoryChecker(const ControlInput& u,const InterpolatorPtr& path);
37 
39  virtual EdgePlannerPtr TrajectoryChecker(const KinodynamicMilestonePath& path);
40 
41  bool IsValidControl(const State& x,const ControlInput& u) { return controlSpace->IsValidControl(x,u); }
42 
45  InterpolatorPtr Simulate(const State& x0, const ControlInput& u) { return controlSpace->Simulate(x0,u); }
46 
48  void Successor(const State& x0, const ControlInput& u,State& x1) { controlSpace->Successor(x0,u,x1); }
49 
53  bool NextState(const State& x0,const ControlInput& u,State& x1);
54 
58  bool PreviousState(const State& x1,const ControlInput& u,State& x0);
59 
61  virtual void Properties(PropertyMap& props) const;
62 
63  std::shared_ptr<CSpace> stateSpace;
64  std::shared_ptr<ControlSpace> controlSpace;
65 };
66 
93 {
94 public:
95  IntegratedKinodynamicSpace(const std::shared_ptr<CSpace>& xspace,const std::shared_ptr<IntegratedControlSpace>& controlSpace);
96  virtual EdgePlannerPtr TrajectoryChecker(const ControlInput& u,const InterpolatorPtr& path);
97 };
98 
104 {
105 public:
106  RandomBiasSteeringFunction(KinodynamicSpace* space,int sampleCount);
107  virtual bool IsExact() const { return false; };
108  virtual bool IsOptimal() const { return false; };
109  virtual bool Connect(const State& x,const State& y,KinodynamicMilestonePath& path);
110 
111  KinodynamicSpace* space;
112  int sampleCount;
113 };
114 
120 {
121 public:
122  RandomBiasReverseSteeringFunction(KinodynamicSpace* space,int sampleCount);
123  virtual bool IsExact() const { return false; };
124  virtual bool IsOptimal() const { return false; };
125  virtual bool Connect(const State& x,const State& y,KinodynamicMilestonePath& path);
126 
127  KinodynamicSpace* space;
128  int sampleCount;
129 };
130 
131 
142 {
143  public:
144  KinematicCSpaceAdaptor(const std::shared_ptr<CSpace>& base,Real maxNeighborhoodRadius=0.1);
145  virtual ~KinematicCSpaceAdaptor() {}
146 };
147 
148 
155 {
156 public:
157  KinodynamicSteeringCSpaceAdaptor(const std::shared_ptr<KinodynamicSpace>& kinodynamicSpace);
158  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
159  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int constraint);
160 
161  virtual Real Distance(const Config& x, const Config& y);
162  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
163  virtual void Properties(PropertyMap& map);
164 
165  std::shared_ptr<KinodynamicSpace> kinodynamicSpace;
166  std::shared_ptr<SteeringFunction> steeringFunction;
167 };
168 
169 #endif
InterpolatorPtr Simulate(const State &x0, const ControlInput &u)
Definition: KinodynamicSpace.h:45
Adapts a kinematic cspace (given to the constructor) to a kinodynamic one.
Definition: KinodynamicSpace.h:141
virtual void Properties(PropertyMap &props) const
Marks this as being a dynamic problem.
Definition: KinodynamicSpace.cpp:166
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Stores a kinodynamic path with piecewise constant controls.
Definition: KinodynamicPath.h:23
bool PreviousState(const State &x1, const ControlInput &u, State &x0)
Definition: KinodynamicSpace.cpp:155
A class that produces a KinodynamicCSpace from a dynamics function subclassed from IntegratedControlS...
Definition: KinodynamicSpace.h:92
virtual EdgePlannerPtr TrajectoryChecker(const ControlInput &u, const InterpolatorPtr &path)
Definition: KinodynamicSpace.cpp:114
void Successor(const State &x0, const ControlInput &u, State &x1)
Executes the simulation function x1 = f(x0,u)
Definition: KinodynamicSpace.h:48
A class used for kinodynamic planning. Combines a CSpace defining the state space, as well as a ControlSpace.
Definition: KinodynamicSpace.h:22
bool NextState(const State &x0, const ControlInput &u, State &x1)
Definition: KinodynamicSpace.cpp:146
Adapts a kinodynamic space with steering function to a kinematic cspace where the steering function i...
Definition: KinodynamicSpace.h:154
A helper class that assists with selective overriding of another cspace&#39;s methods (similar to "monkey...
Definition: CSpaceHelpers.h:196
A simple map from keys to values.
Definition: PropertyMap.h:27
A simple approximate steering function that simply draws several samples and picks the closest one in...
Definition: KinodynamicSpace.h:103
A function in a ControlSpace that attempts to connect two states with a sequence of one or more contr...
Definition: ControlSpace.h:98
A simple approximate steering function that simply draws several samples and picks the closest one in...
Definition: KinodynamicSpace.h:119