MetaTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/api.h>
 
#include <mc_rtc/Configuration.h>
#include <mc_rtc/gui/StateBuilder.h>
#include <mc_rtc/log/Logger.h>
 
#include <mc_solver/QPSolver.h>
#include <mc_solver/api.h>
 
#include <cmath>
 
namespace mc_control
{
struct CompletionCriteria;
}
 
namespace mc_solver
{
 
struct TasksQPSolver;
struct TVMQPSolver;
 
} // namespace mc_solver
 
namespace mc_tasks
{
 
struct MC_SOLVER_DLLAPI MetaTask
{
  friend struct mc_solver::QPSolver;
  friend struct mc_solver::TasksQPSolver;
  friend struct mc_solver::TVMQPSolver;
  friend struct mc_control::CompletionCriteria;
 
  using Backend = mc_solver::QPSolver::Backend;
 
public:
  inline MetaTask() : backend_(mc_solver::QPSolver::context_backend()) {}
 
  virtual ~MetaTask();
 
  const std::string & type() const { return type_; }
 
  virtual inline void name(const std::string & name) { name_ = name; }
 
  inline const std::string & name() const { return name_; }
 
  virtual void reset() = 0;
 
  virtual void dimWeight(const Eigen::VectorXd & dimW) = 0;
 
  virtual Eigen::VectorXd dimWeight() const = 0;
 
  virtual void selectActiveJoints(mc_solver::QPSolver & solver,
                                  const std::vector<std::string> & activeJointsName,
                                  const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {}) = 0;
 
  virtual void selectUnactiveJoints(
      mc_solver::QPSolver & solver,
      const std::vector<std::string> & unactiveJointsName,
      const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {}) = 0;
 
  virtual void resetJointsSelector(mc_solver::QPSolver & solver) = 0;
 
  virtual Eigen::VectorXd eval() const = 0;
 
  virtual Eigen::VectorXd speed() const = 0;
 
  virtual void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config);
 
  inline size_t iterInSolver() const noexcept { return iterInSolver_; }
 
  inline void resetIterInSolver() noexcept { iterInSolver_ = 0; }
 
  inline void incrementIterInSolver() noexcept { iterInSolver_++; }
 
  inline Backend backend() const noexcept { return backend_; }
 
protected:
  virtual void addToSolver(mc_solver::QPSolver & solver) = 0;
 
  static inline void addToSolver(MetaTask & t, mc_solver::QPSolver & solver) { t.addToSolver(solver); }
 
  virtual void removeFromSolver(mc_solver::QPSolver & solver) = 0;
 
  static inline void removeFromSolver(MetaTask & t, mc_solver::QPSolver & solver) { t.removeFromSolver(solver); }
 
  virtual void update(mc_solver::QPSolver & solver) = 0;
 
  static inline void update(MetaTask & t, mc_solver::QPSolver & solver) { t.update(solver); }
 
  virtual void addToLogger(mc_rtc::Logger &) {}
 
  static inline void addToLogger(MetaTask & t, mc_rtc::Logger & logger) { t.addToLogger(logger); }
 
  virtual void removeFromLogger(mc_rtc::Logger &);
 
  static inline void removeFromLogger(MetaTask & t, mc_rtc::Logger & logger) { t.removeFromLogger(logger); }
 
  virtual void addToGUI(mc_rtc::gui::StateBuilder &);
 
  static inline void addToGUI(MetaTask & t, mc_rtc::gui::StateBuilder & gui) { t.addToGUI(gui); }
 
  virtual void removeFromGUI(mc_rtc::gui::StateBuilder &);
 
  static inline void removeFromGUI(MetaTask & t, mc_rtc::gui::StateBuilder & gui) { t.removeFromGUI(gui); }
 
  inline static void ensureHasJoints(const mc_rbdyn::Robot & robot,
                                     const std::vector<std::string> & joints,
                                     const std::string & prefix)
  {
    for(const auto & jName : joints)
    {
      if(!robot.hasJoint(jName))
      {
        mc_rtc::log::error_and_throw("{} No joint named {} in robot {}", prefix, jName, robot.name());
      }
    }
  }
 
  virtual std::function<bool(const mc_tasks::MetaTask & task, std::string &)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const;
 
  Backend backend_;
 
  std::string type_;
  std::string name_;
 
  size_t iterInSolver_ = 0;
};
 
using MetaTaskPtr = std::shared_ptr<MetaTask>;
 
} // namespace mc_tasks