EndEffectorTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/OrientationTask.h>
#include <mc_tasks/PositionTask.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI EndEffectorTask : public MetaTask
{
public:
  EndEffectorTask(const mc_rbdyn::RobotFrame & frame, double stiffness = 2.0, double weight = 1000.0);
 
  EndEffectorTask(const std::string & bodyName,
                  const mc_rbdyn::Robots & robots,
                  unsigned int robotIndex,
                  double stiffness = 2.0,
                  double weight = 1000.0);
 
  void reset() override;
 
  virtual void add_ef_pose(const sva::PTransformd & dtr);
 
  virtual void set_ef_pose(const sva::PTransformd & tf);
 
  virtual sva::PTransformd get_ef_pose();
 
  void dimWeight(const Eigen::VectorXd & dimW) override;
 
  Eigen::VectorXd dimWeight() const override;
 
  void selectActiveJoints(mc_solver::QPSolver & solver,
                          const std::vector<std::string> & activeJointsName,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {}) override;
 
  void selectUnactiveJoints(mc_solver::QPSolver & solver,
                            const std::vector<std::string> & unactiveJointsName,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {}) override;
 
  void resetJointsSelector(mc_solver::QPSolver & solver) override;
 
  Eigen::VectorXd eval() const override;
 
  Eigen::VectorXd speed() const override;
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
  using MetaTask::name;
 
  void name(const std::string & name) override;
 
public:
  std::shared_ptr<mc_tasks::PositionTask> positionTask;
  std::shared_ptr<mc_tasks::OrientationTask> orientationTask;
 
  sva::PTransformd curTransform;
 
protected:
  void removeFromSolver(mc_solver::QPSolver & solver) override;
 
  void addToSolver(mc_solver::QPSolver & solver) override;
 
  void update(mc_solver::QPSolver &) override;
 
  void addToLogger(mc_rtc::Logger & logger) override;
 
  void removeFromLogger(mc_rtc::Logger & logger) override;
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
 
  inline const mc_rbdyn::RobotFrame & frame() const noexcept { return *positionTask->frame_; }
};
 
} // namespace mc_tasks