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 PARABOLIC_RAMP_H
00033 #define PARABOLIC_RAMP_H
00034 
00035 #include <math.h>
00036 #include <assert.h>
00037 #include "constraint_aware_spline_smoother/Math.h"
00038 
00050 class ParabolicRamp1D
00051 {
00052  public:
00054   void SetConstant(Real x);
00056   bool SolveMinTime(Real amax,Real vmax);
00058   bool SolveMinAccel(Real endTime,Real vmax);
00060   Real Evaluate(Real t) const;
00062   Real Derivative(Real t) const;
00064   Real Accel(Real t) const;
00066   Real EndTime() const { return ttotal; }
00068   void Dilate(Real timeScale);
00070   void TrimFront(Real tcut);
00072   void TrimBack(Real tcut);
00074   bool IsValid() const;
00075 
00077   Real x0,dx0;
00078   Real x1,dx1;
00079 
00081   Real tswitch1,tswitch2;  //time to switch between ramp/flat/ramp
00082   Real ttotal;
00083   Real a1,v,a2;   // accel of first ramp, velocity of linear section, accel of second ramp
00084 };
00085 
00090 class ParabolicRampND
00091 {
00092  public:
00093   void SetConstant(const Vector& x);
00094   bool SolveMinTimeLinear(const Vector& amax,const Vector& vmax);
00095   bool SolveMinTime(const Vector& amax,const Vector& vmax);
00096   bool SolveMinAccel(const Vector& vmax,Real time);
00097   void Evaluate(Real t,Vector& x) const;
00098   void Derivative(Real t,Vector& x) const;
00099   void Output(Real dt,std::vector<Vector>& path) const;
00100   void Dilate(Real timeScale);
00101   void TrimFront(Real tcut);
00102   void TrimBack(Real tcut);
00103   bool IsValid() const;
00104 
00106   Vector x0,dx0;
00107   Vector x1,dx1;
00108 
00110   Real endTime;
00111   std::vector<ParabolicRamp1D> ramps;
00112 };
00113 
00114 #endif
ParabolicRamp.h
