ComplianceTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/EndEffectorTask.h>
#include <mc_tasks/MetaTask.h>
 
namespace mc_tasks
{
 
namespace force
{
 
struct MC_TASKS_DLLAPI ComplianceTask : MetaTask
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  static const std::pair<double, double> defaultFGain;
  static const std::pair<double, double> defaultTGain;
 
  ComplianceTask(const mc_rbdyn::Robots & robots,
                 unsigned int robotIndex,
                 const std::string & body,
                 double timestep,
                 const Eigen::Matrix6d & dof = Eigen::Matrix6d::Identity(),
                 double stiffness = 5.0,
                 double weight = 1000.0,
                 double forceThresh = 3.,
                 double torqueThresh = 1.,
                 std::pair<double, double> forceGain = defaultFGain,
                 std::pair<double, double> torqueGain = defaultTGain);
 
  ComplianceTask(const mc_rbdyn::Robots & robots,
                 unsigned int robotIndex,
                 const std::string & body,
                 double timestep,
                 double stiffness = 5.0,
                 double weight = 1000.0,
                 double forceThresh = 3.,
                 double torqueThresh = 1.,
                 std::pair<double, double> forceGain = defaultFGain,
                 std::pair<double, double> torqueGain = defaultTGain);
 
  void reset() override;
 
  sva::ForceVecd getFilteredWrench() const;
 
  void setTargetWrench(const sva::ForceVecd & wrench) { obj_ = wrench; }
 
  sva::ForceVecd getTargetWrench() { return obj_; }
 
  void stiffness(double s)
  {
    efTask_->positionTask->stiffness(s);
    efTask_->orientationTask->stiffness(s);
  }
  double stiffness() { return efTask_->positionTask->stiffness(); }
 
  void weight(double w)
  {
    efTask_->positionTask->weight(w);
    efTask_->orientationTask->weight(w);
  }
  double weight() { return efTask_->positionTask->weight(); }
 
  void forceThresh(double t) { forceThresh_ = t; }
  double forceThresh() { return forceThresh_; }
 
  void torqueThresh(double t) { torqueThresh_ = t; }
  double torqueThresh() { return torqueThresh_; }
 
  void forceGain(std::pair<double, double> t) { forceGain_ = t; }
  std::pair<double, double> forceGain() { return forceGain_; }
 
  void torqueGain(std::pair<double, double> t) { torqueGain_ = t; }
  std::pair<double, double> torqueGain() { return torqueGain_; }
 
  void dof(const Eigen::Matrix6d & dof) { dof_ = dof; }
  Eigen::Matrix6d dof() { return dof_; }
 
  void dimWeight(const Eigen::VectorXd & dimW) override;
 
  Eigen::VectorXd dimWeight() const override;
 
  void selectActiveJoints(mc_solver::QPSolver & solver,
                          const std::vector<std::string> & activeJointsName,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {}) override;
 
  void selectUnactiveJoints(mc_solver::QPSolver & solver,
                            const std::vector<std::string> & unactiveJointsName,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {}) override;
 
  void resetJointsSelector(mc_solver::QPSolver & solver) override;
 
  Eigen::VectorXd eval() const override { return wrench_.vector(); }
 
  Eigen::VectorXd speed() const override
  {
    return robots_.robot(rIndex_).mbc().bodyVelW[robots_.robot(rIndex_).bodyIndexByName(sensor_.parentBody())].vector();
  }
 
private:
  sva::PTransformd computePose();
 
  std::shared_ptr<EndEffectorTask> efTask_;
  sva::ForceVecd wrench_;
  sva::ForceVecd obj_;
  sva::ForceVecd error_;
  sva::ForceVecd errorD_;
  const mc_rbdyn::Robots & robots_;
  unsigned int rIndex_;
  const mc_rbdyn::ForceSensor & sensor_;
  double timestep_;
  double forceThresh_, torqueThresh_;
  std::pair<double, double> forceGain_, torqueGain_;
  Eigen::Matrix6d dof_;
  std::function<double(double)> clampTrans_, clampRot_;
 
  void addToSolver(mc_solver::QPSolver & solver) override;
 
  void removeFromSolver(mc_solver::QPSolver & solver) override;
 
  void update(mc_solver::QPSolver &) override;
};
 
} // namespace force
 
} // namespace mc_tasks