CoPTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/DampingTask.h>
 
namespace mc_tasks
{
 
namespace force
{
 
struct MC_TASKS_DLLAPI CoPTask : DampingTask
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  CoPTask(const mc_rbdyn::RobotFrame & frame, double stiffness = 5.0, double weight = 1000.0);
 
  CoPTask(const std::string & robotSurface,
          const mc_rbdyn::Robots & robots,
          unsigned robotIndex,
          double stiffness = 5.0,
          double weight = 1000.0);
 
  void reset() override;
 
  std::function<bool(const mc_tasks::MetaTask &, std::string &)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const override;
 
  inline void useTargetPressure(bool s) noexcept { useTargetPressure_ = s; }
 
  inline bool useTargetPressure() const noexcept { return useTargetPressure_; }
 
  Eigen::Vector2d measuredCoP() const { return frame_->cop(); }
 
  Eigen::Vector3d measuredCoPW() const { return frame_->copW(); }
 
  void setZeroTargetWrench()
  {
    AdmittanceTask::targetWrench(sva::ForceVecd{Eigen::Vector6d::Zero()});
    targetCoP(Eigen::Vector2d::Zero());
  }
 
  const Eigen::Vector2d & targetCoP() const { return targetCoP_; }
 
  Eigen::Vector3d targetCoPW() const
  {
    Eigen::Vector3d cop_s;
    cop_s << targetCoP_, 0.;
    sva::PTransformd X_0_s = frame_->position();
    return X_0_s.translation() + X_0_s.rotation().transpose() * cop_s;
  }
 
  void targetCoP(const Eigen::Vector2d & targetCoP) { targetCoP_ = targetCoP; }
 
  const Eigen::Vector3d & targetForce() const { return targetForce_; }
 
  void targetForce(const Eigen::Vector3d & targetForce) { targetForce_ = targetForce; }
 
  void targetForceW(const Eigen::Vector3d & targetForceW)
  {
    const auto & X_0_rh = frame_->position();
    targetForce(X_0_rh.dualMul(sva::ForceVecd(Eigen::Vector3d::Zero(), targetForceW)).force());
  }
 
  const sva::ForceVecd & targetWrench() const { return AdmittanceTask::targetWrench(); }
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
protected:
  void addToLogger(mc_rtc::Logger & logger) override;
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
private:
  Eigen::Vector2d targetCoP_ = Eigen::Vector2d::Zero();
  Eigen::Vector3d targetForce_ = Eigen::Vector3d::Zero();
 
  bool useTargetPressure_ = false;
 
  void update(mc_solver::QPSolver &) override;
 
  using AdmittanceTask::targetWrench;
  using AdmittanceTask::targetWrenchW;
};
 
} // namespace force
 
} // namespace mc_tasks