BSpline.hpp

Go to the documentation of this file.

#ifndef AIKIDO_TRAJECTORY_BSPLIINE_HPP_
#define AIKIDO_TRAJECTORY_BSPLIINE_HPP_
 
#include "aikido/common/BSpline.hpp"
#include "aikido/common/pointers.hpp"
#include "aikido/distance/DistanceMetric.hpp"
#include "aikido/statespace/StateSpace.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace trajectory {
 
AIKIDO_DECLARE_POINTERS(BSpline)
 
class BSpline : public Trajectory
{
public:
  using SplineType = common::BSpline<double, 1, Eigen::Dynamic>;
  using KnotVectorType = typename SplineType::KnotVectorType;
  using ControlPointVectorType = typename SplineType::ControlPointVectorType;
 
  BSpline(
      statespace::ConstStateSpacePtr stateSpace,
      const KnotVectorType& knots,
      const ControlPointVectorType& controlPoints);
 
  BSpline(
      statespace::ConstStateSpacePtr stateSpace,
      std::size_t degree,
      const ControlPointVectorType& controlPoints,
      double startTime = 0.0,
      double endTime = 1.0);
 
  BSpline(
      statespace::ConstStateSpacePtr stateSpace,
      std::size_t degree,
      std::size_t numControlPoints,
      double startTime = 0.0,
      double endTime = 1.0);
 
  BSpline(const BSpline& other);
 
  BSpline(BSpline&& other);
 
  ~BSpline() override;
 
  BSpline& operator=(const BSpline& other);
 
  BSpline& operator=(BSpline&& other);
 
  std::unique_ptr<trajectory::Trajectory> clone() const;
 
  std::size_t getDegree() const;
 
  std::size_t getOrder() const;
 
  std::size_t getNumKnots() const;
 
  std::size_t getNumControlPoints() const;
 
  void setStartPoint(std::size_t stateSpaceIndex, double value);
 
  void setStartPoint(const Eigen::VectorXd& point);
 
  void setStartPoint(const statespace::StateSpace::State* state);
 
  void setEndPoint(std::size_t stateSpaceIndex, double value);
 
  void setEndPoint(const Eigen::VectorXd& point);
 
  void setEndPoint(const statespace::StateSpace::State* state);
 
  void setControlPoints(
      std::size_t stateSpaceIndex,
      const ControlPointVectorType& controlPoints,
      bool withStartControlPoint = true,
      bool withEndControlPoint = true);
 
  void setControlPoints(
      std::size_t stateSpaceIndex,
      double value,
      bool withStartControlPoint = true,
      bool withEndControlPoint = true);
 
  const ControlPointVectorType& getControlPoints(
      std::size_t stateSpaceIndex) const;
 
  ControlPointVectorType getControlPoints(
      std::size_t stateSpaceIndex,
      bool withStartControlPoint,
      bool withEndControlPoint) 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;
 
  virtual double computeArcLength(
      const distance::DistanceMetric& distanceMetric,
      double resolution = 0.1) const;
  // TODO(JS): Consider adding this function to the base class as a pure virtual
  // function.
 
protected:
  static std::size_t computeNumKnots(
      std::size_t degree, std::size_t numControlPoints);
 
  static KnotVectorType computeUniformKnots(
      std::size_t degree,
      std::size_t numControlPoints,
      double startTime = 0.0,
      double endTime = 1.0);
 
  statespace::ConstStateSpacePtr mStateSpace;
 
  std::vector<SplineType> mSplines;
 
  double mStartTime;
  // This variable can be redundant because the knots in Eigen::Spline encodes
  // the start time. This variable is for the case of zero dimensional state
  // space since m1DSpline will be empty in that case.
 
  double mEndTime;
  // This variable can be redundant because the knots in Eigen::Spline encodes
  // the end time. This variable is for the case of zero dimensional state space
  // since m1DSpline will be empty in that case.
};
 
} // namespace trajectory
} // namespace aikido
 
#endif // AIKIDO_TRAJECTORY_BSPLIINE_HPP_