SplineTrajectoryTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/TrajectoryTaskGeneric.h>
 
#include <mc_trajectory/InterpolatedRotation.h>
#include <mc_trajectory/SequenceInterpolator.h>
 
namespace mc_tasks
{
 
template<typename Derived>
struct SplineTrajectoryTask : public TrajectoryTaskGeneric
{
  using SplineTrajectoryBase = SplineTrajectoryTask<Derived>;
  using TrajectoryTask = TrajectoryTaskGeneric;
  using SequenceInterpolator6d =
      mc_trajectory::SequenceInterpolator<Eigen::Vector6d, mc_trajectory::LinearInterpolation<Eigen::Vector6d>>;
 
  static std::vector<std::pair<double, Eigen::Matrix3d>> loadOriWaypoints(const mc_rtc::Configuration & c);
 
  SplineTrajectoryTask(const mc_rbdyn::RobotFrame & frame,
                       double duration,
                       double stiffness,
                       double weight,
                       const Eigen::Matrix3d & target,
                       const std::vector<std::pair<double, Eigen::Matrix3d>> & oriWp = {});
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
  std::function<bool(const mc_tasks::MetaTask &, std::string &)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const override;
 
  void oriWaypoints(const std::vector<std::pair<double, Eigen::Matrix3d>> & oriWp);
 
  // Ensures that accessors from the base class are available
  using TrajectoryBase::damping;
  using TrajectoryBase::dimWeight;
  using TrajectoryBase::stiffness;
 
  void dimWeight(const Eigen::VectorXd & dimW) override;
 
  Eigen::VectorXd dimWeight() const override;
 
  void stiffness(double stiffness);
 
  void stiffness(const Eigen::VectorXd & stiffness);
 
  void damping(double damping);
 
  void damping(const Eigen::VectorXd & damping);
 
  void setGains(double stiffness, double damping);
 
  void setGains(const Eigen::VectorXd & stiffness, const Eigen::VectorXd & damping);
 
  void dimWeightInterpolation(const std::vector<std::pair<double, Eigen::Vector6d>> & dimWeights);
  inline const std::vector<std::pair<double, Eigen::Vector6d>> & dimWeightInterpolation() const noexcept
  {
    return dimWeightInterpolator_.values();
  }
 
  void stiffnessInterpolation(const std::vector<std::pair<double, Eigen::Vector6d>> & stiffnessGains);
  inline const std::vector<std::pair<double, Eigen::Vector6d>> & stiffnessInterpolation() const noexcept
  {
    return stiffnessInterpolator_.values();
  }
 
  void dampingInterpolation(const std::vector<std::pair<double, Eigen::Vector6d>> & dampingGains);
  inline const std::vector<std::pair<double, Eigen::Vector6d>> & dampingInterpolation() const noexcept
  {
    return dampingInterpolator_.values();
  }
 
  bool timeElapsed() const;
 
  Eigen::VectorXd eval() const override;
 
  virtual Eigen::VectorXd evalTracking() const;
 
  void target(const sva::PTransformd & target);
  const sva::PTransformd target() const;
 
  void displaySamples(unsigned s);
  unsigned displaySamples() const;
 
  inline void pause(bool paused) { paused_ = paused; }
 
  inline bool pause() const noexcept { return paused_; }
 
  inline double currentTime() const noexcept { return currTime_; }
 
  inline double duration() const noexcept { return duration_; }
 
protected:
  void refPose(const sva::PTransformd & pose);
  const sva::PTransformd & refPose() const;
 
  /* Hide parent's refVel and refAccel implementation as the Spline tasks
   * are overriding these at every iteration according to the underlying curve */
  using TrajectoryTaskGeneric::refAccel;
  using TrajectoryTaskGeneric::refVel;
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
  void addToLogger(mc_rtc::Logger & logger) override;
 
  void update(mc_solver::QPSolver &) override;
 
  void interpolateGains();
 
protected:
  mc_rbdyn::ConstRobotFramePtr frame_;
  double duration_ = 0;
  mc_trajectory::InterpolatedRotation oriSpline_;
  // Linear interpolation for gains
  SequenceInterpolator6d dimWeightInterpolator_;
  SequenceInterpolator6d stiffnessInterpolator_;
  SequenceInterpolator6d dampingInterpolator_;
 
  bool paused_ = false;
 
  double currTime_ = 0.;
  unsigned samples_ = 20;
  bool inSolver_ = false;
};
} // namespace mc_tasks
 
#include <mc_tasks/SplineTrajectoryTask.hpp>