TasksQPSolver.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/QPSolver.h>
 
#include <Tasks/QPMotionConstr.h>
#include <Tasks/QPSolver.h>
 
namespace mc_solver
{
 
struct MC_SOLVER_DLLAPI TasksQPSolver final : public QPSolver
{
  TasksQPSolver(mc_rbdyn::RobotsPtr robots, double timeStep) : QPSolver(robots, timeStep, QPSolver::Backend::Tasks)
  {
    positive_lambda_constraint_.addToSolver(this->robots().mbs(), solver_);
  }
 
  TasksQPSolver(double timeStep) : QPSolver(timeStep, QPSolver::Backend::Tasks)
  {
    positive_lambda_constraint_.addToSolver(robots().mbs(), solver_);
  }
 
  ~TasksQPSolver() final = default;
 
  using QPSolver::addTask;
 
  void addTask(tasks::qp::Task * task);
 
  inline void addTask(std::shared_ptr<tasks::qp::Task> task)
  {
    if(task)
    {
      addTask(task.get());
      shPtrTasksStorage.emplace_back(task);
    }
  }
 
  using QPSolver::removeTask;
 
  void removeTask(tasks::qp::Task * task);
 
  void setContacts(ControllerToken, const std::vector<mc_rbdyn::Contact> & contacts) final;
 
  template<typename... Fun>
  void addConstraint(tasks::qp::ConstraintFunction<Fun...> * constraint)
  {
    constraint->addToSolver(robots().mbs(), solver_);
    solver_.updateConstrSize();
    solver_.updateNrVars(robots().mbs());
  }
 
  template<typename... Fun>
  void removeConstraint(tasks::qp::ConstraintFunction<Fun...> * constraint)
  {
    constraint->removeFromSolver(solver_);
    solver_.updateConstrSize();
    solver_.updateNrVars(robots().mbs());
  }
 
  std::pair<int, const tasks::qp::BilateralContact &> contactById(const tasks::qp::ContactId & id) const;
 
  Eigen::VectorXd lambdaVec(int cIndex) const;
 
  const sva::ForceVecd desiredContactForce(const mc_rbdyn::Contact & id) const final;
 
  void updateNrVars();
 
  void updateNrVars(const mc_rbdyn::Robots & robots);
 
  void updateConstrSize();
 
  inline tasks::qp::SolverData & data() noexcept { return solver_.data(); }
 
  inline const tasks::qp::SolverData & data() const noexcept { return solver_.data(); }
 
  tasks::qp::QPSolver & solver() noexcept { return solver_; }
 
  const tasks::qp::QPSolver & solver() const noexcept { return solver_; }
 
  static inline TasksQPSolver & from_solver(QPSolver & solver) noexcept
  {
    assert(solver.backend() == QPSolver::Backend::Tasks);
    return static_cast<TasksQPSolver &>(solver);
  }
 
  static inline const TasksQPSolver & from_solver(const QPSolver & solver) noexcept
  {
    assert(solver.backend() == QPSolver::Backend::Tasks);
    return static_cast<const TasksQPSolver &>(solver);
  }
 
  double solveTime() final;
 
  double solveAndBuildTime() final;
 
  const Eigen::VectorXd & result() const;
 
private:
  tasks::qp::QPSolver solver_;
  tasks::qp::PositiveLambda positive_lambda_constraint_;
  std::vector<tasks::qp::UnilateralContact> uniContacts_;
  std::vector<tasks::qp::BilateralContact> biContacts_;
  bool runOpenLoop();
  bool runJointsFeedback(bool wVelocity);
  bool runClosedLoop(bool integrateControlState);
 
  std::vector<std::vector<double>> prev_encoders_{};
  std::vector<std::vector<double>> encoders_alpha_{};
  std::vector<std::vector<std::vector<double>>> control_q_{};
  std::vector<std::vector<std::vector<double>>> control_alpha_{};
 
  std::vector<mc_solver::DynamicsConstraint *> dynamicsConstraints_;
 
  bool run_impl(FeedbackType fType = FeedbackType::None) final;
 
  void addDynamicsConstraint(mc_solver::DynamicsConstraint * dynamics) final;
 
  void removeDynamicsConstraint(mc_solver::ConstraintSet * maybe_dynamics) final;
};
 
inline TasksQPSolver & tasks_solver(QPSolver & solver) noexcept
{
  return TasksQPSolver::from_solver(solver);
}
 
/* const version */
inline const TasksQPSolver & tasks_solver(const QPSolver & solver) noexcept
{
  return TasksQPSolver::from_solver(solver);
}
 
} // namespace mc_solver