Klamp't  0.8.1
RealTimePlanner.h
1 #ifndef REAL_TIME_PLANNER_H
2 #define REAL_TIME_PLANNER_H
3 
4 #include "RobotCSpace.h"
5 #include "PlannerSettings.h"
6 #include "PlannerObjective.h"
7 #include "RampCSpace.h"
8 #include "Modeling/DynamicPath.h"
9 #include <KrisLibrary/utils/StatCollector.h>
10 #include <KrisLibrary/utils/threadutils.h>
11 #include <KrisLibrary/Timer.h>
12 
14 
25 {
26  public:
27  enum { Failure=0, Success=1, Timeout=2 };
28 
30  virtual ~DynamicMotionPlannerBase();
31  virtual void Init(CSpace* space,Robot* robot,WorldPlannerSettings* settings);
32  virtual void SetGoal(shared_ptr<PlannerObjectiveBase> newgoal);
33  virtual void SetTime(Real tstart);
34  virtual void SetDefaultLimits();
35  virtual void SetLimits(Real qScale=1.0,Real vScale=1.0,Real aScale=1.0);
36 
37  bool LogBegin(const char* fn="realtimeplanner.log");
38  bool LogEnd();
39 
40 
51  virtual int PlanFrom(ParabolicRamp::DynamicPath& path,Real cutoff)
52  {
53  return Failure;
54  }
55 
57  int PlanFrom(const Config& qstart,Real cutoff,ParabolicRamp::DynamicPath& result)
58  {
59  result.ramps.resize(1);
60  result.ramps[0].SetConstant(qstart);
61  return PlanFrom(result,cutoff);
62  }
63 
69  bool StopPlanning();
70 
72  int Shortcut(ParabolicRamp::DynamicPath& path,Real timeLimit);
75  int SmartShortcut(Real tstart,ParabolicRamp::DynamicPath& path,Real timeLimit);
76 
78  bool GetMilestoneRamp(const Config& q0,const Vector& dq0,const Config& q1,ParabolicRamp::DynamicPath& ramp) const;
80  bool GetMilestoneRamp(const ParabolicRamp::DynamicPath& curPath,const Config& q,ParabolicRamp::DynamicPath& ramp) const;
81 
82  //returns true if the ramp from the current config to q is collision free
83  bool CheckMilestoneRamp(const ParabolicRamp::DynamicPath& curPath,const Config& q,ParabolicRamp::DynamicPath& ramp) const;
84 
86  Real EvaluateDirectPathCost(const ParabolicRamp::DynamicPath& curPath,const Config& q);
87 
90  Real EvaluatePathCost(const ParabolicRamp::DynamicPath& path,Real tStart=-1.0);
91 
93  Real EvaluateTerminalCost(const Config& q,Real tEnd);
94 
95  Robot* robot;
96  WorldPlannerSettings* settings;
97  CSpace* cspace;
98  //objective function
99  shared_ptr<PlannerObjectiveBase> goal;
100  //configuration, velocity, and acceleration limits
101  ParabolicRamp::Vector qMin,qMax,velMax,accMax;
102 
103  //planning start time
104  Real tstart;
105  //flag: another thread may set this to true when the planner should
106  //stop during a long planning cycle. The subclass needs to continually
107  //monitor this flag
108  bool stopPlanning;
109 
110  //log file
111  FILE* flog;
112 };
113 
114 
119 {
120  public:
121  virtual ~SendPathCallbackBase() {}
129  virtual bool Send(Real tplanstart,Real tcut,const ParabolicRamp::DynamicPath& path) =0;
130 };
131 
132 
190 {
191 public:
192  RealTimePlanner();
193  virtual ~RealTimePlanner();
194 
196  void SetSpace(SingleRobotCSpace* space);
197 
199  void SetConstantPath(const Config& q);
202  void SetCurrentPath(Real tglobal,const ParabolicRamp::DynamicPath& path);
203 
205  virtual void Reset(shared_ptr<PlannerObjectiveBase> newgoal);
206 
208  shared_ptr<PlannerObjectiveBase> Objective() const;
209 
216  virtual bool PlanUpdate(Real tglobal,Real& splitTime,Real& planTime);
217 
219  bool StopPlanning() {
220  if(!planner) return true;
221  return planner->StopPlanning();
222  }
223 
226  virtual void MarkSendFailure();
227 
229 
231  shared_ptr<DynamicMotionPlannerBase> planner;
232 
235  ParabolicRamp::DynamicPath currentPath;
236 
238  shared_ptr<SendPathCallbackBase> sendPathCallback;
239 
243 
247 
248  enum SplitUpdateProtocol { Constant, ExponentialBackoff, Learning };
249  SplitUpdateProtocol protocol;
250  Real currentSplitTime,currentPadding,currentExternalPadding;
251  Real maxPadding;
252 
254  StatCollector planFailTimeStats,planSuccessTimeStats,planTimeoutTimeStats;
255 };
256 
286 {
287  public:
290 
292  void SetStartConfig(const Config& qstart);
294  void SetCSpace(SingleRobotCSpace* space);
296  void SetPlanner(const shared_ptr<DynamicMotionPlannerBase>& planner);
297  void SetPlanner(const shared_ptr<RealTimePlanner>& planner);
299  void SetObjective(shared_ptr<PlannerObjectiveBase> newgoal);
302  PlannerObjectiveBase* GetObjective() const;
305  void ResetCurrentPath(Real tglobal,const ParabolicRamp::DynamicPath& path);
308  bool Start();
310  void Stop();
312  bool IsPlanning();
314  void PausePlanning();
316  void BreakPlanning();
318  void StopPlanning();
320  void ResumePlanning();
322  bool HasUpdate();
324  bool SendUpdate(MotionQueueInterface* interface);
327  Real ObjectiveValue();
328 
329  void* internal;
330  shared_ptr<RealTimePlanner> planner;
331  Thread thread;
332 };
333 
334 #endif
int Shortcut(ParabolicRamp::DynamicPath &path, Real timeLimit)
Performs shortcutting up until the time limit.
A real-time planner. Supports constant time-stepping or adaptive time-stepping.
Definition: RealTimePlanner.h:189
The main robot type used in RobotSim.
Definition: Robot.h:79
StatCollector planFailTimeStats
Statistics captured on planning times, depending on PlanMore output.
Definition: RealTimePlanner.h:254
Real pathStartTime
Set the current path before planning, using SetConstantPath or SetCurrentPath.
Definition: RealTimePlanner.h:234
A structure containing settings that should be used for collision detection, contact solving...
Definition: PlannerSettings.h:41
A base class for the path sending callback. Send is called by the planner to update the path used by ...
Definition: RealTimePlanner.h:118
std::vector< ParabolicRampND > ramps
The path is stored as a series of ramps.
Definition: DynamicPath.h:160
An interface to a planning thread.
Definition: RealTimePlanner.h:285
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
bool StopPlanning()
Tells the planner to stop, when called from an external thread.
Definition: RealTimePlanner.h:219
Real EvaluateDirectPathCost(const ParabolicRamp::DynamicPath &curPath, const Config &q)
returns the cost of going straight to q (assuming no collision detection)
Real cognitiveMultiplier
Definition: RealTimePlanner.h:242
A cspace consisting of a single robot configuration in a RobotWorld. Feasibility constraints are join...
Definition: RobotCSpace.h:100
Real EvaluateTerminalCost(const Config &q, Real tEnd)
returns the terminal cost for a path ending at q at time tEnd
A base class for objective functionals in time/config/velocity space.
Definition: PlannerObjective.h:13
bool acceptTimeOverruns
Definition: RealTimePlanner.h:246
A base class for a motion planner that generates dynamic paths. The output should always respect join...
Definition: RealTimePlanner.h:24
shared_ptr< DynamicMotionPlannerBase > planner
Users must set these members before planning.
Definition: RealTimePlanner.h:231
shared_ptr< SendPathCallbackBase > sendPathCallback
Users should set this up to capture the outputted path.
Definition: RealTimePlanner.h:238
virtual int PlanFrom(ParabolicRamp::DynamicPath &path, Real cutoff)
Definition: RealTimePlanner.h:51
int SmartShortcut(Real tstart, ParabolicRamp::DynamicPath &path, Real timeLimit)
bool GetMilestoneRamp(const Config &q0, const Vector &dq0, const Config &q1, ParabolicRamp::DynamicPath &ramp) const
Helper.
int PlanFrom(const Config &qstart, Real cutoff, ParabolicRamp::DynamicPath &result)
Plans from a start configuration.
Definition: RealTimePlanner.h:57
Definition: RobotInterface.h:27
Real EvaluatePathCost(const ParabolicRamp::DynamicPath &path, Real tStart=-1.0)