CoMInConvexFunction.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_tvm/CoM.h>
#include <mc_tvm/Robot.h>
 
#include <mc_rbdyn/fwd.h>
 
#include <tvm/function/abstract/Function.h>
#include <tvm/geometry/Plane.h>
 
namespace mc_tvm
{
 
struct MC_TVM_DLLAPI CoMInConvexFunction : public tvm::function::abstract::Function
{
public:
  using Output = tvm::function::abstract::Function::Output;
  DISABLE_OUTPUTS(Output::JDot)
  SET_UPDATES(CoMInConvexFunction, Value, Velocity, Jacobian, NormalAcceleration)
 
  
  CoMInConvexFunction(const mc_rbdyn::Robot & robot);
 
  void addPlane(tvm::geometry::PlanePtr plane);
 
  void reset();
 
  inline const mc_rbdyn::Robot & robot() const noexcept { return com_.robot().robot(); }
 
  inline const std::vector<tvm::geometry::PlanePtr> & planes() noexcept { return planes_; }
 
  inline tvm::geometry::Plane & plane(size_t i) noexcept
  {
    assert(i < planes_.size() && planes_[i]);
    return *planes_[i];
  }
 
  Eigen::VectorXd & selector() noexcept { return selector_; }
 
  const Eigen::VectorXd & selector() const noexcept { return selector_; }
 
protected:
  void updateValue();
  void updateVelocity();
  void updateJacobian();
  void updateNormalAcceleration();
 
  mc_tvm::CoM & com_;
 
  std::vector<tvm::geometry::PlanePtr> planes_;
 
  Eigen::VectorXd selector_;
};
 
using CoMInConvexFunctionPtr = std::shared_ptr<CoMInConvexFunction>;
 
} // namespace mc_tvm