DynamicPath.h
Go to the documentation of this file.
00001 /*****************************************************************************
00002  *
00003  * Copyright (c) 2010-2011, the Trustees of Indiana University
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Indiana University nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016 
00017  * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
00027  * THE POSSIBILITY OF SUCH DAMAGE.
00028  * 
00029  ***************************************************************************/
00030 
00031 #ifndef DYNAMIC_PATH_H
00032 #define DYNAMIC_PATH_H
00033 
00034 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/ParabolicRamp.h"
00035 
00036 namespace ParabolicRamp {
00037 
00040 class FeasibilityCheckerBase
00041 {
00042  public:
00043   virtual ~FeasibilityCheckerBase() {}
00044   virtual bool ConfigFeasible(const Vector& x)=0;
00045   virtual bool SegmentFeasible(const Vector& a,const Vector& b)=0;
00046 };
00047 
00054 class DistanceCheckerBase
00055 {
00056  public:
00057   virtual ~DistanceCheckerBase() {}
00058   virtual Real ObstacleDistanceNorm() const { return Inf; }
00059   virtual Real ObstacleDistance(const Vector& x)=0;
00060 };
00061 
00063 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters);
00064 
00067 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol);
00068 
00069 
00081 class RampFeasibilityChecker
00082 {
00083  public:
00084   RampFeasibilityChecker(FeasibilityCheckerBase* feas,Real tol);
00085   RampFeasibilityChecker(FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters);
00086   bool Check(const ParabolicRampND& x);
00087 
00088   FeasibilityCheckerBase* feas;
00089   Real tol;
00090   DistanceCheckerBase* distance;
00091   int maxiters;
00092 };
00093 
00097 class RandomNumberGeneratorBase
00098 {
00099  public:
00100   virtual Real Rand() { return ::ParabolicRamp::Rand(); }
00101 };
00102 
00103 
00113 class DynamicPath
00114 {
00115  public:
00116   DynamicPath();
00117   void Init(const Vector& velMax,const Vector& accMax);
00118   void SetJointLimits(const Vector& qMin,const Vector& qMax);
00119   inline void Clear() { ramps.clear(); }
00120   inline bool Empty() const { return ramps.empty(); }
00121   Real GetTotalTime() const;
00122   int GetSegment(Real t,Real& u) const;
00123   void Evaluate(Real t,Vector& x) const;
00124   void Derivative(Real t,Vector& dx) const;
00125   void SetMilestones(const std::vector<Vector>& x);
00126   void SetMilestones(const std::vector<Vector>& x,const std::vector<Vector>& dx);
00127   void GetMilestones(std::vector<Vector>& x,std::vector<Vector>& dx) const;
00128   void Append(const Vector& x);
00129   void Append(const Vector& x,const Vector& dx);
00130   void Concat(const DynamicPath& suffix);
00131   void Split(Real t,DynamicPath& before,DynamicPath& after) const;
00132   bool TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check);
00133   int Shortcut(int numIters,RampFeasibilityChecker& check);
00134   int Shortcut(int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
00135   int ShortCircuit(RampFeasibilityChecker& check);
00138   int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check);
00139   int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
00140 
00141   bool IsValid() const;
00142 
00144   Vector xMin,xMax,velMax,accMax;
00146   std::vector<ParabolicRampND> ramps;
00147 };
00148 
00149 } //namespace ParabolicRamp
00150 
00151 #endif


constraint_aware_spline_smoother
Author(s): Sachin Chitta
autogenerated on Thu Dec 12 2013 11:10:27