MomentumFunction.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/Momentum.h>
 
#include <mc_rbdyn/fwd.h>
 
#include <tvm/function/abstract/Function.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI MomentumFunction : tvm::function::abstract::Function
{
  SET_UPDATES(MomentumFunction, Value, Velocity, Jacobian, NormalAcceleration, JDot)
 
  
  MomentumFunction(const mc_rbdyn::Robot & robot);
 
  void reset();
 
  inline const sva::ForceVecd & momentum() const noexcept { return momentum_; }
 
  inline void momentum(const sva::ForceVecd & momentum) noexcept { momentum_ = momentum; }
 
  inline const Eigen::Vector6d & refVel() const noexcept { return refVel_; }
 
  inline void refVel(const Eigen::Vector6d & refVel) noexcept { refVel_ = refVel; }
 
  inline const Eigen::Vector6d & refAccel() const noexcept { return refAccel_; }
 
  inline void refAccel(const Eigen::Vector6d & refAccel) noexcept { refAccel_ = refAccel; }
 
  inline const mc_tvm::Momentum & algo() const noexcept { return momentumAlgo_; }
 
private:
  mc_tvm::Momentum & momentumAlgo_;
  sva::ForceVecd momentum_;
  Eigen::Vector6d refVel_;
  Eigen::Vector6d refAccel_;
 
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  void updateNormalAcceleration();
  void updateJDot();
};
 
} // namespace mc_tvm