QPSolver.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/api.h>
 
#include <mc_control/api.h>
#include <mc_rbdyn/Contact.h>
#include <mc_rbdyn/Robots.h>
 
#include <mc_rtc/pragma.h>
 
#include <memory>
 
namespace mc_tasks
{
 
struct MetaTask;
 
} // namespace mc_tasks
 
namespace mc_rtc
{
 
struct Logger;
 
namespace gui
{
 
struct StateBuilder;
 
} // namespace gui
 
} // namespace mc_rtc
 
namespace mc_control
{
 
struct MCController;
 
} // namespace mc_control
 
namespace mc_solver
{
 
// Work around GCC bug see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=43407
MC_RTC_diagnostic_push
MC_RTC_diagnostic_ignored(GCC, "-Wattributes")
 
 
enum class MC_SOLVER_DLLAPI FeedbackType
{
  None,
  OpenLoop = None,
  Joints,
  JointsWVelocity,
  ObservedRobots,
  ClosedLoop = ObservedRobots,
  ClosedLoopIntegrateReal,
  SkipQP
};
 
MC_RTC_diagnostic_pop
 
struct ConstraintSet;
struct DynamicsConstraint;
struct TasksQPSolver;
 
struct MC_SOLVER_DLLAPI QPSolver
{
public:
  enum class Backend
  {
    Unset,
    Tasks,
    TVM
  };
 
  struct MC_SOLVER_DLLAPI ControllerToken
  {
    friend struct mc_control::MCController;
    friend struct QPSolver;
 
  private:
    ControllerToken() = default;
  };
 
  QPSolver(mc_rbdyn::RobotsPtr robots, double timeStep, Backend backend);
 
  QPSolver(double timeStep, Backend backend);
 
  virtual ~QPSolver() = default;
 
  inline Backend backend() const noexcept { return backend_; }
 
  static Backend context_backend();
 
  static void context_backend(Backend backend);
 
  void addConstraintSet(ConstraintSet & cs);
 
  template<typename T>
  inline void addConstraintSet(const std::unique_ptr<T> & ptr)
  {
    addConstraintSet(*ptr);
  }
 
  void removeConstraintSet(ConstraintSet & cs);
 
  template<typename T>
  inline void removeConstraintSet(const std::unique_ptr<T> & ptr)
  {
    removeConstraintSet(*ptr);
  }
 
  void addTask(mc_tasks::MetaTask * task);
 
  inline void addTask(std::shared_ptr<mc_tasks::MetaTask> task)
  {
    if(task)
    {
      addTask(task.get());
      shPtrTasksStorage.emplace_back(task);
    }
  }
 
  void removeTask(mc_tasks::MetaTask * task);
 
  inline void removeTask(std::shared_ptr<mc_tasks::MetaTask> task)
  {
    if(task) { removeTask(task.get()); }
  }
 
  void setContacts(const std::vector<mc_rbdyn::Contact> & contacts = {});
 
  /* Called by the owning controller to actually set the contacts or internally by QPSolver when it has no owning
   * controller */
  virtual void setContacts(ControllerToken, const std::vector<mc_rbdyn::Contact> & contacts) = 0;
 
  inline const std::vector<mc_rbdyn::Contact> & contacts() const noexcept { return contacts_; }
 
  inline const std::vector<mc_solver::ConstraintSet *> & constraints() const noexcept { return constraints_; }
 
  inline const std::vector<mc_tasks::MetaTask *> & tasks() const noexcept { return metaTasks_; }
 
  virtual const sva::ForceVecd desiredContactForce(const mc_rbdyn::Contact & id) const = 0;
 
  bool run(FeedbackType fType = FeedbackType::None);
 
  const mc_rbdyn::Robot & robot() const;
  mc_rbdyn::Robot & robot();
 
  mc_rbdyn::Robot & robot(unsigned int idx);
  const mc_rbdyn::Robot & robot(unsigned int idx) const;
 
  const mc_rbdyn::Robot & env() const;
  mc_rbdyn::Robot & env();
 
  const mc_rbdyn::Robots & robots() const;
  mc_rbdyn::Robots & robots();
 
  const mc_rbdyn::Robots & realRobots() const;
  mc_rbdyn::Robots & realRobots();
 
  double dt() const;
 
  virtual double solveTime() = 0;
 
  virtual double solveAndBuildTime() = 0;
 
  void logger(std::shared_ptr<mc_rtc::Logger> logger);
  std::shared_ptr<mc_rtc::Logger> logger() const;
 
  void gui(std::shared_ptr<mc_rtc::gui::StateBuilder> gui);
  std::shared_ptr<mc_rtc::gui::StateBuilder> gui() const;
 
  inline void controller(mc_control::MCController * ctl) noexcept { controller_ = ctl; }
  inline const mc_control::MCController * controller() const noexcept { return controller_; }
  inline mc_control::MCController * controller() noexcept { return controller_; }
 
protected:
  Backend backend_;
  mc_rbdyn::RobotsPtr robots_p;
  mc_rbdyn::RobotsPtr realRobots_p;
  double timeStep;
 
  std::vector<mc_rbdyn::Contact> contacts_;
 
  std::vector<mc_tasks::MetaTask *> metaTasks_;
 
  std::vector<std::shared_ptr<void>> shPtrTasksStorage;
 
  std::vector<mc_solver::ConstraintSet *> constraints_;
 
  std::shared_ptr<mc_rtc::Logger> logger_ = nullptr;
 
  std::shared_ptr<mc_rtc::gui::StateBuilder> gui_ = nullptr;
 
  void addTaskToGUI(mc_tasks::MetaTask * task);
 
  mc_control::MCController * controller_ = nullptr;
 
  virtual bool run_impl(FeedbackType fType = FeedbackType::None) = 0;
 
  virtual void addDynamicsConstraint(mc_solver::DynamicsConstraint * dynamics) = 0;
 
  virtual void removeDynamicsConstraint(mc_solver::ConstraintSet * maybe_dynamics) = 0;
};
 
} // namespace mc_solver
 
namespace fmt
{
 
template<>
struct formatter<mc_solver::QPSolver::Backend> : public formatter<string_view>
{
  template<typename FormatContext>
#if FMT_VERSION <= 9 * 10000
  auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx)
#else
  auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out())
#endif
  {
    using Backend = mc_solver::QPSolver::Backend;
    switch(backend)
    {
      case Backend::Tasks:
        return formatter<string_view>::format("Tasks", ctx);
      case Backend::TVM:
        return formatter<string_view>::format("TVM", ctx);
      case Backend::Unset:
        return formatter<string_view>::format("Unset", ctx);
      default:
        return formatter<string_view>::format("UNEXPECTED", ctx);
    }
  }
};
 
} // namespace fmt