Interpolated.hpp

Go to the documentation of this file.

#ifndef AIKIDO_TRAJECTORY_PIECEWISELINEAR_TRAJECTORY_HPP_
#define AIKIDO_TRAJECTORY_PIECEWISELINEAR_TRAJECTORY_HPP_
 
#include "aikido/common/pointers.hpp"
#include "aikido/statespace/GeodesicInterpolator.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace trajectory {
 
AIKIDO_DECLARE_POINTERS(Interpolated)
 
class Interpolated : public Trajectory
{
public:
  Interpolated(
      statespace::ConstStateSpacePtr _stateSpace,
      statespace::ConstInterpolatorPtr _interpolator);
 
  virtual ~Interpolated();
 
  void addWaypoint(double _t, const statespace::StateSpace::State* _state);
 
  const statespace::StateSpace::State* getWaypoint(std::size_t _index) const;
 
  double getWaypointTime(std::size_t _index) const;
 
  std::size_t getNumWaypoints() const;
 
  // Documentation inherited
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
  statespace::ConstInterpolatorPtr getInterpolator() const;
 
  // Documentation inherited
  std::size_t getNumDerivatives() const override;
 
  // Documentation inherited
  double getStartTime() const override;
 
  // Documentation inherited
  double getEndTime() const override;
 
  // Documentation inherited
  double getDuration() const override;
 
  // Documentation inherited
  void evaluate(
      double _t, statespace::StateSpace::State* _state) const override;
 
  // Documentation inherited
  void evaluateDerivative(
      double _t,
      int _derivative,
      Eigen::VectorXd& _tangentVector) const override;
 
private:
  struct Waypoint
  {
    Waypoint(double _t, statespace::StateSpace::State* _state);
 
    bool operator<(const Waypoint& rhs) const;
 
    bool operator<(double rhs) const;
 
    void free(const statespace::ConstStateSpacePtr& stateSpace);
 
    double t;
    statespace::StateSpace::State* state;
  };
 
  int getWaypointIndexAfterTime(double _t) const;
 
  statespace::ConstStateSpacePtr mStateSpace;
  statespace::ConstInterpolatorPtr mInterpolator;
  std::vector<Waypoint> mWaypoints;
};
 
} // namespace trajectory
} // namespace aikido
 
#endif // ifndef AIKIDO_TRAJECTORY_PIECEWISELINEAR_TRAJECTORY_HPP_