Spline.hpp

Go to the documentation of this file.

#ifndef AIKIDO_TRAJECTORY_SPLINETRAJECTORY2_HPP_
#define AIKIDO_TRAJECTORY_SPLINETRAJECTORY2_HPP_
 
#include "aikido/common/pointers.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace trajectory {
 
AIKIDO_DECLARE_POINTERS(Spline)
 
class Spline : public Trajectory
{
public:
  Spline(statespace::ConstStateSpacePtr _stateSpace, double _startTime = 0.);
 
  virtual ~Spline();
 
  void addSegment(
      const Eigen::MatrixXd& _coefficients,
      double _duration,
      const statespace::StateSpace::State* _startState);
 
  void addSegment(const Eigen::MatrixXd& _coefficients, double _duration);
 
  std::size_t getNumSegments() const;
 
  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
  // 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;
 
  std::size_t getNumWaypoints() const;
 
  void getWaypoint(
      std::size_t _index, statespace::StateSpace::State* state) const;
 
  double getWaypointTime(std::size_t _index) const;
 
  void getWaypointDerivative(
      std::size_t _index,
      int _derivative,
      Eigen::VectorXd& _tangentVector) const;
 
  double getSegmentDuration(std::size_t _index) const;
 
  const Eigen::MatrixXd& getSegmentCoefficients(std::size_t _index) const;
 
  const aikido::statespace::StateSpace::State* getSegmentStartState(
      std::size_t _index) const;
 
private:
  struct PolynomialSegment
  {
    statespace::StateSpace::State* mStartState;
    Eigen::MatrixXd mCoefficients;
    double mDuration;
  };
 
  static Eigen::VectorXd evaluatePolynomial(
      const Eigen::MatrixXd& _coefficients, double _t, int _derivative);
 
  std::pair<std::size_t, double> getSegmentForTime(double _t) const;
 
  statespace::ConstStateSpacePtr mStateSpace;
  double mStartTime;
  std::vector<PolynomialSegment> mSegments;
};
 
} // namespace trajectory
} // namespace aikido
 
#endif // ifndef AIKIDO_TRAJECTORY_SPLINETRAJECTORY2_HPP_