ParabolicRamp.h

Go to the documentation of this file.

/*****************************************************************************
 *
 * Copyright (c) 2010-2011, the Trustees of Indiana University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Indiana University nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.

 * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 * 
 ***************************************************************************/

#ifndef PARABOLIC_RAMP_H
#define PARABOLIC_RAMP_H

#include <math.h>
#include "constraint_aware_spline_smoother/ParabolicPathSmooth/Math.h"

namespace ParabolicRamp {

class ParabolicRamp1D
{
 public:
  void SetConstant(Real x,Real t=0);
  void SetLinear(Real x0,Real x1,Real t);
  bool SolveMinTime(Real amax,Real vmax);
  bool SolveMinTime2(Real amax,Real vmax,Real tLowerBound);
  bool SolveMinAccel(Real endTime,Real vmax);
  Real SolveMinAccel2(Real endTime,Real vmax);
  void SolveBraking(Real amax);
  Real Evaluate(Real t) const;
  Real Derivative(Real t) const;
  Real Accel(Real t) const;
  Real EndTime() const { return ttotal; }
  void Dilate(Real timeScale);
  void TrimFront(Real tcut);
  void TrimBack(Real tcut);
  void Bounds(Real& xmin,Real& xmax) const;
  void Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const;
  void DerivBounds(Real& vmin,Real& vmax) const;
  void DerivBounds(Real ta,Real tb,Real& vmin,Real& vmax) const;
  bool IsValid() const;

  Real x0,dx0;
  Real x1,dx1;

  Real tswitch1,tswitch2;  //time to switch between ramp/flat/ramp
  Real ttotal;
  Real a1,v,a2;   // accel of first ramp, velocity of linear section, accel of second ramp
};

class ParabolicRampND
{
 public:
  void SetConstant(const Vector& x,Real t=0);
  void SetLinear(const Vector& x0,const Vector& x1,Real t);
  bool SolveMinTimeLinear(const Vector& amax,const Vector& vmax);
  bool SolveMinTime(const Vector& amax,const Vector& vmax);
  bool SolveMinAccel(const Vector& vmax,Real time);
  bool SolveMinAccelLinear(const Vector& vmax,Real time);
  void SolveBraking(const Vector& amax);
  void Evaluate(Real t,Vector& x) const;
  void Derivative(Real t,Vector& dx) const;
  void Accel(Real t,Vector& ddx) const;
  void Output(Real dt,std::vector<Vector>& path) const;
  void Dilate(Real timeScale);
  void TrimFront(Real tcut);
  void TrimBack(Real tcut);
  void Bounds(Vector& xmin,Vector& xmax) const;
  void Bounds(Real ta,Real tb,Vector& xmin,Vector& xmax) const;
  void DerivBounds(Vector& vmin,Vector& vmax) const;
  void DerivBounds(Real ta,Real tb,Vector& vmin,Vector& vmax) const;
  bool IsValid() const;

  Vector x0,dx0;
  Vector x1,dx1;

  Real endTime;
  std::vector<ParabolicRamp1D> ramps;
};

bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1,
                         Real amax,Real vmax,Real xmin,Real xmax,
                         ParabolicRamp1D& ramp);

bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1,
                          Real endTime,Real vmax,Real xmin,Real xmax,
                          std::vector<ParabolicRamp1D>& ramps);

Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
                         const Vector& amax,const Vector& vmax,const Vector& xmin,const Vector& xmax,
                         std::vector<std::vector<ParabolicRamp1D> >& ramps);

bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
                         Real endTime,const Vector& vmax,const Vector& xmin,const Vector& xmax,
                         std::vector<std::vector<ParabolicRamp1D> >& ramps);

void CombineRamps(const std::vector<std::vector<ParabolicRamp1D> >& ramps,std::vector<ParabolicRampND>& ndramps);

} //namespace ParabolicRamp

#endif