Controller.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_control/MCController.h>
#include <mc_control/fsm/Executor.h>
#include <mc_tasks/EndEffectorTask.h>
#include <mc_tasks/PostureTask.h>
 
#include <functional>
 
namespace mc_control
{
 
namespace fsm
{
 
struct Controller;
 
using Contact = mc_control::Contact;
 
} // namespace fsm
 
} // namespace mc_control
 
namespace mc_control
{
 
namespace fsm
{
 
using ContactSet = mc_control::ContactSet;
 
struct MC_CONTROL_FSM_DLLAPI Controller : public MCController
{
  friend struct Executor;
 
  Controller(mc_rbdyn::RobotModulePtr rm,
             double dt,
             const mc_rtc::Configuration & config,
             ControllerParameters params = {});
 
  ~Controller() override;
 
  bool run() override;
 
  bool run(mc_solver::FeedbackType fType);
 
  void reset(const ControllerResetData & data) override;
 
  virtual void interrupt() { executor_.interrupt(); }
 
  bool running() { return executor_.running(); }
 
  bool resume(const std::string & state);
 
  std::shared_ptr<mc_tasks::PostureTask> getPostureTask(const std::string & robot);
 
  mc_solver::ContactConstraint & contactConstraint() { return *contact_constraint_; }
 
#ifndef MC_RTC_BUILD_STATIC
  StateFactory & factory() { return factory_; }
#else
  static StateFactory & factory()
  {
    if(!factory_ptr_) { factory_ptr_.reset(new StateFactory({}, {}, false)); }
    return *factory_ptr_;
  }
#endif
 
private:
  void resetPostures();
 
  void startIdleState();
 
  void teardownIdleState();
 
protected:
  std::map<std::string, std::shared_ptr<mc_tasks::PostureTask>> posture_tasks_;
  std::map<std::string, double> saved_posture_weights_;
 
  std::map<std::string, std::shared_ptr<mc_tasks::EndEffectorTask>> ff_tasks_;
 
#ifndef MC_RTC_BUILD_STATIC
  StateFactory factory_;
#else
  static std::unique_ptr<StateFactory> factory_ptr_;
  StateFactory & factory_;
#endif
 
  bool idle_keep_state_ = false;
  bool running_ = false;
  bool first_reset_ = true;
  Executor executor_;
};
 
namespace details
{
 
template<MCController::Backend backend, typename SolverT>
struct BackendSpecificController : public Controller
{
  BackendSpecificController(mc_rbdyn::RobotModulePtr rm,
                            double dt,
                            const mc_rtc::Configuration & config,
                            ControllerParameters params = {})
  : Controller(rm, dt, config, params.backend(backend))
  {
  }
 
  const SolverT & solver() const noexcept { return SolverT::from_solver(MCController::solver()); }
 
  SolverT & solver() noexcept { return SolverT::from_solver(MCController::solver()); }
};
 
} // namespace details
 
} // namespace fsm
 
} // namespace mc_control