PostureFunction.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/api.h>
 
#include <mc_rbdyn/fwd.h>
 
#include <tvm/function/IdentityFunction.h>
 
namespace mc_tvm
{
 
class MC_TVM_DLLAPI PostureFunction : public tvm::function::IdentityFunction
{
public:
  SET_UPDATES(PostureFunction, Value, Velocity)
 
  
  PostureFunction(const mc_rbdyn::Robot & robot);
 
  void reset();
 
  void posture(const std::string & j, const std::vector<double> & q);
 
  void posture(const std::vector<std::vector<double>> & p);
 
  const std::vector<std::vector<double>> & posture() const noexcept { return posture_; }
 
  inline const Eigen::VectorXd & refVel() const noexcept { return refVel_; }
 
  inline void refVel(const Eigen::VectorXd & vel) noexcept
  {
    assert(refVel_.size() == vel.size());
    refVel_ = vel;
  }
 
  inline const Eigen::VectorXd & refAccel() const noexcept { return refAccel_; }
 
  inline void refAccel(const Eigen::VectorXd & acc) noexcept
  {
    assert(refAccel_.size() == acc.size());
    refAccel_ = acc;
    normalAcceleration_ = -acc;
  }
 
protected:
  void updateValue_() override;
  void updateVelocity_() override;
 
  const mc_rbdyn::Robot & robot_;
 
  std::vector<std::vector<double>> posture_;
 
  int j0_;
 
  Eigen::VectorXd refVel_;
 
  Eigen::VectorXd refAccel_;
};
 
} // namespace mc_tvm