AdmittanceTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_filter/utils/clamp.h>
 
#include <mc_tasks/TransformTask.h>
 
namespace mc_tasks
{
 
namespace force
{
 
struct MC_TASKS_DLLAPI AdmittanceTask : TransformTask
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  AdmittanceTask(const mc_rbdyn::RobotFrame & frame, double stiffness = 5.0, double weight = 1000.0);
 
  AdmittanceTask(const std::string & robotSurface,
                 const mc_rbdyn::Robots & robots,
                 unsigned robotIndex,
                 double stiffness = 5.0,
                 double weight = 1000.0);
 
  void reset() override;
 
  const sva::ForceVecd & admittance() const { return admittance_; }
 
  void admittance(const sva::ForceVecd & admittance) { admittance_ = admittance; }
 
  sva::PTransformd surfacePose() const { return frame_->position(); }
 
  sva::PTransformd targetPose() { return this->target(); }
 
  void targetPose(const sva::PTransformd & X_0_target) { this->target(X_0_target); }
 
  sva::PTransformd poseError() { return targetPose() * surfacePose().inv(); }
 
  const sva::ForceVecd & targetWrench() const { return targetWrench_; }
 
  void targetWrenchW(const sva::ForceVecd & wrenchW) { targetWrench(frame_->position().dualMul(wrenchW)); }
 
  void targetWrench(const sva::ForceVecd & wrench) { targetWrench_ = wrench; }
 
  sva::ForceVecd measuredWrench() const { return frame_->wrench(); }
 
  void maxLinearVel(const Eigen::Vector3d & maxLinearVel)
  {
    if((maxLinearVel.array() <= 0.).any())
    {
      mc_rtc::log::error("discarding maxLinearVel update as it is not positive");
      ;
      return;
    }
    maxLinearVel_ = maxLinearVel;
  }
 
  const Eigen::Vector3d & maxLinearVel() const noexcept { return maxLinearVel_; }
 
  void maxAngularVel(const Eigen::Vector3d & maxAngularVel)
  {
    if((maxAngularVel.array() <= 0.).any())
    {
      mc_rtc::log::error("discarding maxAngularVel update as it is not positive");
      ;
      return;
    }
    maxAngularVel_ = maxAngularVel;
  }
 
  const Eigen::Vector3d & maxAngularVel() const noexcept { return maxAngularVel_; }
 
  void velFilterGain(double gain) { velFilterGain_ = mc_filter::utils::clamp(gain, 0, 1); }
 
  double velFilterGain() const noexcept { return velFilterGain_; }
 
  void refVelB(const sva::MotionVecd & velB) { feedforwardVelB_ = velB; }
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
protected:
  Eigen::Vector3d maxAngularVel_ = {0.1, 0.1, 0.1}; // [rad] / [s]
  Eigen::Vector3d maxLinearVel_ = {0.1, 0.1, 0.1}; // [m] / [s]
  double timestep_;
  double velFilterGain_ = 0.8; //< Gain for the low-pass filter on reference velocity [0..1]
  sva::ForceVecd admittance_ = sva::ForceVecd(Eigen::Vector6d::Zero());
  sva::ForceVecd targetWrench_ = sva::ForceVecd(Eigen::Vector6d::Zero());
  sva::ForceVecd wrenchError_ = sva::ForceVecd(Eigen::Vector6d::Zero());
  sva::MotionVecd feedforwardVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero());
  sva::MotionVecd refVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero());
 
  void update(mc_solver::QPSolver &) override;
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
  void addToLogger(mc_rtc::Logger & logger) override;
 
  using TransformTask::refVelB;
 
  using TransformTask::target;
 
  void addToSolver(mc_solver::QPSolver & solver) override;
};
 
} // namespace force
 
} // namespace mc_tasks