3#ifdef ALIGATOR_WITH_PINOCCHIO
12#ifdef ALIGATOR_PINOCCHIO_V3
18using context::ConstMatrixRef;
19using context::ConstVectorRef;
30#ifdef ALIGATOR_PINOCCHIO_V3
31void exposeContactForce();
33void exposeCenterOfMassFunctions();
34void exposeFrameFunctions();
35void exposeGravityCompensation();
41void exposeFrameFunctions() {
44 using SE3 = pinocchio::SE3Tpl<Scalar>;
45 using Motion = pinocchio::MotionTpl<Scalar>;
47 using FramePlacement = FramePlacementResidualTpl<Scalar>;
48 using FramePlacementData = FramePlacementDataTpl<Scalar>;
50 using FrameVelocity = FrameVelocityResidualTpl<Scalar>;
51 using FrameVelocityData = FrameVelocityDataTpl<Scalar>;
53 using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
54 using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
56 using FrameCollision = FrameCollisionResidualTpl<Scalar>;
57 using FrameCollisionData = FrameCollisionDataTpl<Scalar>;
59 using pinocchio::GeometryModel;
61 if (!eigenpy::check_registration<shared_ptr<PinData>>())
62 bp::register_ptr_to_python<shared_ptr<PinData>>();
66 bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
67 "FramePlacementResidual",
"Frame placement residual function.",
68 bp::init<int, int, const PinModel &, const SE3 &, pinocchio::FrameIndex>(
69 (
"self"_a,
"ndx",
"nu",
"model",
"p_ref",
"id")))
72 .def(
"getReference", &FramePlacement::getReference,
"self"_a,
73 bp::return_internal_reference<>(),
"Get the target frame in SE3.")
74 .def(
"setReference", &FramePlacement::setReference, (
"self"_a,
"p_new"),
75 "Set the target frame in SE3.");
77 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
79 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
80 "FramePlacementData",
"Data struct for FramePlacementResidual.",
82 .def_readonly(
"rMf", &FramePlacementData::rMf_,
"Frame placement error.")
83 .def_readonly(
"rJf", &FramePlacementData::rJf_)
84 .def_readonly(
"fJf", &FramePlacementData::fJf_)
85 .def_readonly(
"pin_data", &FramePlacementData::pin_data_,
86 "Pinocchio data struct.");
88 bp::class_<FrameVelocity, bp::bases<UnaryFunction>>(
89 "FrameVelocityResidual",
"Frame velocity residual function.",
90 bp::init<int, int,
const PinModel &,
const Motion &,
91 pinocchio::FrameIndex, pinocchio::ReferenceFrame>(
92 (
"self"_a,
"ndx",
"nu",
"model",
"v_ref",
"id",
"reference_frame")))
95 .def(
"getReference", &FrameVelocity::getReference,
"self"_a,
96 bp::return_internal_reference<>(),
"Get the target frame velocity.")
97 .def(
"setReference", &FrameVelocity::setReference, (
"self"_a,
"v_new"),
98 "Set the target frame velocity.");
100 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
102 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
103 "FrameVelocityData",
"Data struct for FrameVelocityResidual.",
105 .def_readonly(
"pin_data", &FrameVelocityData::pin_data_,
106 "Pinocchio data struct.");
108 bp::class_<FrameTranslation, bp::bases<UnaryFunction>>(
109 "FrameTranslationResidual",
"Frame placement residual function.",
110 bp::init<int, int,
const PinModel &,
const context::Vector3s &,
111 pinocchio::FrameIndex>(
112 (
"self"_a,
"ndx",
"nu",
"model",
"p_ref",
"id")))
115 .def(
"getReference", &FrameTranslation::getReference,
"self"_a,
116 bp::return_internal_reference<>(),
117 "Get the target frame translation.")
118 .def(
"setReference", &FrameTranslation::setReference, (
"self"_a,
"p_new"),
119 "Set the target frame translation.");
121 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
123 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
124 "FrameTranslationData",
"Data struct for FrameTranslationResidual.",
126 .def_readonly(
"fJf", &FrameTranslationData::fJf_)
127 .def_readonly(
"pin_data", &FrameTranslationData::pin_data_,
128 "Pinocchio data struct.");
130 bp::class_<FrameCollision, bp::bases<UnaryFunction>>(
131 "FrameCollisionResidual",
"Frame collision residual function.",
132 bp::init<int, int,
const PinModel &,
const GeometryModel &,
133 pinocchio::PairIndex>(bp::args(
"self",
"ndx",
"nu",
"model",
134 "geom_model",
"frame_pair_id")))
138 bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
140 bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
141 "FrameCollisionData",
"Data struct for FrameCollisionResidual.",
143 .def_readonly(
"pin_data", &FrameCollisionData::pin_data_,
144 "Pinocchio data struct.")
145 .def_readonly(
"geom_data", &FrameCollisionData::geometry_,
146 "Geometry data struct.");
149#ifdef ALIGATOR_PINOCCHIO_V3
150auto underactuatedConstraintInvDyn_proxy(
151 const PinModel &model, PinData &data,
const ConstVectorRef &q,
152 const ConstVectorRef &v,
const ConstMatrixRef &actMatrix,
153 const StdVectorEigenAligned<context::RCM> &constraint_models,
154 StdVectorEigenAligned<context::RCD> &constraint_datas) {
155 long nu = actMatrix.cols();
157 for (
size_t k = 0; k < constraint_models.size(); ++k) {
158 d += (int)constraint_models[k].size();
160 context::VectorXs out(nu + d);
162 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
164 return bp::make_tuple((context::VectorXs)out.head(nu),
165 (context::VectorXs)out.tail(d));
169void exposePinocchioFunctions() {
170 exposeFrameFunctions();
172#ifdef ALIGATOR_PINOCCHIO_V3
173 exposeContactForce();
175 exposeCenterOfMassFunctions();
176 exposeGravityCompensation();
178#ifdef ALIGATOR_PINOCCHIO_V3
179 bp::def(
"underactuatedConstrainedInverseDynamics",
180 underactuatedConstraintInvDyn_proxy,
181 (
"model"_a,
"data",
"q",
"v",
"actMatrix",
"constraint_model",
183 "Compute the gravity-compensating torque for a pinocchio Model under "
184 "a rigid constraint.");
ManifoldAbstractTpl< Scalar > Manifold
pinocchio::ModelTpl< Scalar, Options > PinModel
StageFunctionTpl< Scalar > StageFunction
UnaryFunctionTpl< Scalar > UnaryFunction
pinocchio::DataTpl< Scalar, Options > PinData
const PolymorphicMultiBaseVisitor< UnaryFunction, StageFunction > unary_visitor
void underactuatedConstrainedInverseDynamics(const ModelTpl< Scalar, Options > &model, DataTpl< Scalar, Options > &data, const Eigen::MatrixBase< ConfigType > &q, Eigen::MatrixBase< VelType > const &v, const Eigen::MatrixBase< MatrixType > &actMatrix, const StdVectorEigenAligned< RigidConstraintModelTpl< Scalar, Options > > &constraint_models, StdVectorEigenAligned< RigidConstraintDataTpl< Scalar, Options > > &constraint_datas, const Eigen::MatrixBase< OutType > &res_)