aligator 0.17.1
A versatile and efficient C++ library for real-time constrained 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"
5#include "aligator/python/modelling/multibody-utils.hpp"
6
7#include "aligator/modelling/multibody/frame-placement.hpp"
8#include "aligator/modelling/multibody/frame-equality.hpp"
9#include "aligator/modelling/multibody/frame-velocity.hpp"
10#include "aligator/modelling/multibody/frame-translation.hpp"
11#include "aligator/modelling/multibody/frame-collision.hpp"
12
13#include "aligator/modelling/multibody/constrained-rnea.hpp"
14
15#include <eigenpy/deprecation-policy.hpp>
16
17namespace aligator {
18namespace python {
19using context::ConstMatrixRef;
20using context::ConstVectorRef;
21using context::PinData;
22using context::PinModel;
23using context::StageFunction;
24using context::UnaryFunction;
25using eigenpy::deprecated_member;
26
27//
28// FORWARD DECLARATIONS
29//
30
31void exposeFlyHigh();
32void exposeContactForce();
33void exposeCenterOfMassFunctions();
34void exposeFrameFunctions();
35void exposeGravityCompensation();
36
37//
38// DEFINITIONS
39//
40
41void exposeFrameFunctions() {
42 using context::Manifold;
43 using context::Scalar;
44 using SE3 = pinocchio::SE3Tpl<Scalar>;
45 using Motion = pinocchio::MotionTpl<Scalar>;
46
47 using FrameEquality = FrameEqualityResidualTpl<Scalar>;
48 using FrameEqualityData = FrameEqualityDataTpl<Scalar>;
49
50 using FramePlacement = FramePlacementResidualTpl<Scalar>;
51 using FramePlacementData = FramePlacementDataTpl<Scalar>;
52
53 using FrameVelocity = FrameVelocityResidualTpl<Scalar>;
54 using FrameVelocityData = FrameVelocityDataTpl<Scalar>;
55
56 using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
57 using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
58
59 using FrameCollision = FrameCollisionResidualTpl<Scalar>;
60 using FrameCollisionData = FrameCollisionDataTpl<Scalar>;
61
62 using pinocchio::GeometryModel;
63
64 if (!eigenpy::check_registration<shared_ptr<PinData>>())
65 bp::register_ptr_to_python<shared_ptr<PinData>>();
66
67 PolymorphicMultiBaseVisitor<UnaryFunction, StageFunction> unary_visitor;
68
69 bp::class_<FrameEquality, bp::bases<UnaryFunction>>(
70 "FrameEqualityResidual", "Frame equality residual function.",
71 bp::init<int, int, const PinModel &, pinocchio::FrameIndex,
72 pinocchio::FrameIndex>(
73 ("self"_a, "ndx", "nu", "model", "frame_id1", "frame_id2")))
74 .def(unary_visitor)
75 .def("getReference", &FrameEquality::getReference, "self"_a,
76 bp::return_internal_reference<>(),
77 "Get the target transform between frame1 and frame2.")
78 .def("setReference", &FrameEquality::setReference, ("self"_a, "p_new"),
79 "Set the target transform between frame1 and frame2.")
80 .add_property("frame1_id", &FrameEquality::getFrame1Id,
81 &FrameEquality::setFrame1Id,
82 "Get the Pinocchio ID of frame1.")
83 .add_property("frame2_id", &FrameEquality::getFrame2Id,
84 &FrameEquality::setFrame2Id,
85 "Get the Pinocchio ID of frame2.");
86
87 bp::register_ptr_to_python<shared_ptr<FrameEqualityData>>();
88
89 bp::class_<FrameEqualityData, bp::bases<context::StageFunctionData>>(
90 "FrameEqualityData", "Data struct for FrameEqualityResidual.",
91 bp::no_init)
92 .def_readonly("RMf2", &FrameEqualityData::RMf2_, "Frame placement error.")
93 .def_readonly("RJlog6f2", &FrameEqualityData::RJlog6f2_)
94 .def_readonly("wJf1", &FrameEqualityData::wJf1_)
95 .def_readonly("wJf2", &FrameEqualityData::wJf2_)
96 .def_readonly("pin_data", &FrameEqualityData::pin_data_,
97 "Pinocchio data struct.");
98
99 bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
100 "FramePlacementResidual", "Frame placement residual function.",
101 bp::init<int, int, const PinModel &, const SE3 &, pinocchio::FrameIndex>(
102 ("self"_a, "ndx", "nu", "model", "p_ref", "id")))
103 .def(FrameAPIVisitor<FramePlacement>())
104 .def(unary_visitor)
105 .def("getReference", &FramePlacement::getReference, "self"_a,
106 bp::return_internal_reference<>(), "Get the target frame in SE3.")
107 .def("setReference", &FramePlacement::setReference, ("self"_a, "p_new"),
108 "Set the target frame in SE3.");
109
110 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
111
112 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
113 "FramePlacementData", "Data struct for FramePlacementResidual.",
114 bp::no_init)
115 .def_readonly("rMf", &FramePlacementData::rMf_, "Frame placement error.")
116 .def_readonly("rJf", &FramePlacementData::rJf_)
117 .def_readonly("fJf", &FramePlacementData::fJf_)
118 .def_readonly("pin_data", &FramePlacementData::pin_data_,
119 "Pinocchio data struct.");
120
121 bp::class_<FrameVelocity, bp::bases<UnaryFunction>>(
122 "FrameVelocityResidual", "Frame velocity residual function.",
123 bp::init<int, int, const PinModel &, const Motion &,
124 pinocchio::FrameIndex, pinocchio::ReferenceFrame>(
125 ("self"_a, "ndx", "nu", "model", "v_ref", "id", "reference_frame")))
126 .def(FrameAPIVisitor<FrameVelocity>())
127 .def(unary_visitor)
128 .def_readonly("pin_model", &FrameVelocity::pin_model_)
129 .def_readwrite("vref", &FrameVelocity::vref_)
130 .def_readwrite("type", &FrameVelocity::type_)
131 .def("getReference", &FrameVelocity::getReference, "self"_a,
132 bp::return_internal_reference<>(),
133 deprecated_member<>("Get the target frame velocity."))
134 .def("setReference", &FrameVelocity::setReference, ("self"_a, "v_new"),
135 deprecated_member<>("Set the target frame velocity."));
136
137 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
138
139 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
140 "FrameVelocityData", "Data struct for FrameVelocityResidual.",
141 bp::no_init)
142 .def_readonly("pin_data", &FrameVelocityData::pin_data_,
143 "Pinocchio data struct.");
144
145 bp::class_<FrameTranslation, bp::bases<UnaryFunction>>(
146 "FrameTranslationResidual", "Frame placement residual function.",
147 bp::init<int, int, const PinModel &, const context::Vector3s &,
148 pinocchio::FrameIndex>(
149 ("self"_a, "ndx", "nu", "model", "p_ref", "id")))
150 .def(FrameAPIVisitor<FrameTranslation>())
151 .def(unary_visitor)
152 .def("getReference", &FrameTranslation::getReference, "self"_a,
153 bp::return_internal_reference<>(),
154 "Get the target frame translation.")
155 .def("setReference", &FrameTranslation::setReference, ("self"_a, "p_new"),
156 "Set the target frame translation.");
157
158 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
159
160 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
161 "FrameTranslationData", "Data struct for FrameTranslationResidual.",
162 bp::no_init)
163 .def_readonly("fJf", &FrameTranslationData::fJf_)
164 .def_readonly("pin_data", &FrameTranslationData::pin_data_,
165 "Pinocchio data struct.");
166
167 bp::class_<FrameCollision, bp::bases<UnaryFunction>>(
168 "FrameCollisionResidual", "Frame collision residual function.",
169 bp::init<int, int, const PinModel &, const GeometryModel &,
170 pinocchio::PairIndex>(
171 ("self"_a, "ndx", "nu", "model", "geom_model", "frame_pair_id")))
172 .def(FrameAPIVisitor<FrameCollision>())
173 .def(unary_visitor);
174
175 bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
176
177 bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
178 "FrameCollisionData", "Data struct for FrameCollisionResidual.",
179 bp::no_init)
180 .def_readonly("pin_data", &FrameCollisionData::pin_data_,
181 "Pinocchio data struct.")
182 .def_readonly("geom_data", &FrameCollisionData::geom_data,
183 "Geometry data struct.");
184}
185
186auto underactuatedConstraintInvDyn_proxy(
187 const PinModel &model, PinData &data, const ConstVectorRef &q,
188 const ConstVectorRef &v, const ConstMatrixRef &actMatrix,
189 const StdVectorEigenAligned<context::RCM> &constraint_models,
190 StdVectorEigenAligned<context::RCD> &constraint_datas) {
191 long nu = actMatrix.cols();
192 int d = 0;
193 for (size_t k = 0; k < constraint_models.size(); ++k) {
194 d += (int)constraint_models[k].size();
195 }
196 context::VectorXs out(nu + d);
197 underactuatedConstrainedInverseDynamics(
198 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
199
200 return bp::make_tuple((context::VectorXs)out.head(nu),
201 (context::VectorXs)out.tail(d));
202}
203
204void exposePinocchioFunctions() {
205 exposeFrameFunctions();
206 exposeFlyHigh();
207 exposeContactForce();
208 exposeCenterOfMassFunctions();
209 exposeGravityCompensation();
210
211 bp::def("underactuatedConstrainedInverseDynamics",
212 underactuatedConstraintInvDyn_proxy,
213 ("model"_a, "data", "q", "v", "actMatrix", "constraint_model",
214 "constraint_data"),
215 "Compute the gravity-compensating torque for a pinocchio Model under "
216 "a rigid constraint.");
217}
218} // namespace python
219} // namespace aligator
220
221#endif
fwd.hpp
Forward declarations.
aligator::context::Manifold
ManifoldAbstractTpl< Scalar > Manifold
Definition context.hpp:14
aligator::context::PinModel
pinocchio::ModelTpl< Scalar, Options > PinModel
Definition fwd.hpp:21
aligator::context::StageFunction
StageFunctionTpl< Scalar > StageFunction
Definition context.hpp:17
aligator::context::UnaryFunction
UnaryFunctionTpl< Scalar > UnaryFunction
Definition context.hpp:18
aligator::context::PinData
pinocchio::DataTpl< Scalar, Options > PinData
Definition fwd.hpp:22
aligator::python
The Python bindings.
Definition blk-matrix.hpp:7
aligator::python::underactuatedConstraintInvDyn_proxy
auto underactuatedConstraintInvDyn_proxy(const PinModel &model, PinData &data, const ConstVectorRef &q, const ConstVectorRef &v, const ConstMatrixRef &actMatrix, const StdVectorEigenAligned< context::RCM > &constraint_models, StdVectorEigenAligned< context::RCD > &constraint_datas)
Definition expose-pinocchio-functions.cpp:186
aligator::python::unary_visitor
const PolymorphicMultiBaseVisitor< UnaryFunction, StageFunction > unary_visitor
Definition expose-function-ops.cpp:20
aligator
Main package namespace.
Definition action-model-wrap.hpp:14
aligator::StdVectorEigenAligned
std::vector< T, Eigen::aligned_allocator< T > > StdVectorEigenAligned
Typedef for vector with Eigen::aligned_allocator allocator type.
Definition fwd.hpp:170
aligator::underactuatedConstrainedInverseDynamics
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_)
Definition constrained-rnea.hpp:21
aligator::FrameEqualityResidualTpl
Residual enforcing equality between two Pinocchio frames.
Definition frame-equality.hpp:31