DynamicFunction.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_rbdyn/fwd.h>
 
#include <tvm/function/abstract/LinearFunction.h>
 
#include <RBDyn/Jacobian.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFunction
{
public:
  using Output = tvm::function::abstract::LinearFunction::Output;
  DISABLE_OUTPUTS(Output::JDot)
  SET_UPDATES(DynamicFunction, Jacobian, B)
 
  
  DynamicFunction(const mc_rbdyn::Robot & robot);
 
  const tvm::VariableVector & addContact(const mc_rbdyn::RobotFrame & frame,
                                         std::vector<sva::PTransformd> points,
                                         double dir);
 
  void removeContact(const mc_rbdyn::RobotFrame & frame);
 
  sva::ForceVecd contactForce(const mc_rbdyn::RobotFrame & f) const;
 
protected:
  void updateb();
 
  const mc_rbdyn::Robot & robot_;
 
  struct ForceContact
  {
    ForceContact(const mc_rbdyn::RobotFrame & frame, std::vector<sva::PTransformd> points, double dir);
 
    void updateJacobians(DynamicFunction & parent);
 
    sva::ForceVecd force() const;
 
    mc_rbdyn::ConstRobotFramePtr frame_;
 
    tvm::VariableVector forces_;
 
    std::vector<sva::PTransformd> points_;
 
    double dir_;
 
    rbd::Jacobian jac_;
    rbd::Blocks blocks_;
 
    Eigen::MatrixXd force_jac_;
    Eigen::MatrixXd full_jac_;
  };
  std::vector<ForceContact> contacts_;
 
  std::vector<ForceContact>::const_iterator findContact(const mc_rbdyn::RobotFrame & frame) const;
 
  void updateJacobian();
};
 
using DynamicFunctionPtr = std::shared_ptr<DynamicFunction>;
 
} // namespace mc_tvm