TVMQPSolver.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 <mc_rtc/clock.h>
 
#include <tvm/ControlProblem.h>
#include <tvm/LinearizedControlProblem.h>
#include <tvm/scheme/WeightedLeastSquares.h>
 
namespace mc_solver
{
 
struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver
{
  TVMQPSolver(mc_rbdyn::RobotsPtr robots, double timeStep);
 
  TVMQPSolver(double timeStep);
 
  ~TVMQPSolver() final = default;
 
  void setContacts(ControllerToken, const std::vector<mc_rbdyn::Contact> & contacts) final;
 
  const sva::ForceVecd desiredContactForce(const mc_rbdyn::Contact & id) const final;
 
  double solveTime() final;
 
  double solveAndBuildTime() final;
 
  inline tvm::LinearizedControlProblem & problem() noexcept { return problem_; }
 
  inline const tvm::LinearizedControlProblem & problem() const noexcept { return problem_; }
 
  static inline TVMQPSolver & from_solver(QPSolver & solver) noexcept
  {
    assert(solver.backend() == QPSolver::Backend::TVM);
    return static_cast<TVMQPSolver &>(solver);
  }
 
  static inline const TVMQPSolver & from_solver(const QPSolver & solver) noexcept
  {
    assert(solver.backend() == QPSolver::Backend::TVM);
    return static_cast<const TVMQPSolver &>(solver);
  }
 
private:
  tvm::LinearizedControlProblem problem_;
  tvm::scheme::WeightedLeastSquares solver_;
  struct ContactData
  {
    tvm::TaskWithRequirementsPtr contactConstraint_;
    tvm::VariableVector f1_;
    std::vector<tvm::TaskWithRequirementsPtr> f1Constraints_;
    tvm::VariableVector f2_;
    std::vector<tvm::TaskWithRequirementsPtr> f2Constraints_;
  };
  std::vector<ContactData> contactsData_;
 
  mc_rtc::duration_ms solve_dt_{0};
 
  bool runCommon();
  bool runOpenLoop();
  bool runJointsFeedback(bool wVelocity);
 
  bool runClosedLoop(bool integrateControlState);
 
  void updateRobot(mc_rbdyn::Robot & robot);
 
  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::unordered_map<std::string, DynamicsConstraint *> dynamics_;
 
  bool run_impl(FeedbackType fType = FeedbackType::None) final;
 
  void addDynamicsConstraint(mc_solver::DynamicsConstraint * dynamics) final;
 
  void removeDynamicsConstraint(mc_solver::ConstraintSet * maybe_dynamics) final;
  void removeDynamicsConstraint(mc_solver::DynamicsConstraint * dyn);
 
  size_t getContactIdx(const mc_rbdyn::Contact & contact);
  void addContact(const mc_rbdyn::Contact & contact);
  using ContactIterator = std::vector<mc_rbdyn::Contact>::iterator;
  ContactIterator removeContact(size_t idx);
  std::tuple<size_t, bool> addVirtualContactImpl(const mc_rbdyn::Contact & contact);
  void addContactToDynamics(const std::string & robot,
                            const mc_rbdyn::RobotFrame & frame,
                            const std::vector<sva::PTransformd> & points,
                            tvm::VariableVector & forces,
                            std::vector<tvm::TaskWithRequirementsPtr> & constraints,
                            const Eigen::MatrixXd & frictionCone,
                            double dir);
};
 
inline TVMQPSolver & tvm_solver(QPSolver & solver) noexcept
{
  return TVMQPSolver::from_solver(solver);
}
 
/* const version */
inline const TVMQPSolver & tvm_solver(const QPSolver & solver) noexcept
{
  return TVMQPSolver::from_solver(solver);
}
 
} // namespace mc_solver