AddRemoveContactTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/Robots.h>
#include <mc_solver/BoundedSpeedConstr.h>
#include <mc_tasks/MetaTask.h>
#include <mc_tasks/api.h>
 
#include <Tasks/QPTasks.h>
 
namespace mc_rbdyn
{
struct Contact;
}
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI AddRemoveContactTask : public MetaTask
{
public:
  AddRemoveContactTask(const mc_rbdyn::Robots & robots,
                       std::shared_ptr<mc_solver::BoundedSpeedConstr> constSpeedConstr,
                       const mc_rbdyn::Contact & contact,
                       double direction,
                       double speed = 0.01,
                       double stiffness = 2,
                       double weight = 1000,
                       Eigen::Vector3d * userT_0_s = nullptr);
 
  AddRemoveContactTask(const mc_solver::QPSolver & solver,
                       const mc_rbdyn::Contact & contact,
                       double direction,
                       double speed = 0.01,
                       double stiffness = 2,
                       double weight = 1000,
                       Eigen::Vector3d * userT_0_s = nullptr);
 
  void direction(double direction);
 
  double speed() { return speed_; }
  void speed(double s);
 
  double stiffness() { return stiffness_; }
  double weight() { return weight_; }
 
  Eigen::Vector3d velError();
 
  void dimWeight(const Eigen::VectorXd & dimW) override;
 
  Eigen::VectorXd dimWeight() const override;
 
  Eigen::VectorXd eval() const override;
 
  Eigen::VectorXd speed() const override;
 
public:
  const mc_rbdyn::Robots & robots;
  unsigned int robotIndex;
  unsigned int envIndex;
  std::shared_ptr<mc_solver::BoundedSpeedConstr> constSpeedConstr;
 
  mc_rbdyn::SurfacePtr robotSurf;
  unsigned int robotBodyIndex;
 
  sva::PTransformd targetTf;
 
  std::string bodyId;
  Eigen::Matrix6d dofMat;
  Eigen::Vector6d speedMat;
  Eigen::Vector3d normal;
 
  double stiffness_;
  double weight_;
  double speed_;
  double direction_;
  Eigen::Vector3d targetSpeed;
  mc_rtc::void_ptr impl_;
  double targetVelWeight;
 
private:
  /* Hide these virtual functions */
  void selectActiveJoints(mc_solver::QPSolver &,
                          const std::vector<std::string> &,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & = {}) override
  {
  }
 
  void selectUnactiveJoints(mc_solver::QPSolver &,
                            const std::vector<std::string> &,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & = {}) override
  {
  }
 
  void resetJointsSelector(mc_solver::QPSolver &) override {}
 
  void reset() override {}
 
private:
  void addToSolver(mc_solver::QPSolver & solver) override;
 
  void removeFromSolver(mc_solver::QPSolver & solver) override;
 
  void update(mc_solver::QPSolver &) override;
 
  bool manageConstraint = false;
};
 
struct MC_TASKS_DLLAPI AddContactTask : public AddRemoveContactTask
{
public:
  AddContactTask(mc_rbdyn::Robots & robots,
                 std::shared_ptr<mc_solver::BoundedSpeedConstr> constSpeedConstr,
                 mc_rbdyn::Contact & contact,
                 double speed = 0.01,
                 double stiffness = 2,
                 double weight = 1000,
                 Eigen::Vector3d * userT_0_s = nullptr);
 
  AddContactTask(mc_solver::QPSolver & solver,
                 mc_rbdyn::Contact & contact,
                 double speed = 0.01,
                 double stiffness = 2,
                 double weight = 1000,
                 Eigen::Vector3d * userT_0_s = nullptr);
};
 
struct MC_TASKS_DLLAPI RemoveContactTask : public AddRemoveContactTask
{
public:
  RemoveContactTask(mc_rbdyn::Robots & robots,
                    std::shared_ptr<mc_solver::BoundedSpeedConstr> constSpeedConstr,
                    mc_rbdyn::Contact & contact,
                    double speed = 0.01,
                    double stiffness = 2,
                    double weight = 1000,
                    Eigen::Vector3d * userT_0_s = nullptr);
 
  RemoveContactTask(mc_solver::QPSolver & solver,
                    mc_rbdyn::Contact & contact,
                    double speed = 0.01,
                    double stiffness = 2,
                    double weight = 1000,
                    Eigen::Vector3d * userT_0_s = nullptr);
};
 
} // namespace mc_tasks