8#include <Eigen/src/Core/util/Constants.h>
10#include <pinocchio/algorithm/proximal.hpp>
19template <
typename _Scalar>
26 using Model = pinocchio::ModelTpl<Scalar>;
27 using SE3 = pinocchio::SE3Tpl<Scalar>;
44 const MatrixXs &actuation,
48 const std::string &contact_name)
49 :
Base(ndx, (int)actuation.cols(), (int)fref.size())
56 if (model.nv != actuation.rows()) {
57 ALIGATOR_DOMAIN_ERROR(
"Actuation matrix should have number of rows = "
58 "model.nv ({:d} and {:d}).",
59 actuation.rows(), model.nv);
62 for (std::size_t i = 0; i < constraint_models.size(); i++) {
63 if (constraint_models[i].name == contact_name) {
69 "Contact name is not included in constraint models");
76 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
83 return std::make_shared<Data>(
this);
87template <
typename Scalar>
89 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
91 using PinData = pinocchio::DataTpl<Scalar>;
109#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
110#include "aligator/modelling/multibody/contact-force.txx"
#define ALIGATOR_RUNTIME_ERROR(...)
Base definitions for ternary functions.
StageFunctionDataTpl(const int ndx, const int nu, const int nr)
StageFunctionDataTpl< Scalar > Data
StageFunctionTpl(const int ndx, const int nu, const int nr)
Typedefs for math (Eigen vectors, matrices) depending on scalar type.