ImpedanceTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/ImpedanceGains.h>
#include <mc_tasks/TransformTask.h>
 
#include <mc_filter/LowPass.h>
 
#include <mc_rtc/constants.h>
 
namespace mc_tasks
{
 
namespace force
{
 
struct MC_TASKS_DLLAPI ImpedanceTask : TransformTask
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  ImpedanceTask(const std::string & surfaceName,
                const mc_rbdyn::Robots & robots,
                unsigned robotIndex,
                double stiffness = 5.0,
                double weight = 1000.0);
 
  ImpedanceTask(const mc_rbdyn::RobotFrame & frame, double stiffness = 5.0, double weight = 1000.0);
 
  void reset() override;
 
  inline const ImpedanceGains & gains() const noexcept { return gains_; }
 
  inline ImpedanceGains & gains() noexcept { return gains_; }
 
  const sva::PTransformd & targetPose() const noexcept { return targetPoseW_; }
 
  void targetPose(const sva::PTransformd & pose) { targetPoseW_ = pose; }
 
  const sva::MotionVecd & targetVel() const noexcept { return targetVelW_; }
 
  void targetVel(const sva::MotionVecd & worldVel) override { targetVelW_ = worldVel; }
 
  const sva::MotionVecd & targetAccel() const noexcept { return targetAccelW_; }
 
  void targetAccel(const sva::MotionVecd & accel) { targetAccelW_ = accel; }
 
  const sva::PTransformd & deltaCompliancePose() const { return deltaCompPoseW_; }
 
  const sva::PTransformd compliancePose() const
  {
    sva::PTransformd T_0_d(targetPoseW_.rotation());
    return T_0_d * deltaCompPoseW_ * T_0_d.inv() * targetPoseW_;
  }
 
  const sva::ForceVecd & targetWrench() const noexcept { return targetWrench_; }
 
  void targetWrenchW(const sva::ForceVecd & wrenchW) { targetWrench(frame_->position().dualMul(wrenchW)); }
 
  void targetWrench(const sva::ForceVecd & wrench) { targetWrench_ = wrench; }
 
  const sva::ForceVecd & measuredWrench() const { return measuredWrench_; }
 
  const sva::ForceVecd & filteredMeasuredWrench() const { return filteredMeasuredWrench_; }
 
  double cutoffPeriod() const { return lowPass_.cutoffPeriod(); }
 
  void cutoffPeriod(double cutoffPeriod)
  {
    cutoffPeriod_ = cutoffPeriod;
    lowPass_.cutoffPeriod(cutoffPeriod_);
  }
 
  inline bool hold() const noexcept { return hold_; }
 
  inline void hold(bool hold) noexcept { hold_ = hold; }
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
protected:
  ImpedanceGains gains_ = ImpedanceGains::Default();
 
  sva::PTransformd deltaCompPoseW_ = sva::PTransformd::Identity();
  sva::MotionVecd deltaCompVelW_ = sva::MotionVecd::Zero();
  sva::MotionVecd deltaCompAccelW_ = sva::MotionVecd::Zero();
 
  // Limits of relative pose, velocity, and acceleration from target frame to compliance frame.
  double deltaCompPoseLinLimit_ = 1.0;
  double deltaCompPoseAngLimit_ = mc_rtc::constants::PI;
  double deltaCompVelLinLimit_ = 1e3;
  double deltaCompVelAngLimit_ = 1e3;
  double deltaCompAccelLinLimit_ = 1e3;
  double deltaCompAccelAngLimit_ = 1e3;
 
  // Target pose, velocity, and acceleration in the world frame
  sva::PTransformd targetPoseW_ = sva::PTransformd::Identity();
  sva::MotionVecd targetVelW_ = sva::MotionVecd::Zero();
  sva::MotionVecd targetAccelW_ = sva::MotionVecd::Zero();
 
  // Wrench in the surface frame
  sva::ForceVecd targetWrench_ = sva::ForceVecd::Zero();
  sva::ForceVecd measuredWrench_ = sva::ForceVecd::Zero();
  sva::ForceVecd filteredMeasuredWrench_ = sva::ForceVecd::Zero();
 
  double cutoffPeriod_ = 0.05;
  mc_filter::LowPass<sva::ForceVecd> lowPass_;
 
  // Hold mode
  bool hold_ = false;
 
  void update(mc_solver::QPSolver & solver) override;
 
  void addToSolver(mc_solver::QPSolver & solver) override;
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
  void addToLogger(mc_rtc::Logger & logger) override;
 
private:
  using TransformTask::refAccel;
  using TransformTask::refVelB;
 
  /* \brief Same as targetPose(const sva::PTransformd &) */
  void target(const sva::PTransformd & pos) override { targetPose(pos); }
 
  /* \brief Same as targetPose() */
  sva::PTransformd target() const override { return targetPose(); }
};
 
} // namespace force
 
} // namespace mc_tasks