3#ifdef ALIGATOR_WITH_PINOCCHIO
10#ifdef ALIGATOR_PINOCCHIO_V3
16using context::ConstMatrixRef;
17using context::ConstVectorRef;
23#ifdef ALIGATOR_PINOCCHIO_V3
24void exposeContactForce();
26void exposeCenterOfMassFunctions();
27void exposeFrameFunctions() {
31 using SE3 = pinocchio::SE3Tpl<Scalar>;
32 using Motion = pinocchio::MotionTpl<Scalar>;
34 using FramePlacement = FramePlacementResidualTpl<Scalar>;
35 using FramePlacementData = FramePlacementDataTpl<Scalar>;
37 using FrameVelocity = FrameVelocityResidualTpl<Scalar>;
38 using FrameVelocityData = FrameVelocityDataTpl<Scalar>;
40 using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
41 using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
43 bp::register_ptr_to_python<shared_ptr<PinData>>();
45 bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
46 "FramePlacementResidual",
"Frame placement residual function.",
47 bp::init<int, int, shared_ptr<PinModel>,
const SE3 &,
48 pinocchio::FrameIndex>(
49 bp::args(
"self",
"ndx",
"nu",
"model",
"p_ref",
"id")))
50 .def(FrameAPIVisitor<FramePlacement>())
51 .def(
"getReference", &FramePlacement::getReference, bp::args(
"self"),
52 bp::return_internal_reference<>(),
"Get the target frame in SE3.")
53 .def(
"setReference", &FramePlacement::setReference,
54 bp::args(
"self",
"p_new"),
"Set the target frame in SE3.");
56 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
58 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
59 "FramePlacementData",
"Data struct for FramePlacementResidual.",
61 .def_readonly(
"rMf", &FramePlacementData::rMf_,
"Frame placement error.")
62 .def_readonly(
"rJf", &FramePlacementData::rJf_)
63 .def_readonly(
"fJf", &FramePlacementData::fJf_)
64 .def_readonly(
"pin_data", &FramePlacementData::pin_data_,
65 "Pinocchio data struct.");
67 bp::class_<FrameVelocity, bp::bases<UnaryFunction>>(
68 "FrameVelocityResidual",
"Frame velocity residual function.",
69 bp::init<int, int, shared_ptr<PinModel>,
const Motion &,
70 pinocchio::FrameIndex, pinocchio::ReferenceFrame>(bp::args(
71 "self",
"ndx",
"nu",
"model",
"v_ref",
"id",
"reference_frame")))
72 .def(FrameAPIVisitor<FrameVelocity>())
73 .def(
"getReference", &FrameVelocity::getReference, bp::args(
"self"),
74 bp::return_internal_reference<>(),
"Get the target frame velocity.")
75 .def(
"setReference", &FrameVelocity::setReference,
76 bp::args(
"self",
"v_new"),
"Set the target frame velocity.");
78 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
80 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
81 "FrameVelocityData",
"Data struct for FrameVelocityResidual.",
83 .def_readonly(
"pin_data", &FrameVelocityData::pin_data_,
84 "Pinocchio data struct.");
86 bp::class_<FrameTranslation, bp::bases<UnaryFunction>>(
87 "FrameTranslationResidual",
"Frame placement residual function.",
88 bp::init<int, int, shared_ptr<PinModel>,
const context::Vector3s &,
89 pinocchio::FrameIndex>(
90 bp::args(
"self",
"ndx",
"nu",
"model",
"p_ref",
"id")))
91 .def(FrameAPIVisitor<FrameTranslation>())
92 .def(
"getReference", &FrameTranslation::getReference, bp::args(
"self"),
93 bp::return_internal_reference<>(),
94 "Get the target frame translation.")
95 .def(
"setReference", &FrameTranslation::setReference,
96 bp::args(
"self",
"p_new"),
"Set the target frame translation.");
98 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
100 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
101 "FrameTranslationData",
"Data struct for FrameTranslationResidual.",
103 .def_readonly(
"fJf", &FrameTranslationData::fJf_)
104 .def_readonly(
"pin_data", &FrameTranslationData::pin_data_,
105 "Pinocchio data struct.");
108#ifdef ALIGATOR_PINOCCHIO_V3
109auto underactuatedConstraintInvDyn_proxy(
110 const PinModel &model, PinData &data,
const ConstVectorRef &q,
111 const ConstVectorRef &v,
const ConstMatrixRef &actMatrix,
112 const StdVectorEigenAligned<context::RCM> &constraint_models,
113 StdVectorEigenAligned<context::RCD> &constraint_datas) {
114 long nu = actMatrix.cols();
116 for (
size_t k = 0; k < constraint_models.size(); ++k) {
117 d += (int)constraint_models[k].size();
119 context::VectorXs out(nu + d);
121 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
123 return bp::make_tuple((context::VectorXs)out.head(nu),
124 (context::VectorXs)out.tail(d));
128void exposePinocchioFunctions() {
129 exposeFrameFunctions();
131#ifdef ALIGATOR_PINOCCHIO_V3
132 exposeContactForce();
134 exposeCenterOfMassFunctions();
136#ifdef ALIGATOR_PINOCCHIO_V3
137 bp::def(
"underactuatedConstrainedInverseDynamics",
138 underactuatedConstraintInvDyn_proxy,
139 bp::args(
"model",
"data",
"q",
"v",
"actMatrix",
"constraint_model",
141 "Compute the gravity-compensating torque for a pinocchio Model under "
142 "a rigid constraint.");
ManifoldAbstractTpl< Scalar > Manifold
pinocchio::ModelTpl< Scalar, Options > PinModel
UnaryFunctionTpl< Scalar > UnaryFunction
pinocchio::DataTpl< Scalar, Options > PinData
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_)