Robot.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/CoM.h>
#include <mc_tvm/Limits.h>
#include <mc_tvm/Momentum.h>
 
#include <mc_rbdyn/Robot.h>
 
#include <tvm/Variable.h>
#include <tvm/VariableVector.h>
#include <tvm/graph/abstract/Node.h>
 
#include <RBDyn/FD.h>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
{
  SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C)
  SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C)
 
  friend struct mc_rbdyn::Robot;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  using mimic_variables_t = std::pair<tvm::VariableVector, Eigen::VectorXd>;
 
protected:
  struct NewRobotToken
  {
  };
 
public:
  Robot(NewRobotToken, const mc_rbdyn::Robot & robot);
 
  Robot(Robot &&) = delete;
  Robot & operator=(Robot &&) = delete;
 
  Robot(const Robot &) = delete;
  Robot & operator=(const Robot &) = delete;
 
  inline const mc_rbdyn::Robot & robot() const { return robot_; }
 
  inline const Limits & limits() const noexcept { return limits_; }
 
  inline Limits & limits() noexcept { return limits_; }
 
  inline const tvm::VariablePtr & q() const noexcept { return q_; }
  inline tvm::VariablePtr & q() noexcept { return q_; }
 
  inline const tvm::VariablePtr & alpha() const noexcept { return dq_; }
  inline tvm::VariablePtr & alpha() noexcept { return dq_; }
 
  inline const tvm::VariablePtr & alphaD() const noexcept { return ddq_; }
  inline tvm::VariablePtr & alphaD() noexcept { return ddq_; }
 
  inline const tvm::VariablePtr & qFloatingBase() const noexcept { return q_fb_; }
  inline tvm::VariablePtr & qFloatingBase() noexcept { return q_fb_; }
 
  inline const tvm::VariablePtr & qJoints() const noexcept { return q_joints_; }
  inline tvm::VariablePtr & qJoints() noexcept { return q_joints_; }
 
  tvm::VariablePtr qJoint(size_t jIdx);
 
  inline tvm::VariablePtr qJoint(const std::string & jName) { return qJoint(robot().jointIndexByName(jName)); }
 
  inline const std::map<tvm::VariablePtr, mimic_variables_t> & mimics() const noexcept { return mimics_; }
  inline std::map<tvm::VariablePtr, mimic_variables_t> & mimics() noexcept { return mimics_; }
 
  inline const tvm::VariablePtr & tau() const noexcept { return tau_; }
  inline tvm::VariablePtr & tau() { return tau_; }
 
  inline const CoM & comAlgo() const noexcept { return *com_; }
 
  inline CoM & comAlgo() noexcept { return *com_; }
 
  inline const Momentum & momentumAlgo() const noexcept { return *momentum_; }
 
  inline Momentum & momentumAlgo() noexcept { return *momentum_; }
 
  inline const Eigen::MatrixXd & H() const noexcept { return fd_.H(); }
 
  inline const Eigen::VectorXd & C() const noexcept { return fd_.C(); }
 
  inline const std::vector<sva::MotionVecd> & normalAccB() const noexcept { return normalAccB_; }
 
  inline Eigen::DenseIndex refJointIndexToQIndex(size_t jointIndex) const
  {
    return refJointIndexToQIndex_.at(jointIndex);
  }
 
  inline Eigen::DenseIndex refJointIndexToQDotIndex(size_t jointIndex) const
  {
    return refJointIndexToQDotIndex_.at(jointIndex);
  }
 
private:
  const mc_rbdyn::Robot & robot_;
  Limits limits_;
  tvm::VariablePtr q_fb_;
  tvm::VariablePtr q_joints_;
  tvm::VariablePtr q_;
  std::map<tvm::VariablePtr, mimic_variables_t> mimics_;
  tvm::VariablePtr dq_;
  tvm::VariablePtr ddq_;
  tvm::VariablePtr tau_;
  std::vector<sva::MotionVecd> normalAccB_;
  rbd::ForwardDynamics fd_;
  CoMPtr com_;
  MomentumPtr momentum_;
  std::vector<Eigen::DenseIndex> refJointIndexToQIndex_;
  std::vector<Eigen::DenseIndex> refJointIndexToQDotIndex_;
 
  /* Update functions */
  void updateFK();
  void updateFV();
  void updateFA();
  void updateNormalAcceleration();
  void updateH();
  void updateC();
};
 
} // namespace mc_tvm