PathController.h

#ifndef PATH_CONTROLLER_H
#define PATH_CONTROLLER_H

#include "JointTrackingController.h"
#include <Klampt/Modeling/DynamicPath.h>
#include <KrisLibrary/spline/PiecewisePolynomial.h>
#include <list>


class PolynomialMotionQueue
{
public:
  PolynomialMotionQueue();
  virtual ~PolynomialMotionQueue() {}
  void SetLimits(const Robot& robot);
  void SetConstant(const Config& x);
  void SetPath(const Spline::PiecewisePolynomialND& path);
  void SetPath(const ParabolicRamp::DynamicPath& path);
  void SetPiecewiseLinear(const vector<Config>& milestones,const vector<Real>& times);
  void SetPiecewiseCubic(const vector<Config>& milestones,const vector<Vector>& velocities,const vector<Real>& times);
  void SetPiecewiseLinearRamp(const vector<Config>& milestones);
  void Advance(Real dt);
  void Append(const Spline::PiecewisePolynomialND& path);
  void Append(const ParabolicRamp::DynamicPath& path);
  void AppendLinear(const Config& config,Real dt);
  void AppendCubic(const Config& x,const Config& v,Real dt);
  void AppendRamp(const Config& x);
  void AppendLinearRamp(const Config& x);
  void AppendRamp(const Config& x,const Vector& v);
  void GetPath(Spline::PiecewisePolynomialND& path) const;
  void Cut(Real time,bool relative=true);
  Real CurTime() const;
  Config CurConfig() const;
  Config CurVelocity() const;
  Config Endpoint() const;
  Vector EndpointVelocity() const;
  void Eval(Real time,Config& x,bool relative=true) const;
  void Deriv(Real time,Config& dx,bool relative=true) const;
  bool Done() const;
  Real TimeRemaining() const;

  Real pathOffset;
  Spline::PiecewisePolynomialND path;

  Vector qMin,qMax,velMax,accMax;
};

class PolynomialPathController : public JointTrackingController, public PolynomialMotionQueue
{
 public:
  PolynomialPathController(Robot& robot);
  virtual ~PolynomialPathController() {}

  virtual const char* Type() const { return "PolynomialPathController"; }
  virtual void GetDesiredState(Config& q_des,Vector& dq_des);
  virtual void Update(Real dt);  
  virtual void Reset();
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;

  //commands
  virtual vector<string> Commands() const;
  virtual bool SendCommand(const string& name,const string& str);
};

#endif