openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
planner.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
22 #ifndef OPENRAVE_PLANNER_H
23 #define OPENRAVE_PLANNER_H
24 
25 namespace OpenRAVE {
26 
29 {
30  PS_Failed = 0,
34 };
35 
38 {
39  PA_None=0,
42 };
43 
48 {
49 public:
50  typedef std::list< std::vector<dReal> > ConfigurationList;
51  typedef boost::shared_ptr< PlannerBase::ConfigurationList > ConfigurationListPtr;
52 
64  {
65 public:
67  virtual ~PlannerParameters() {
68  }
69 
72  {
73 public:
74  StateSaver(boost::shared_ptr<PlannerParameters> params);
75  virtual ~StateSaver();
76  inline boost::shared_ptr<PlannerParameters> GetParameters() const {
77  return _params;
78  }
79  virtual void Restore();
80 protected:
81  boost::shared_ptr<PlannerParameters> _params;
82  std::vector<dReal> _values;
83 private:
84  virtual void _Restore();
85  };
86 
87  typedef boost::shared_ptr<StateSaver> StateSaverPtr;
88 
94  virtual PlannerParameters& operator=(const PlannerParameters& r);
95  virtual void copy(boost::shared_ptr<PlannerParameters const> r);
96 
98  virtual void SetRobotActiveJoints(RobotBasePtr robot);
99 
126  virtual void SetConfigurationSpecification(EnvironmentBasePtr env, const ConfigurationSpecification& spec);
127 
133  virtual void Validate() const;
134 
137 
142  typedef boost::function<dReal(const std::vector<dReal>&)> CostFn;
144 
152  typedef boost::function<dReal(const std::vector<dReal>&)> GoalFn;
154 
160  typedef boost::function<dReal(const std::vector<dReal>&, const std::vector<dReal>&)> DistMetricFn;
162 
179  typedef boost::function<bool (const std::vector<dReal>&, const std::vector<dReal>&, IntervalType, PlannerBase::ConfigurationListPtr)> CheckPathConstraintFn;
181 
187  typedef boost::function<bool (std::vector<dReal>&)> SampleFn;
189 
197  typedef boost::function<bool (std::vector<dReal>&)> SampleGoalFn;
199 
207  typedef boost::function<bool (std::vector<dReal>&)> SampleInitialFn;
209 
219  typedef boost::function<bool (std::vector<dReal>&, const std::vector<dReal>&, dReal)> SampleNeighFn;
221 
223  typedef boost::function<void (const std::vector<dReal>&)> SetStateFn;
226  typedef boost::function<void (std::vector<dReal>&)> GetStateFn;
228 
236  typedef boost::function<void (std::vector<dReal>&,const std::vector<dReal>&)> DiffStateFn;
238 
249  typedef boost::function<bool (std::vector<dReal>&,const std::vector<dReal>&, int)> NeighStateFn;
251 
255  std::vector<dReal> vinitialconfig, vgoalconfig;
256 
258  std::vector<dReal> _vConfigLowerLimit, _vConfigUpperLimit;
259 
261  std::vector<dReal> _vConfigVelocityLimit;
262 
264  std::vector<dReal> _vConfigAccelerationLimit;
265 
267  std::vector<dReal> _vConfigResolution;
268 
276 
279 
284 
289 
291  std::string _sExtraParameters;
292 
294  virtual int GetDOF() const {
295  return _configurationspecification.GetDOF();
296  }
297 
298 protected:
299  inline boost::shared_ptr<PlannerBase::PlannerParameters> shared_parameters() {
300  return boost::static_pointer_cast<PlannerBase::PlannerParameters>(shared_from_this());
301  }
302  inline boost::shared_ptr<PlannerBase::PlannerParameters const > shared_parameters_const() const {
303  return boost::static_pointer_cast<PlannerBase::PlannerParameters const>(shared_from_this());
304  }
305 
310  virtual bool serialize(std::ostream& O, int options=0) const;
311 
313  virtual ProcessElement startElement(const std::string& name, const AttributesList& atts);
314  virtual bool endElement(const std::string& name);
315  virtual void characters(const std::string& ch);
316  std::stringstream _ss;
317  boost::shared_ptr<std::stringstream> _sslocal;
319  std::vector<std::string> _vXMLParameters;
321 
322 private:
325  BaseXMLReaderPtr __pcurreader;
326  std::string __processingtag;
327  int _plannerparametersdepth;
328 
330  friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerParameters& v);
332  friend OPENRAVE_API std::istream& operator>>(std::istream& I, PlannerParameters& v);
333  };
334  typedef boost::shared_ptr<PlannerBase::PlannerParameters> PlannerParametersPtr;
335  typedef boost::shared_ptr<PlannerBase::PlannerParameters const> PlannerParametersConstPtr;
336  typedef boost::weak_ptr<PlannerBase::PlannerParameters> PlannerParametersWeakPtr;
337 
340  {
341 public:
342  PlannerProgress();
344  };
345 
347  virtual ~PlannerBase() {
348  }
349 
351  static inline InterfaceType GetInterfaceTypeStatic() {
352  return PT_Planner;
353  }
354 
360  virtual bool InitPlan(RobotBasePtr robot, PlannerParametersConstPtr params) = 0;
361 
369  virtual bool InitPlan(RobotBasePtr robot, std::istream& isParameters);
370 
377  virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj) = 0;
378 
380  virtual PlannerStatus PlanPath(TrajectoryBasePtr ptraj, boost::shared_ptr<std::ostream> pOutStream) RAVE_DEPRECATED {
381  if( !!pOutStream ) {
382  RAVELOG_WARN("planner does not support pOutputStream anymore, please find another method to return information like using SendCommand or writing the data into the returned trajectory\n");
383  }
384  return PlanPath(ptraj);
385  }
386 
388  virtual PlannerParametersConstPtr GetParameters() const = 0;
389 
394  typedef boost::function<PlannerAction(const PlannerProgress&)> PlanCallbackFn;
395 
400  virtual UserDataPtr RegisterPlanCallback(const PlanCallbackFn& callbackfn);
401 
402 protected:
404  return boost::static_pointer_cast<PlannerBase>(shared_from_this());
405  }
407  return boost::static_pointer_cast<PlannerBase const>(shared_from_this());
408  }
409 
418  virtual PlannerStatus _ProcessPostPlanners(RobotBasePtr probot, TrajectoryBasePtr ptraj);
419 
421  return !!(_ProcessPostPlanners(probot,ptraj) & PS_HasSolution);
422  }
423 
427  virtual PlannerAction _CallCallbacks(const PlannerProgress& progress);
428 
429 private:
430  virtual const char* GetHash() const {
431  return OPENRAVE_PLANNER_HASH;
432  }
433 
434  std::list<UserDataWeakPtr> __listRegisteredCallbacks;
435 
437 };
438 
439 OPENRAVE_API std::ostream& operator<<(std::ostream& O, const PlannerBase::PlannerParameters& v);
440 OPENRAVE_API std::istream& operator>>(std::istream& I, PlannerBase::PlannerParameters& v);
441 
442 } // end namespace OpenRAVE
443 
444 #endif