aligator  0.12.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
 
Loading...
Searching...
No Matches
expose-pinocchio-functions.cpp
Go to the documentation of this file.
1
3#ifdef ALIGATOR_WITH_PINOCCHIO
4#include "aligator/fwd.hpp"
6
12#ifdef ALIGATOR_PINOCCHIO_V3
14#endif
15
16namespace aligator {
17namespace python {
18using context::ConstMatrixRef;
19using context::ConstVectorRef;
24
25//
26// FORWARD DECLARATIONS
27//
28
29void exposeFlyHigh();
30#ifdef ALIGATOR_PINOCCHIO_V3
31void exposeContactForce();
32#endif
33void exposeCenterOfMassFunctions();
34void exposeFrameFunctions();
35void exposeGravityCompensation();
36
37//
38// DEFINITIONS
39//
40
41void exposeFrameFunctions() {
43 using context::Scalar;
44 using SE3 = pinocchio::SE3Tpl<Scalar>;
45 using Motion = pinocchio::MotionTpl<Scalar>;
46
47 using FramePlacement = FramePlacementResidualTpl<Scalar>;
48 using FramePlacementData = FramePlacementDataTpl<Scalar>;
49
50 using FrameVelocity = FrameVelocityResidualTpl<Scalar>;
51 using FrameVelocityData = FrameVelocityDataTpl<Scalar>;
52
53 using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
54 using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
55
56 using FrameCollision = FrameCollisionResidualTpl<Scalar>;
57 using FrameCollisionData = FrameCollisionDataTpl<Scalar>;
58
59 using pinocchio::GeometryModel;
60
61 if (!eigenpy::check_registration<shared_ptr<PinData>>())
62 bp::register_ptr_to_python<shared_ptr<PinData>>();
63
65
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")))
71 .def(unary_visitor)
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.");
76
77 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
78
79 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
80 "FramePlacementData", "Data struct for FramePlacementResidual.",
81 bp::no_init)
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.");
87
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")))
94 .def(unary_visitor)
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.");
99
100 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
101
102 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
103 "FrameVelocityData", "Data struct for FrameVelocityResidual.",
104 bp::no_init)
105 .def_readonly("pin_data", &FrameVelocityData::pin_data_,
106 "Pinocchio data struct.");
107
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")))
114 .def(unary_visitor)
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.");
120
121 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
122
123 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
124 "FrameTranslationData", "Data struct for FrameTranslationResidual.",
125 bp::no_init)
126 .def_readonly("fJf", &FrameTranslationData::fJf_)
127 .def_readonly("pin_data", &FrameTranslationData::pin_data_,
128 "Pinocchio data struct.");
129
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")))
136 .def(unary_visitor);
137
138 bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
139
140 bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
141 "FrameCollisionData", "Data struct for FrameCollisionResidual.",
142 bp::no_init)
143 .def_readonly("pin_data", &FrameCollisionData::pin_data_,
144 "Pinocchio data struct.")
145 .def_readonly("geom_data", &FrameCollisionData::geometry_,
146 "Geometry data struct.");
147}
148
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();
156 int d = 0;
157 for (size_t k = 0; k < constraint_models.size(); ++k) {
158 d += (int)constraint_models[k].size();
159 }
160 context::VectorXs out(nu + d);
162 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
163
164 return bp::make_tuple((context::VectorXs)out.head(nu),
165 (context::VectorXs)out.tail(d));
166}
167#endif
168
169void exposePinocchioFunctions() {
170 exposeFrameFunctions();
171 exposeFlyHigh();
172#ifdef ALIGATOR_PINOCCHIO_V3
173 exposeContactForce();
174#endif
175 exposeCenterOfMassFunctions();
176 exposeGravityCompensation();
177
178#ifdef ALIGATOR_PINOCCHIO_V3
179 bp::def("underactuatedConstrainedInverseDynamics",
180 underactuatedConstraintInvDyn_proxy,
181 ("model"_a, "data", "q", "v", "actMatrix", "constraint_model",
182 "constraint_data"),
183 "Compute the gravity-compensating torque for a pinocchio Model under "
184 "a rigid constraint.");
185#endif
186}
187} // namespace python
188} // namespace aligator
189
190#endif
Forward declarations.
ManifoldAbstractTpl< Scalar > Manifold
Definition context.hpp:14
pinocchio::ModelTpl< Scalar, Options > PinModel
Definition context.hpp:9
StageFunctionTpl< Scalar > StageFunction
Definition context.hpp:16
UnaryFunctionTpl< Scalar > UnaryFunction
Definition context.hpp:17
pinocchio::DataTpl< Scalar, Options > PinData
Definition context.hpp:10
The Python bindings.
Definition blk-matrix.hpp:5
const PolymorphicMultiBaseVisitor< UnaryFunction, StageFunction > unary_visitor
Main package namespace.
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_)