3#ifdef ALIGATOR_WITH_PINOCCHIO
8#ifdef ALIGATOR_PINOCCHIO_V3
15using context::ConstVectorRef;
21using RigidConstraintModel =
22 pinocchio::RigidConstraintModelTpl<context::Scalar, 0>;
23using RigidConstraintData =
24 pinocchio::RigidConstraintDataTpl<context::Scalar, 0>;
26using RigidConstraintModelVector =
27 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel);
28using RigidConstraintDataVector =
29 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData);
31void exposeContactForce() {
32 using ContactForceResidual = ContactForceResidualTpl<Scalar>;
33 using ContactForceData = ContactForceDataTpl<Scalar>;
35 using MultibodyWrenchConeResidual = MultibodyWrenchConeResidualTpl<Scalar>;
36 using MultibodyWrenchConeData = MultibodyWrenchConeDataTpl<Scalar>;
38 bp::class_<ContactForceResidual, bp::bases<StageFunction>>(
39 "ContactForceResidual",
40 "A residual function :math:`r(x) = v_{j,xy} e^{-s z_j}` where :math:`j` "
41 "is a given frame index.",
43 .def(bp::init<
int, PinModel,
const context::MatrixXs &,
44 const RigidConstraintModelVector &,
45 const pinocchio::ProximalSettingsTpl<Scalar> &,
46 const context::Vector6s &,
int>(
47 bp::args(
"self",
"ndx",
"model",
"actuation_matrix",
48 "constraint_models",
"prox_settings",
"fref",
"contact_id")))
49 .def(FrameAPIVisitor<ContactForceResidual>())
50 .def(
"getReference", &ContactForceResidual::getReference,
51 bp::args(
"self"), bp::return_internal_reference<>(),
52 "Get the target force.")
53 .def(
"setReference", &ContactForceResidual::setReference,
54 bp::args(
"self",
"fnew"),
"Set the target force.")
55 .def_readwrite(
"constraint_models",
56 &ContactForceResidual::constraint_models_);
58 bp::class_<ContactForceData, bp::bases<StageFunctionData>>(
"ContactForceData",
60 .def_readwrite(
"tau", &ContactForceData::tau_)
61 .def_readwrite(
"pin_data", &ContactForceData::pin_data_)
62 .def_readwrite(
"constraint_datas", &ContactForceData::constraint_datas_);
64 bp::class_<MultibodyWrenchConeResidual, bp::bases<StageFunction>>(
65 "MultibodyWrenchConeResidual",
"A residual function :math:`r(x) = Af` ",
67 .def(bp::init<
int, PinModel,
const context::MatrixXs &,
68 const RigidConstraintModelVector &,
69 const pinocchio::ProximalSettingsTpl<Scalar> &,
const int,
70 const double,
const double,
const double>(bp::args(
71 "self",
"ndx",
"model",
"actuation_matrix",
"constraint_models",
72 "prox_settings",
"contact_id",
"mu",
"half_length",
"half_width")))
73 .def(FrameAPIVisitor<MultibodyWrenchConeResidual>())
74 .def_readwrite(
"constraint_models",
75 &MultibodyWrenchConeResidual::constraint_models_);
77 bp::class_<MultibodyWrenchConeData, bp::bases<StageFunctionData>>(
78 "MultibodyWrenchConeData", bp::no_init)
79 .def_readwrite(
"tau", &MultibodyWrenchConeData::tau_)
80 .def_readwrite(
"pin_data", &MultibodyWrenchConeData::pin_data_)
81 .def_readwrite(
"constraint_datas",
82 &MultibodyWrenchConeData::constraint_datas_);
pinocchio::ModelTpl< Scalar, Options > PinModel
StageFunctionTpl< Scalar > StageFunction
proxsuite::nlp::MultibodyPhaseSpace< Scalar > MultibodyPhaseSpace
StageFunctionDataTpl< Scalar > StageFunctionData