ExactCubic.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rtc/gui/StateBuilder.h>
#include <mc_trajectory/Spline.h>
#include <mc_trajectory/api.h>
 
#include <memory>
#include <ndcurves/exact_cubic.h>
#include <vector>
 
namespace mc_trajectory
{
 
struct MC_TRAJECTORY_DLLAPI ExactCubic : public Spline<Eigen::Vector3d, std::vector<std::pair<double, Eigen::Vector3d>>>
{
public:
  using point_t = Eigen::Vector3d;
  using waypoint_t = std::pair<double, point_t>;
  using exact_cubic_t = ndcurves::exact_cubic<double, double, false, point_t>;
  using spline_constraints_t = ndcurves::curve_constraints<point_t>;
 
public:
  /* \brief Construct a curve passing through the specified optional waypoints
   * with optional initial/final constraints.
   * There should be at least two waypoints (initial and final position).
   *
   * \param duration Duration of the curve
   * \param start Starting position at time t=0
   * \param target Target position at time t=duration
   * \param init_vel Initial velocity
   * \param init_acc Initial acceleration
   * \param end_vel Final velocity
   * \param end_acc Final acceleration
   */
  ExactCubic(double duration,
             const point_t & start,
             const point_t & target,
             const std::vector<waypoint_t> & waypoints = {},
             const point_t & init_vel = {},
             const point_t & init_acc = {},
             const point_t & end_vel = {},
             const point_t & end_acc = {});
 
  void update() override;
 
  void waypoint(size_t idx, const point_t & waypoint);
 
  void waypoint(size_t idx, const double t);
 
  const waypoint_t & waypoint(size_t idx) const;
  double waypointTime(size_t idx) const;
 
  void constraints(const point_t & init_vel,
                   const point_t & init_acc,
                   const point_t & end_vel,
                   const point_t & end_acc);
 
  const point_t & init_vel() const;
  const point_t & init_acc() const;
  const point_t & end_vel() const;
  const point_t & end_acc() const;
 
  std::vector<Eigen::Vector3d> splev(double t, unsigned int der = 0);
 
  std::vector<Eigen::Vector3d> sampleTrajectory();
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector<std::string> & category);
 
private:
  spline_constraints_t constraints_;
  std::unique_ptr<exact_cubic_t> spline_ = nullptr;
};
 
} // namespace mc_trajectory