constraint_aware_spline_smoother: constraint_aware_spline

00001 /*****************************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2009, the Trustees of Indiana University
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of Indiana University nor the
00015  *       names of its contributors may be used to endorse or promote products
00016  *       derived from this software without specific prior written permission.
00017 
00018  * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
00028  * THE POSSIBILITY OF SUCH DAMAGE.
00029  * 
00030  ***************************************************************************/
00031 
00032 #ifndef DYNAMIC_PATH_H
00033 #define DYNAMIC_PATH_H
00034 
00035 #include "constraint_aware_spline_smoother/ParabolicRamp.h"
00036 
00039 class FeasibilityCheckerBase
00040 {
00041  public:
00042   FeasibilityCheckerBase(){};
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 
00065 class DynamicPath
00066 {
00067  public:
00068   DynamicPath();
00069   void Init(const Vector& velMax,const Vector& accMax);
00070   inline void Clear() { ramps.clear(); }
00071   inline bool Empty() const { return ramps.empty(); }
00072   Real GetTotalTime() const;
00073   void Evaluate(Real t,Vector& x);
00074   void Derivative(Real t,Vector& dx);
00075   void SetMilestones(const std::vector<Vector>& x);
00076   void SetMilestones(const std::vector<Vector>& x,const std::vector<Vector>& dx);
00077   void GetMilestones(std::vector<Vector>& x,std::vector<Vector>& dx) const;
00078   void Append(const Vector& x);
00079   void Append(const Vector& x,const Vector& dx);
00080   bool TryShortcut(Real t1,Real t2,FeasibilityCheckerBase*,Real tol);
00081   bool TryShortcut(Real t1,Real t2,FeasibilityCheckerBase*,DistanceCheckerBase*);
00082   int Shortcut(int numIters,FeasibilityCheckerBase*,Real tol);
00083   int ShortCircuit(FeasibilityCheckerBase*,Real tol);
00084   int Shortcut(int numIters,FeasibilityCheckerBase*,DistanceCheckerBase*);
00085   int ShortCircuit(FeasibilityCheckerBase*,DistanceCheckerBase*);
00086 
00088   Vector velMax,accMax;
00090   std::vector<ParabolicRampND> ramps;
00091 };
00092 
00093 
00094 #endif
DynamicPath.h
