PositionTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/TrajectoryTaskGeneric.h>
 
#include <mc_tvm/PositionFunction.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI PositionTask : public TrajectoryTaskGeneric
{
public:
  friend struct EndEffectorTask;
 
  PositionTask(const mc_rbdyn::RobotFrame & frame, double stiffness = 2.0, double weight = 500.0);
 
  PositionTask(const std::string & bodyName,
               const mc_rbdyn::Robots & robots,
               unsigned int robotIndex,
               double stiffness = 2.0,
               double weight = 500);
 
  virtual ~PositionTask() = default;
 
  void reset() override;
 
  inline const Eigen::Vector3d & position() const noexcept
  {
    switch(backend_)
    {
      case Backend::Tasks:
        return static_cast<tasks::qp::PositionTask *>(errorT.get())->position();
      case Backend::TVM:
        return static_cast<mc_tvm::PositionFunction *>(errorT.get())->position();
      default:
        mc_rtc::log::error_and_throw("Not implemented");
    }
  }
 
  inline void position(const Eigen::Vector3d & pos) noexcept
  {
    switch(backend_)
    {
      case Backend::Tasks:
        static_cast<tasks::qp::PositionTask *>(errorT.get())->position(pos);
        break;
      case Backend::TVM:
        static_cast<mc_tvm::PositionFunction *>(errorT.get())->position(pos);
        break;
      default:
        mc_rtc::log::error_and_throw("Not implemented");
    }
  }
 
  inline const Eigen::Vector3d & bodyPoint() const noexcept
  {
    switch(backend_)
    {
      case Backend::Tasks:
        return static_cast<tasks::qp::PositionTask *>(errorT.get())->bodyPoint();
      case Backend::TVM:
      {
        static Eigen::Vector3d res = Eigen::Vector3d::Zero();
        return res;
      }
      default:
        mc_rtc::log::error_and_throw("Not implemented");
    }
  }
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
protected:
  mc_rbdyn::ConstRobotFramePtr frame_;
  void addToLogger(mc_rtc::Logger & logger) override;
};
 
} // namespace mc_tasks