ExactCubicTrajectoryTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/SplineTrajectoryTask.h>
#include <mc_trajectory/ExactCubic.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI ExactCubicTrajectoryTask : public SplineTrajectoryTask<ExactCubicTrajectoryTask>
{
  friend struct SplineTrajectoryTask<ExactCubicTrajectoryTask>;
 
  ExactCubicTrajectoryTask(const mc_rbdyn::RobotFrame & frame,
                           double duration,
                           double stiffness,
                           double weight,
                           const sva::PTransformd & target,
                           const std::vector<std::pair<double, Eigen::Vector3d>> & posWp = {},
                           const Eigen::Vector3d & init_vel = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & init_acc = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & end_vel = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & end_acc = Eigen::Vector3d::Zero(),
                           const std::vector<std::pair<double, Eigen::Matrix3d>> & oriWp = {});
 
  ExactCubicTrajectoryTask(const mc_rbdyn::Robots & robots,
                           unsigned int robotIndex,
                           const std::string & surfaceName,
                           double duration,
                           double stiffness,
                           double weight,
                           const sva::PTransformd & target,
                           const std::vector<std::pair<double, Eigen::Vector3d>> & posWp = {},
                           const Eigen::Vector3d & init_vel = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & init_acc = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & end_vel = Eigen::Vector3d::Zero(),
                           const Eigen::Vector3d & end_acc = Eigen::Vector3d::Zero(),
                           const std::vector<std::pair<double, Eigen::Matrix3d>> & oriWp = {});
 
  const mc_trajectory::ExactCubic & spline() const { return bspline; };
  mc_trajectory::ExactCubic & spline() { return bspline; };
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
  void posWaypoints(const std::vector<std::pair<double, Eigen::Vector3d>> & posWp);
 
  void constraints(const Eigen::Vector3d & init_vel,
                   const Eigen::Vector3d & init_acc,
                   const Eigen::Vector3d & end_vel,
                   const Eigen::Vector3d & end_acc);
 
protected:
  void targetPos(const Eigen::Vector3d & target);
 
  const Eigen::Vector3d & targetPos() const;
 
protected:
  mc_trajectory::ExactCubic bspline;
  sva::PTransformd initialPose_;
};
 
} // namespace mc_tasks