TrajectoryTaskGeneric.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/MetaTask.h>
 
#include <mc_tasks/TVMTrajectoryTaskGeneric.h>
 
#include <mc_rbdyn/Robots.h>
 
#include <mc_rtc/logging.h>
#include <mc_rtc/void_ptr.h>
 
#include <Tasks/QPTasks.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI TrajectoryTaskGeneric : public MetaTask
{
  using TrajectoryBase = TrajectoryTaskGeneric;
 
  TrajectoryTaskGeneric(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double stiffness, double weight);
 
  TrajectoryTaskGeneric(const mc_rbdyn::RobotFrame & frame, double stiffness, double weight);
 
  virtual ~TrajectoryTaskGeneric() = default;
 
  void reset() override;
 
  void refVel(const Eigen::VectorXd & vel);
 
  const Eigen::VectorXd & refVel() const;
 
  void refAccel(const Eigen::VectorXd & accel);
 
  const Eigen::VectorXd & refAccel() const;
 
  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);
 
  double stiffness() const;
 
  double damping() const;
 
  const Eigen::VectorXd & dimStiffness() const;
 
  const Eigen::VectorXd & dimDamping() const;
 
  void weight(double w);
 
  double weight() const;
 
  void dimWeight(const Eigen::VectorXd & dimW) override;
 
  Eigen::VectorXd dimWeight() const override;
 
  void selectActiveJoints(const std::vector<std::string> & activeJointsName,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {},
                          bool checkJoints = true);
 
  void selectActiveJoints(mc_solver::QPSolver & solver,
                          const std::vector<std::string> & activeJointsName,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {}) override;
 
  void selectUnactiveJoints(const std::vector<std::string> & unactiveJointsName,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {},
                            bool checkJoints = true);
 
  void selectUnactiveJoints(mc_solver::QPSolver & solver,
                            const std::vector<std::string> & unactiveJointsName,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {}) override;
 
  virtual void resetJointsSelector();
 
  void resetJointsSelector(mc_solver::QPSolver & solver) override;
 
  Eigen::VectorXd eval() const override;
 
  Eigen::VectorXd speed() const override;
 
  const Eigen::VectorXd & normalAcc() const;
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
protected:
  template<Backend backend, typename ErrorT, typename... Args>
  inline void finalize(Args &&... args)
  {
    assert(backend == backend_);
    errorT = mc_rtc::make_void_ptr<ErrorT>(std::forward<Args>(args)...);
    if constexpr(backend == Backend::Tasks)
    {
      trajectoryT_ = mc_rtc::make_void_ptr<tasks::qp::TrajectoryTask>(
          robots.mbs(), rIndex, static_cast<ErrorT *>(errorT.get()), stiffness_(0), damping_(0), weight_);
      auto & trajectory = *static_cast<tasks::qp::TrajectoryTask *>(trajectoryT_.get());
      stiffness_ = trajectory.stiffness();
      damping_ = trajectory.damping();
      if(refVel_.size() != trajectory.refVel().size()) { refVel_ = trajectory.refVel(); }
      if(refAccel_.size() != trajectory.refAccel().size()) { refAccel_ = trajectory.refAccel(); }
    }
    else if constexpr(backend == Backend::TVM)
    {
      auto error = static_cast<ErrorT *>(errorT.get());
      trajectoryT_ = mc_rtc::make_void_ptr<details::TVMTrajectoryTaskGenericPtr>(
          std::make_shared<details::TVMTrajectoryTaskGeneric>());
      static_cast<details::TVMTrajectoryTaskGenericPtr *>(trajectoryT_.get())->get()->init<ErrorT>(error);
      int size = error->size();
      stiffness_ = Eigen::VectorXd::Constant(size, 1, stiffness_(0));
      damping_ = Eigen::VectorXd::Constant(size, 1, damping_(0));
      if constexpr(details::has_refVel_v<ErrorT>) { refVel_ = error->refVel(); }
      if constexpr(details::has_refAccel_v<ErrorT>) { refAccel_ = error->refAccel(); }
    }
    else { mc_rtc::log::error_and_throw("[{} task] Not implemented for backend: {}", backend_); }
  }
 
  void addToGUI(mc_rtc::gui::StateBuilder &) override;
 
  void addToLogger(mc_rtc::Logger & logger) override;
 
  std::function<bool(const mc_tasks::MetaTask & task, std::string &)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const override;
 
  const mc_rbdyn::Robots & robots;
  unsigned int rIndex;
  mc_rtc::void_ptr errorT{nullptr, nullptr};
  Eigen::VectorXd refVel_;
  Eigen::VectorXd refAccel_;
  bool inSolver_ = false;
  mc_rtc::void_ptr trajectoryT_{nullptr, nullptr};
 
  void addToSolver(mc_solver::QPSolver & solver) override;
 
  Eigen::VectorXd stiffness_;
  Eigen::VectorXd damping_;
  double weight_;
  mc_rtc::void_ptr selectorT_{nullptr, nullptr};
 
  void removeFromSolver(mc_solver::QPSolver & solver) override;
 
  void update(mc_solver::QPSolver &) override;
};
 
} // namespace mc_tasks