GazeFunction.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>
 
#include <optional>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI GazeFunction : tvm::function::abstract::Function
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  SET_UPDATES(GazeFunction, Value, Velocity, Jacobian, NormalAcceleration)
 
  
  GazeFunction(const mc_rbdyn::RobotFrame & frame);
 
  inline void reset() { pointRef_ = point_; }
 
  void estimate(const Eigen::Vector2d & point, std::optional<double> depth = std::nullopt);
 
  void estimate(const Eigen::Vector3d & point);
 
  inline void target(const Eigen::Vector2d & ref) noexcept { pointRef_ = ref; }
 
  inline const Eigen::Vector2d & target() const noexcept { return pointRef_; }
 
  inline const mc_rbdyn::RobotFrame & frame() const noexcept { return *frame_; }
 
protected:
  mc_rbdyn::ConstRobotFramePtr frame_;
  mc_tvm::RobotFrame & tvm_frame_;
  rbd::Jacobian frameJac_;
 
  /* Inputs to the function */
  Eigen::Vector2d point_ = Eigen::Vector2d::Zero();
  Eigen::Vector2d pointRef_ = Eigen::Vector2d::Zero();
  double depthEstimate_ = 1.0;
 
  /* Computation intermediates */
  Eigen::Matrix<double, 2, 6> L_img_;
  Eigen::Matrix<double, 6, 1> surfaceVelocity_;
  Eigen::Matrix<double, 1, 6> L_Z_dot_;
  Eigen::Matrix<double, 2, 6> L_img_dot_;
 
  /* TVM update methods */
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  Eigen::MatrixXd shortJacMat_;
  Eigen::MatrixXd jacMat_;
  void updateNormalAcceleration();
 
  void setDepthEstimate(double depth);
};
 
} // namespace mc_tvm