CoM.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_tvm/fwd.h>
 
#include <mc_rtc/shared.h>
 
#include <RBDyn/CoM.h>
 
#include <tvm/graph/abstract/Node.h>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI CoM : public tvm::graph::abstract::Node<CoM>
{
  SET_OUTPUTS(CoM, CoM, Jacobian, Velocity, NormalAcceleration, Acceleration, JDot)
  SET_UPDATES(CoM, CoM, Jacobian, Velocity, NormalAcceleration, Acceleration, JDot)
 
  friend struct Robot;
 
private:
  struct NewCoMToken
  {
  };
 
public:
  CoM(NewCoMToken, Robot & robot);
 
  inline const Eigen::Vector3d & com() const noexcept { return com_; }
 
  inline const Eigen::Vector3d & velocity() const noexcept { return velocity_; }
 
  inline const Eigen::Vector3d & normalAcceleration() const noexcept { return normalAcceleration_; }
 
  inline const Eigen::Vector3d & acceleration() const noexcept { return acceleration_; }
 
  inline const Eigen::MatrixXd & jacobian() const noexcept { return jac_.jacobian(); }
 
  inline const Eigen::MatrixXd & JDot() const noexcept { return jac_.jacobianDot(); }
 
  inline const Robot & robot() const noexcept { return robot_; }
 
  inline Robot & robot() noexcept { return robot_; }
 
  inline const rbd::CoMJacobian & comJacobian() const noexcept { return jac_; }
 
private:
  Robot & robot_;
  rbd::CoMJacobian jac_;
 
  Eigen::Vector3d com_;
  void updateCoM();
 
  Eigen::Vector3d velocity_;
  void updateVelocity();
 
  Eigen::Vector3d normalAcceleration_;
  void updateNormalAcceleration();
 
  Eigen::Vector3d acceleration_;
  void updateAcceleration();
 
  void updateJacobian();
 
  void updateJDot();
};
 
} // namespace mc_tvm