Klamp't  0.8.1
PlannerObjective.h
1 #ifndef PLANNER_OBJECTIVES_H
2 #define PLANNER_OBJECTIVES_H
3 
4 #include <Klampt/Modeling/Robot.h>
6 #include <Klampt/Modeling/DynamicPath.h>
7 #include <KrisLibrary/robotics/IK.h>
8 
14 {
15  public:
17  virtual ~PlannerObjectiveBase() {}
18 
20  virtual const char* TypeString() { return NULL; }
21 
23  virtual string Description() { if(TypeString()) return TypeString(); return ""; }
24 
27  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) { return 0.0; }
28 
31  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq) { return 0.0; }
32 
34  virtual bool DifferentialTimeInvariant() const { return false; }
35  virtual bool TerminalTimeInvariant() const { return false; }
37  virtual bool PathInvariant() const { return false; }
38 
42  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp) { return 0.0; }
43  virtual Real IncrementalCost(Real t,const ParabolicRamp::DynamicPath& path);
44 
47  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
48 
51  virtual Real Delta(PlannerObjectiveBase* priorGoal) { return Inf; }
52 
54  virtual bool Read(File& file) { return false; }
55  virtual bool Write(File& file) { return false; }
56 };
57 
62 {
63  public:
64  TimeObjective() {}
65  virtual ~TimeObjective() {}
66  virtual const char* TypeString() { return "time"; }
67  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
68  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq) { return 1.0; }
69  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
70  virtual Real Delta(PlannerObjectiveBase* priorGoal);
71  virtual bool DifferentialTimeInvariant() const { return true; }
72  virtual bool TerminalTimeInvariant() const { return true; }
73 };
74 
80 {
81  public:
82  TerminalTimeObjective(Real tgoal);
83  virtual ~TerminalTimeObjective() {}
84  virtual const char* TypeString() { return "term_time"; }
85  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
86  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
87  virtual Real Delta(PlannerObjectiveBase* priorGoal);
88  virtual bool DifferentialTimeInvariant() const { return true; }
89 
90  Real tgoal;
91 };
92 
97 {
98  public:
99  ConfigObjective(const Config& qgoal);
100  virtual ~ConfigObjective() {}
101  virtual const char* TypeString() { return "config"; }
102  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) {
103  return qend.distance(qgoal);
104  }
105  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
106  return qgoal.distance(path.ramps.back().x1);
107  }
108  virtual Real Delta(PlannerObjectiveBase* priorGoal);
109  virtual bool TerminalTimeInvariant() const { return true; }
110  virtual bool DifferentialTimeInvariant() const { return true; }
111  virtual bool PathInvariant() const { return true; }
112 
113  Vector qgoal;
114 };
115 
120 {
121  public:
122  VelocityObjective(const Vector& vgoal);
123  virtual ~VelocityObjective() {}
124  virtual const char* TypeString() { return "velocity"; }
125  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend) {
126  return dqend.distance(vgoal);
127  }
128  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
129  return vgoal.distance(path.ramps.back().dx1);
130  }
131  virtual Real Delta(PlannerObjectiveBase* priorGoal);
132  virtual bool TerminalTimeInvariant() const { return true; }
133  virtual bool DifferentialTimeInvariant() const { return true; }
134  virtual bool PathInvariant() const { return true; }
135 
136  Vector vgoal;
137 };
138 
144 {
145  public:
148 
150  void Add(const shared_ptr<PlannerObjectiveBase>& obj,Real weight=1.0);
151 
152  virtual const char* TypeString() { return "composite"; }
153  virtual string Description();
154 
155  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
156  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq);
157  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
158  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
159  virtual Real Delta(PlannerObjectiveBase* priorGoal);
160  virtual bool TerminalTimeInvariant() const;
161  virtual bool DifferentialTimeInvariant() const;
162  virtual bool PathInvariant() const;
163 
164  Real norm;
165  vector<shared_ptr<PlannerObjectiveBase> > components;
166  vector<Real> weights;
167 };
168 
169 
174 {
175  public:
176  CartesianObjective(Robot* robot);
177  virtual ~CartesianObjective() {}
178 
179  virtual const char* TypeString() { return "cartesian"; }
180 
181  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
182  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
183  return TerminalCost(0,path.ramps.back().x1,path.ramps.back().dx1);
184  }
185  virtual Real Delta(PlannerObjectiveBase* priorGoal);
186  virtual bool TerminalTimeInvariant() const { return true; }
187  virtual bool DifferentialTimeInvariant() const { return true; }
188  virtual bool PathInvariant() const { return true; }
189 
190  Robot* robot;
191  IKGoal ikGoal;
192 };
193 
198 {
199  public:
200  IKObjective(Robot* robot);
201  virtual ~IKObjective() {}
202 
203  virtual const char* TypeString() { return "ik"; }
204 
205  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
206 
207  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0) {
208  return TerminalCost(0,path.ramps.back().x1,path.ramps.back().dx1);
209  }
210 
211  virtual Real Delta(PlannerObjectiveBase* priorGoal);
212 
213  virtual bool TerminalTimeInvariant() const { return true; }
214  virtual bool DifferentialTimeInvariant() const { return true; }
215  virtual bool PathInvariant() const { return true; }
216 
217  Robot* robot;
218  IKGoal ikGoal;
219  Real posCoeff,oriCoeff;
220 };
221 
226 {
227  public:
229  virtual ~CartesianTrackingObjective() {}
230  virtual const char* TypeString() { return "cartesian_tracking"; }
231 
232  virtual Real TerminalCost(Real tend,const Vector& qend,const Vector& dqend);
233  virtual Real DifferentialCost(Real t,const Vector& q,const Vector& dq);
234  virtual Real IncrementalCost(Real t,const ParabolicRamp::ParabolicRampND& ramp);
235  virtual Real PathCost(const ParabolicRamp::DynamicPath& path,Real tstart=0);
236  virtual Real Delta(PlannerObjectiveBase* priorGoal);
237 
240  int FindSegment(Real t) const;
241  //integrates the time range [a,b] over the ramp starting at tstart
242  Real IntegrateSegment(int index,Real a,Real b,Real tstart,const ParabolicRamp::ParabolicRampND& ramp);
243  //computes desired 3-space position at time t
244  Vector3 GetDesiredPosition(Real t) const;
245  //differential cost at time t in the form (x-b)^T A (x-b)
246  void GetDifferentialCostFunction(Real t,Matrix3& A,Vector3& b) const;
247  Real DifferentialCost(Real t,const Vector& q,int index);
248 
249  Robot* robot;
250  //track localPosition on this link
251  int link;
252  Vector3 localPosition;
253  //3-space trajectory to follow
254  vector<Vector3> positions;
255  vector<Real> times;
256  //optional: scalar weight for each time
257  vector<Real> weights;
258  //optional: matrix weight e^T A e for each time
259  vector<Matrix3> matWeights;
260  //optional: terminal cost
261  Real endTimeWeight,endPosWeight;
262  Matrix3 endPosMatWeight;
263 };
264 
290 PlannerObjectiveBase* LoadPlannerObjective(istream& in,Robot* robot=NULL);
291 
293 bool SavePlannerObjective(PlannerObjectiveBase* obj,ostream& out);
294 
295 class AnyCollection;
296 
298 PlannerObjectiveBase* LoadPlannerObjective(AnyCollection& msg,Robot* robot=NULL);
299 bool SavePlannerObjective(PlannerObjectiveBase* obj,AnyCollection& msg);
300 
301 #endif
302 
An objective that merges contributions from multiple other objective functions.
Definition: PlannerObjective.h:143
virtual Real TerminalCost(Real tend, const Vector &qend, const Vector &dqend)
Definition: PlannerObjective.h:102
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:152
virtual Real PathCost(const ParabolicRamp::DynamicPath &path, Real tstart=0)
Definition: PlannerObjective.h:105
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:133
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:124
virtual bool PathInvariant() const
Subclasses: planners may exploit path-invariant costs for faster performance.
Definition: PlannerObjective.h:134
virtual bool PathInvariant() const
Subclasses: planners may exploit path-invariant costs for faster performance.
Definition: PlannerObjective.h:37
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:187
The main robot type used in RobotSim.
Definition: Robot.h:79
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:20
virtual Real PathCost(const ParabolicRamp::DynamicPath &path, Real tstart=0)
Definition: PlannerObjective.h:182
Solves for optimal trajectores for a velocity-bounded ND system.
Definition: ParabolicRamp.h:110
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:214
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:84
virtual Real PathCost(const ParabolicRamp::DynamicPath &path, Real tstart=0)
A goal that measures absolute difference in terminal time (i.e., penalize stopping at a different tim...
Definition: PlannerObjective.h:79
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:88
virtual Real TerminalCost(Real tend, const Vector &qend, const Vector &dqend)
Definition: PlannerObjective.h:125
std::vector< ParabolicRampND > ramps
The path is stored as a series of ramps.
Definition: DynamicPath.h:160
virtual Real TerminalCost(Real tend, const Vector &qend, const Vector &dqend)
Definition: PlannerObjective.h:27
virtual Real DifferentialCost(Real t, const Vector &q, const Vector &dq)
Definition: PlannerObjective.h:68
virtual Real DifferentialCost(Real t, const Vector &q, const Vector &dq)
Definition: PlannerObjective.h:31
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:71
A bounded-velocity, bounded-acceleration trajectory consisting of parabolic ramps.
Definition: DynamicPath.h:114
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:34
virtual Real Delta(PlannerObjectiveBase *priorGoal)
Definition: PlannerObjective.h:51
A goal that measures distance to a goal velocity vgoal.
Definition: PlannerObjective.h:119
virtual Real PathCost(const ParabolicRamp::DynamicPath &path, Real tstart=0)
Definition: PlannerObjective.h:128
A goal that measures distance to a goal configuration qgoal.
Definition: PlannerObjective.h:96
virtual Real PathCost(const ParabolicRamp::DynamicPath &path, Real tstart=0)
Definition: PlannerObjective.h:207
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:66
virtual string Description()
Subclasses: return a string for printing (optional)
Definition: PlannerObjective.h:23
virtual bool PathInvariant() const
Subclasses: planners may exploit path-invariant costs for faster performance.
Definition: PlannerObjective.h:215
virtual Real IncrementalCost(Real t, const ParabolicRamp::ParabolicRampND &ramp)
Definition: PlannerObjective.h:42
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:101
A base class for objective functionals in time/config/velocity space.
Definition: PlannerObjective.h:13
An objective that measures path execution time.
Definition: PlannerObjective.h:61
virtual bool Read(File &file)
Subclasses: read and write to binary file (optional, not implemented yet)
Definition: PlannerObjective.h:54
A goal for an IK solution (including possibly rotation)
Definition: PlannerObjective.h:197
A goal that measures point-to-point distance.
Definition: PlannerObjective.h:173
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:179
Functions for optimal acceleration-bounded trajectories.
virtual bool PathInvariant() const
Subclasses: planners may exploit path-invariant costs for faster performance.
Definition: PlannerObjective.h:111
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:203
virtual bool PathInvariant() const
Subclasses: planners may exploit path-invariant costs for faster performance.
Definition: PlannerObjective.h:188
virtual const char * TypeString()
Subclasses: return an identifier for this goal type.
Definition: PlannerObjective.h:230
virtual bool DifferentialTimeInvariant() const
Subclasses: planners may exploit time invariant costs for faster performance.
Definition: PlannerObjective.h:110
Tracking a path in cartesian space.
Definition: PlannerObjective.h:225