ContactFunction.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 ContactFunction : public tvm::function::abstract::Function
{
public:
  using Output = tvm::function::abstract::Function::Output;
  DISABLE_OUTPUTS(Output::JDot)
  SET_UPDATES(ContactFunction, Value, Derivatives)
 
  
  ContactFunction(const mc_rbdyn::Frame & f1,
                  const mc_rbdyn::Frame & f2,
                  const Eigen::Vector6d & dof = Eigen::Vector6d::Ones());
 
  inline const Eigen::Vector6d & dof() const noexcept { return dof_; }
 
  inline void dof(const Eigen::Vector6d & dof) noexcept { dof_ = dof; }
 
private:
  const Frame * f1_;
  const Frame * f2_;
  Eigen::Vector6d dof_;
 
  bool use_f1_ = false;
  rbd::Jacobian f1Jacobian_;
  bool use_f2_ = false;
  rbd::Jacobian f2Jacobian_;
  sva::PTransformd X_0_cf_;
  sva::PTransformd X_cf_f1_;
  sva::PTransformd X_cf_f2_;
 
  Eigen::MatrixXd jacTmp_;
  Eigen::MatrixXd jac_;
 
  void updateValue();
  // Derivatives update involve many intermediary that are used for all computations
  void updateDerivatives();
};
 
using ContactFunctionPtr = std::shared_ptr<mc_tvm::ContactFunction>;
 
} // namespace mc_tvm