TransformTask.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_rbdyn/RobotFrame.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
{
public:
  TransformTask(const mc_rbdyn::RobotFrame & frame,
                double stiffness = 2.0,
                double weight = 500.0,
                bool showTarget = true,
                bool showPose = true);
 
  TransformTask(const std::string & surfaceName,
                const mc_rbdyn::Robots & robots,
                unsigned int robotIndex,
                double stiffness = 2.0,
                double weight = 500,
                bool showTarget = true,
                bool showPose = true);
 
  void reset() override;
 
  virtual sva::PTransformd target() const;
 
  virtual void target(const sva::PTransformd & worldPos);
 
  virtual void targetVel(const sva::MotionVecd & worldVel);
 
  void targetSurface(unsigned int robotIndex,
                     const std::string & surfaceName,
                     const sva::PTransformd & offset = sva::PTransformd::Identity());
 
  void targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset = sva::PTransformd::Identity());
 
  void targetFrameVelocity(const mc_rbdyn::Frame & targetFrame,
                           const sva::PTransformd & offset = sva::PTransformd::Identity());
 
  virtual void target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset);
 
  inline const std::string & surface() const noexcept { return frame_->name(); }
 
  const mc_rbdyn::RobotFrame & frame() const noexcept { return *frame_; }
 
  inline sva::PTransformd surfacePose() const noexcept { return frame_->position(); }
 
  std::function<bool(const mc_tasks::MetaTask & task, std::string &)> buildCompletionCriteria(
      double dt,
      const mc_rtc::Configuration & config) const override;
 
  void addToLogger(mc_rtc::Logger & logger) override;
 
  using TrajectoryTaskGeneric::damping;
  using TrajectoryTaskGeneric::setGains;
  using TrajectoryTaskGeneric::stiffness;
 
  void setGains(const sva::MotionVecd & stiffness, const sva::MotionVecd & damping)
  {
    return TrajectoryTaskGeneric::setGains(stiffness.vector(), damping.vector());
  }
 
  void stiffness(const sva::MotionVecd & stiffness) { return TrajectoryTaskGeneric::stiffness(stiffness.vector()); }
 
  sva::MotionVecd mvStiffness() { return sva::MotionVecd(dimStiffness()); }
 
  void damping(const sva::MotionVecd & damping) { return TrajectoryTaskGeneric::damping(damping.vector()); }
 
  sva::MotionVecd mvDamping() { return sva::MotionVecd(dimDamping()); }
 
  void refVelB(const sva::MotionVecd & velB) { TrajectoryTaskGeneric::refVel(velB.vector()); }
 
  sva::MotionVecd refVelB() const { return sva::MotionVecd(TrajectoryTaskGeneric::refVel()); }
 
  void refAccel(const sva::MotionVecd & accel) { return TrajectoryTaskGeneric::refAccel(accel.vector()); }
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
protected:
  mc_rbdyn::ConstRobotFramePtr frame_;
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
  /* Don't use parent's refVel() as the velocity frame (spatial or body) is
   * ambiguous. */
  using TrajectoryTaskGeneric::refVel;
 
  bool showTarget_ = true;
  bool showPose_ = true;
};
 
} // namespace mc_tasks