BoundedSpeedConstr.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/ConstraintSet.h>
 
#include <mc_rtc/void_ptr.h>
 
namespace mc_solver
{
 
struct MC_SOLVER_DLLAPI BoundedSpeedConstr : public ConstraintSet
{
public:
  BoundedSpeedConstr(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double dt);
 
  void addToSolverImpl(QPSolver & solver) override;
 
  void removeFromSolverImpl(QPSolver & solver) override;
 
  inline void addBoundedSpeed(QPSolver & solver,
                              const std::string & bodyName,
                              const Eigen::Vector3d & bodyPoint,
                              const Eigen::MatrixXd & dof,
                              const Eigen::VectorXd & speed)
  {
    addBoundedSpeed(solver, bodyName, bodyPoint, dof, speed, speed);
  }
 
  void addBoundedSpeed(QPSolver & solver,
                       const std::string & bodyName,
                       const Eigen::Vector3d & bodyPoint,
                       const Eigen::MatrixXd & dof,
                       const Eigen::VectorXd & lowerSpeed,
                       const Eigen::VectorXd & upperSpeed);
 
  inline void addBoundedSpeed(QPSolver & solver,
                              const mc_rbdyn::RobotFrame & frame,
                              const Eigen::MatrixXd & dof,
                              const Eigen::VectorXd & speed)
  {
    addBoundedSpeed(solver, frame, dof, speed, speed);
  }
 
  void addBoundedSpeed(QPSolver & solver,
                       const mc_rbdyn::RobotFrame & frame,
                       const Eigen::MatrixXd & dof,
                       const Eigen::VectorXd & lowerSpeed,
                       const Eigen::VectorXd & upperSpeed);
 
  bool removeBoundedSpeed(QPSolver & solver, const std::string & frameName);
 
  size_t nrBoundedSpeeds() const;
 
  void reset(QPSolver & solver);
 
private:
  mc_rtc::void_ptr constraint_;
  unsigned int robotIndex;
};
 
} // namespace mc_solver