PostureTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tasks/MetaTask.h>
 
#include <mc_rtc/void_ptr.h>
 
#include <Tasks/QPTasks.h>
 
namespace mc_tasks
{
 
struct MC_TASKS_DLLAPI PostureTask : public MetaTask
{
public:
  PostureTask(const mc_solver::QPSolver & solver, unsigned int rIndex, double stiffness = 1, double weight = 10);
 
  void reset() override;
 
  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
 
  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 posture(const std::vector<std::vector<double>> & p);
 
  void refVel(const Eigen::VectorXd & refVel) noexcept;
 
  const Eigen::VectorXd & refVel() const noexcept;
 
  void refAccel(const Eigen::VectorXd & refAccel) noexcept;
 
  const Eigen::VectorXd & refAccel() const noexcept;
 
  std::vector<std::vector<double>> posture() const;
 
  void jointGains(const mc_solver::QPSolver & solver, const std::vector<tasks::qp::JointGains> & jgs);
 
  void jointStiffness(const mc_solver::QPSolver & solver, const std::vector<tasks::qp::JointStiffness> & jss);
 
  void jointWeights(const std::map<std::string, double> & jws);
 
  void target(const std::map<std::string, std::vector<double>> & joints);
 
  void stiffness(double s);
 
  double stiffness() const;
 
  void damping(double d);
 
  double damping() const;
 
  void setGains(double stiffness, double damping);
 
  void weight(double w);
 
  double weight() const;
 
  bool inSolver() const;
 
protected:
  void addToSolver(mc_solver::QPSolver & solver) override;
 
  void removeFromSolver(mc_solver::QPSolver & solver) override;
 
  void update(mc_solver::QPSolver &) override;
 
  void addToGUI(mc_rtc::gui::StateBuilder &) override;
 
  void addToLogger(mc_rtc::Logger & logger) override;
 
private:
  bool inSolver_ = false;
  const mc_rbdyn::Robots & robots_;
  unsigned int rIndex_;
  mc_rtc::void_ptr pt_;
  double dt_;
  std::vector<std::vector<double>> posture_;
  std::unordered_map<std::string, std::vector<int>> mimics_;
  Eigen::VectorXd eval_;
  Eigen::VectorXd speed_;
};
 
using PostureTaskPtr = std::shared_ptr<PostureTask>;
 
} // namespace mc_tasks