openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
planningutils.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/>.
17 
23 #ifndef OPENRAVE_PLANNINGUTILS_H
24 #define OPENRAVE_PLANNINGUTILS_H
25 
26 #include <openrave/openrave.h>
27 
28 namespace OpenRAVE {
29 
30 namespace planningutils {
31 
36 
38 OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMaxIterations=1000);
39 
49 
62 OPENRAVE_API PlannerStatus SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::string& plannerparameters="");
63 
72 {
73 public:
79  ActiveDOFTrajectorySmoother(RobotBasePtr robot, const std::string& plannername="", const std::string& plannerparameters="");
81  }
82 
87  virtual PlannerStatus PlanPath(TrajectoryBasePtr traj);
88 
89 protected:
93 };
94 
95 typedef boost::shared_ptr<ActiveDOFTrajectorySmoother> ActiveDOFTrajectorySmootherPtr;
96 
107 OPENRAVE_API PlannerStatus SmoothAffineTrajectory(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, const std::string& plannername="", const std::string& plannerparameters="");
108 
119 OPENRAVE_API PlannerStatus SmoothTrajectory(TrajectoryBasePtr traj, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::string& plannerparameters="");
120 
133 OPENRAVE_API PlannerStatus RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBasePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::string& plannerparameters="");
134 
143 {
144 public:
151  ActiveDOFTrajectoryRetimer(RobotBasePtr robot, const std::string& plannername="", const std::string& plannerparameters="");
153  }
154 
159  virtual PlannerStatus PlanPath(TrajectoryBasePtr traj, bool hastimestamps=false);
160 
161 protected:
165 };
166 
167 typedef boost::shared_ptr<ActiveDOFTrajectoryRetimer> ActiveDOFTrajectoryRetimerPtr;
168 
181 OPENRAVE_API PlannerStatus RetimeAffineTrajectory(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::string& plannerparameters="");
182 
190 {
191 public:
200  AffineTrajectoryRetimer(const std::string& plannername="", const std::string& plannerparameters="");
202  }
203 
205  virtual void SetPlanner(const std::string& plannername="", const std::string& plannerparameters="");
206 
211  virtual PlannerStatus PlanPath(TrajectoryBasePtr traj, const std::vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false);
212 
213 protected:
214  std::string _plannername, _extraparameters;
217 };
218 
219 typedef boost::shared_ptr<AffineTrajectoryRetimer> AffineTrajectoryRetimerPtr;
220 
232 OPENRAVE_API PlannerStatus RetimeTrajectory(TrajectoryBasePtr traj, bool hastimestamps=false, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::string& plannerparameters="");
233 
244 OPENRAVE_API void InsertActiveDOFWaypointWithRetiming(int index, const std::vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, TrajectoryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="");
245 
255 OPENRAVE_API void InsertWaypointWithSmoothing(int index, const std::vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, TrajectoryBasePtr traj, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="");
256 
259 
268 
273 
279 OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<TrajectoryBaseConstPtr>& listtrajectories);
280 
292 {
293 public:
301 };
302 
312 OPENRAVE_API void GetDHParameters(std::vector<DHParameter>& vparameters, KinBodyConstPtr pbody);
313 
316 {
317 public:
320  LineCollisionConstraint(const std::list<KinBodyPtr>& listCheckCollisions, bool bCheckEnv=true);
322  }
323 
328  virtual void SetUserCheckFunction(const boost::function<bool() >& usercheckfn, bool bCallAfterCheckCollision=false);
329 
331  virtual bool Check(PlannerBase::PlannerParametersWeakPtr _params, KinBodyPtr body, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, IntervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations) RAVE_DEPRECATED;
332 
334  virtual bool Check(PlannerBase::PlannerParametersWeakPtr _params, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, IntervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations);
335 
337  return _report;
338  }
339 
340 protected:
341  virtual bool _CheckState();
342 
343  std::vector<dReal> _vtempconfig, dQ;
345  std::list<KinBodyPtr> _listCheckSelfCollisions;
347  boost::array< boost::function<bool() >, 2> _usercheckfns;
348 };
349 
352 {
353 public:
355  dReal Eval(const std::vector<dReal>& c0, const std::vector<dReal>& c1);
356 protected:
358  std::vector<dReal> weights2;
359 };
360 
363 {
364 public:
365  SimpleNeighborhoodSampler(SpaceSamplerBasePtr psampler, const boost::function<dReal(const std::vector<dReal>&, const std::vector<dReal>&)>&distmetricfn);
366 
367  bool Sample(std::vector<dReal>& vNewSample, const std::vector<dReal>& vCurSample, dReal fRadius);
368  bool Sample(std::vector<dReal>& samples);
369 protected:
371  boost::function<dReal(const std::vector<dReal>&, const std::vector<dReal>&)> _distmetricfn;
372 };
373 
381 {
382 public:
383  ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const std::list<IkParameterization>&listparameterizations, int nummaxsamples=20, int nummaxtries=10, dReal fsampleprob=1, bool searchfreeparameters=true);
385  }
386 
388  virtual IkReturnPtr Sample();
389  virtual bool Sample(std::vector<dReal>& vgoal);
390 
397  bool SampleAll(std::list<IkReturnPtr>& samples, int maxsamples=0, int maxchecksamples=0);
398 
399  //void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters::CheckPathConstraintFn& checkfn)
400 
401 
402  virtual int GetIkParameterizationIndex(int index);
403  virtual void SetSamplingProb(dReal fsampleprob);
404 
408  virtual void SetJitter(dReal maxdist);
409 
410 protected:
411  struct SampleInfo
412  {
414  int _numleft;
417  };
420  int _nummaxsamples, _nummaxtries;
421  std::list<SampleInfo> _listsamples;
423  dReal _fsampleprob, _fjittermaxdist;
425  std::vector< IkReturnPtr > _vikreturns;
426  std::list<int> _listreturnedsamples;
427  std::vector<dReal> _vfreestart;
430 };
431 
432 typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSamplerPtr;
433 
434 } // planningutils
435 } // OpenRAVE
436 
437 #endif