VectorOrientationFunction.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_rbdyn/fwd.h>
 
#include <tvm/function/abstract/Function.h>
 
#include <RBDyn/Jacobian.h>
 
namespace mc_tvm
{
 
class MC_TVM_DLLAPI VectorOrientationFunction : public tvm::function::abstract::Function
{
public:
  SET_UPDATES(VectorOrientationFunction, Value, Velocity, Jacobian, NormalAcceleration)
 
  
  VectorOrientationFunction(const mc_rbdyn::RobotFrame & frame, const Eigen::Vector3d & frameVector);
 
  void reset();
 
  inline const Eigen::Vector3d & target() const noexcept { return target_; }
 
  inline void target(const Eigen::Vector3d & target) noexcept { target_ = target; }
 
  inline const Eigen::Vector3d & frameVector() const noexcept { return frameVectorIn_; }
 
  void frameVector(const Eigen::Vector3d & frameVector) noexcept;
 
  inline const Eigen::Vector3d & actual() const noexcept { return actualVector_; }
 
  inline const Eigen::Vector3d & refVel() const noexcept { return refVel_; }
 
  inline void refVel(const Eigen::Vector3d & refVel) noexcept { refVel_ = refVel; }
 
  inline const Eigen::Vector3d & refAccel() const noexcept { return refAccel_; }
 
  inline void refAccel(const Eigen::Vector3d & refAccel) noexcept { refAccel_ = refAccel; }
 
  inline const mc_rbdyn::RobotFrame & frame() const noexcept { return *frame_; }
 
protected:
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  void updateNormalAcceleration();
 
  mc_rbdyn::ConstRobotFramePtr frame_;
  mc_tvm::RobotFrame & body_frame_;
 
  Eigen::Vector3d frameVectorIn_;
 
  rbd::Jacobian jac_;
  Eigen::Vector3d actualVector_;
  Eigen::Vector3d bodyVector_;
  Eigen::Matrix3d bodyVectorHat_;
  Eigen::MatrixXd fullJacobian_;
  Eigen::Matrix3d E_0_b_;
  Eigen::Vector3d w_b_b_;
 
  Eigen::Vector3d target_;
  Eigen::Vector3d refVel_;
  Eigen::Vector3d refAccel_;
};
 
} // namespace mc_tvm