JointsSelectorFunction.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/Function.h>
 
namespace mc_tvm
{
 
// FIXME Needs to implement active DoF selection
 
class MC_TVM_DLLAPI JointsSelectorFunction : public tvm::function::abstract::Function
{
public:
  SET_UPDATES(JointsSelectorFunction, Jacobian, JDot)
 
  using Function = tvm::function::abstract::Function;
 
  static std::unique_ptr<JointsSelectorFunction> ActiveJoints(Function * f,
                                                              const mc_rbdyn::Robot & robot,
                                                              const std::vector<std::string> & activeJoints);
 
  static std::unique_ptr<JointsSelectorFunction> InactiveJoints(Function * f,
                                                                const mc_rbdyn::Robot & robot,
                                                                const std::vector<std::string> & inactiveJoints);
 
  inline const Eigen::VectorXd & value() const override { return f_->value(); }
 
  inline const Eigen::VectorXd & velocity() const override { return f_->velocity(); }
 
  inline const Eigen::VectorXd & normalAcceleration() const override { return f_->normalAcceleration(); }
 
protected:
  JointsSelectorFunction(Function * f,
                         tvm::VariablePtr variable,
                         const std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> & activeIndex);
 
protected:
  void updateJacobian();
  void updateJDot();
 
  Function * f_;
  tvm::VariablePtr variable_;
  std::vector<std::pair<Eigen::DenseIndex, Eigen::DenseIndex>> activeIndex_;
};
 
} // namespace mc_tvm