RobotFrame.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/Frame.h>
 
#include <mc_rbdyn/RobotFrame.h>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI RobotFrame : public Frame
{
  SET_OUTPUTS(RobotFrame, Jacobian, NormalAcceleration, JDot)
  SET_UPDATES(RobotFrame, Jacobian, NormalAcceleration, JDot)
 
  friend struct mc_rbdyn::RobotFrame;
 
protected:
  struct NewRobotFrameToken : public Frame::NewFrameToken
  {
  };
 
public:
  RobotFrame(NewRobotFrameToken, const mc_rbdyn::RobotFrame & frame);
 
  inline const tvm::internal::MatrixWithProperties & jacobian() const noexcept { return jacobian_; }
 
  inline const sva::MotionVecd & normalAcceleration() const noexcept { return normalAcceleration_; }
 
  inline const tvm::internal::MatrixWithProperties & JDot() const noexcept { return jacDot_; }
 
  inline const rbd::Jacobian & rbdJacobian() const noexcept { return jac_; }
 
  inline const mc_rbdyn::RobotFrame & frame() const noexcept
  {
    return static_cast<const mc_rbdyn::RobotFrame &>(frame_);
  }
 
protected:
  Eigen::Matrix3d h_ = Eigen::Matrix3d::Zero();
 
  rbd::Jacobian jac_;
  rbd::Blocks blocks_;
 
  Eigen::MatrixXd jacTmp_;
 
  tvm::internal::MatrixWithProperties jacobian_;
  void updateJacobian();
 
  sva::MotionVecd normalAcceleration_;
  void updateNormalAcceleration();
 
  tvm::internal::MatrixWithProperties jacDot_;
  void updateJDot();
};
 
} // namespace mc_tvm