Robot.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/Clock.h>
#include <tvm/Variable.h>
#include <tvm/VariableVector.h>
#include <tvm/graph/abstract/Node.h>
 
#include <RBDyn/FD.h>
#include <RBDyn/MultiBody.h>
#include <RBDyn/MultiBodyConfig.h>
#include <RBDyn/MultiBodyGraph.h>
 
#include <RBDyn/parsers/urdf.h>
 
namespace tvm
{
 
class TVM_DLLAPI Robot : public graph::abstract::Node<Robot>
{
public:
  SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, CoM, H, C, Geometry, Dynamics)
  SET_UPDATES(Robot, Time, FK, FV, FA, NormalAcceleration, CoM, H, C)
 
  
  Robot(Clock & clock,
        const std::string & name,
        rbd::MultiBodyGraph & mbg,
        rbd::MultiBody mb,
        rbd::MultiBodyConfig mbc,
        const rbd::parsers::Limits & limits = {{}, {}, {}, {}});
 
  inline const std::string & name() const { return name_; }
 
  inline double mass() const { return mass_; }
 
  inline const VariableVector & q() const { return q_; }
  inline VariableVector & q() { return q_; }
 
  inline const VariablePtr & qFreeFlyer() const { return q_ff_; }
  inline VariablePtr & qFreeFlyer() { return q_ff_; }
 
  inline const VariablePtr & qJoints() const { return q_joints_; }
  inline VariablePtr & qJoints() { return q_joints_; }
 
  inline const VariablePtr & tau() const { return tau_; }
  inline VariablePtr & tau() { return tau_; }
 
  inline const rbd::MultiBody & mb() const { return mb_; }
  inline rbd::MultiBody & mb() { return mb_; }
 
  inline const rbd::MultiBodyConfig & mbc() const { return mbc_; }
  inline rbd::MultiBodyConfig & mbc() { return mbc_; }
 
  inline const std::vector<sva::MotionVecd> & normalAccB() const { return normalAccB_; }
  inline std::vector<sva::MotionVecd> & normalAccB() { return normalAccB_; }
 
  inline const Eigen::VectorXd & lQBound() const { return lQBound_; }
  inline Eigen::VectorXd & lQBound() { return lQBound_; }
 
  inline const Eigen::VectorXd & uQBound() const { return uQBound_; }
  inline Eigen::VectorXd & uQBound() { return uQBound_; }
 
  inline const Eigen::VectorXd & lVelBound() const { return lVelBound_; }
  inline Eigen::VectorXd & lVelBound() { return lVelBound_; }
 
  inline const Eigen::VectorXd & uVelBound() const { return uVelBound_; }
  inline Eigen::VectorXd & uVelBound() { return uVelBound_; }
 
  inline const Eigen::VectorXd & lTauBound() const { return lTauBound_; }
  inline Eigen::VectorXd & lTauBound() { return lTauBound_; }
 
  inline const Eigen::VectorXd & uTauBound() const { return uTauBound_; }
  inline Eigen::VectorXd & uTauBound() { return uTauBound_; }
 
  inline const Eigen::MatrixXd & H() const { return fd_.H(); }
  inline const Eigen::VectorXd & C() const { return fd_.C(); }
 
  inline const Eigen::Vector3d & com() const { return com_; }
 
  inline const sva::PTransformd & bodyTransform(const std::string & b) const { return bodyTransforms_.at(b); }
 
private:
  Clock & clock_;
  uint64_t last_tick_ = 0;
  std::string name_;
  double mass_;
  rbd::MultiBody mb_;
  rbd::MultiBodyConfig mbc_;
  std::vector<sva::MotionVecd> normalAccB_;
  Eigen::VectorXd lQBound_;
  Eigen::VectorXd uQBound_;
  Eigen::VectorXd lVelBound_;
  Eigen::VectorXd uVelBound_;
  Eigen::VectorXd lTauBound_;
  Eigen::VectorXd uTauBound_;
  rbd::ForwardDynamics fd_;
  std::map<std::string, sva::PTransformd> bodyTransforms_;
  VariablePtr q_ff_;
  VariablePtr q_joints_;
  VariableVector q_;
  VariableVector dq_;
  VariableVector ddq_;
  VariablePtr tau_;
  Eigen::Vector3d com_;
 
private:
  void computeNormalAccB();
 
  void updateTimeDependency();
  void updateFK();
  void updateFV();
  void updateFA();
  void updateNormalAcceleration();
  void updateH();
  void updateC();
  void updateCoM();
};
 
} // namespace tvm