aligator  0.16.0
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"
6
11
13
14#include <eigenpy/deprecation-policy.hpp>
15
16namespace aligator {
17namespace python {
18using context::ConstMatrixRef;
19using context::ConstVectorRef;
24using eigenpy::deprecated_member;
25
26//
27// FORWARD DECLARATIONS
28//
29
30void exposeFlyHigh();
35
36//
37// DEFINITIONS
38//
39
42 using context::Scalar;
43 using SE3 = pinocchio::SE3Tpl<Scalar>;
44 using Motion = pinocchio::MotionTpl<Scalar>;
45
46 using FramePlacement = FramePlacementResidualTpl<Scalar>;
47 using FramePlacementData = FramePlacementDataTpl<Scalar>;
48
49 using FrameVelocity = FrameVelocityResidualTpl<Scalar>;
50 using FrameVelocityData = FrameVelocityDataTpl<Scalar>;
51
52 using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
53 using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
54
55 using FrameCollision = FrameCollisionResidualTpl<Scalar>;
56 using FrameCollisionData = FrameCollisionDataTpl<Scalar>;
57
58 using pinocchio::GeometryModel;
59
60 if (!eigenpy::check_registration<shared_ptr<PinData>>())
61 bp::register_ptr_to_python<shared_ptr<PinData>>();
62
64
65 bp::class_<FramePlacement, bp::bases<UnaryFunction>>(
66 "FramePlacementResidual", "Frame placement residual function.",
67 bp::init<int, int, const PinModel &, const SE3 &, pinocchio::FrameIndex>(
68 ("self"_a, "ndx", "nu", "model", "p_ref", "id")))
70 .def(unary_visitor)
71 .def("getReference", &FramePlacement::getReference, "self"_a,
72 bp::return_internal_reference<>(), "Get the target frame in SE3.")
73 .def("setReference", &FramePlacement::setReference, ("self"_a, "p_new"),
74 "Set the target frame in SE3.");
75
76 bp::register_ptr_to_python<shared_ptr<FramePlacementData>>();
77
78 bp::class_<FramePlacementData, bp::bases<context::StageFunctionData>>(
79 "FramePlacementData", "Data struct for FramePlacementResidual.",
80 bp::no_init)
81 .def_readonly("rMf", &FramePlacementData::rMf_, "Frame placement error.")
82 .def_readonly("rJf", &FramePlacementData::rJf_)
83 .def_readonly("fJf", &FramePlacementData::fJf_)
84 .def_readonly("pin_data", &FramePlacementData::pin_data_,
85 "Pinocchio data struct.");
86
87 bp::class_<FrameVelocity, bp::bases<UnaryFunction>>(
88 "FrameVelocityResidual", "Frame velocity residual function.",
89 bp::init<int, int, const PinModel &, const Motion &,
90 pinocchio::FrameIndex, pinocchio::ReferenceFrame>(
91 ("self"_a, "ndx", "nu", "model", "v_ref", "id", "reference_frame")))
93 .def(unary_visitor)
94 .def_readonly("pin_model", &FrameVelocity::pin_model_)
95 .def_readwrite("vref", &FrameVelocity::vref_)
96 .def_readwrite("type", &FrameVelocity::type_)
97 .def("getReference", &FrameVelocity::getReference, "self"_a,
98 bp::return_internal_reference<>(),
99 deprecated_member<>("Get the target frame velocity."))
100 .def("setReference", &FrameVelocity::setReference, ("self"_a, "v_new"),
101 deprecated_member<>("Set the target frame velocity."));
102
103 bp::register_ptr_to_python<shared_ptr<FrameVelocityData>>();
104
105 bp::class_<FrameVelocityData, bp::bases<context::StageFunctionData>>(
106 "FrameVelocityData", "Data struct for FrameVelocityResidual.",
107 bp::no_init)
108 .def_readonly("pin_data", &FrameVelocityData::pin_data_,
109 "Pinocchio data struct.");
110
111 bp::class_<FrameTranslation, bp::bases<UnaryFunction>>(
112 "FrameTranslationResidual", "Frame placement residual function.",
113 bp::init<int, int, const PinModel &, const context::Vector3s &,
114 pinocchio::FrameIndex>(
115 ("self"_a, "ndx", "nu", "model", "p_ref", "id")))
117 .def(unary_visitor)
118 .def("getReference", &FrameTranslation::getReference, "self"_a,
119 bp::return_internal_reference<>(),
120 "Get the target frame translation.")
121 .def("setReference", &FrameTranslation::setReference, ("self"_a, "p_new"),
122 "Set the target frame translation.");
123
124 bp::register_ptr_to_python<shared_ptr<FrameTranslationData>>();
125
126 bp::class_<FrameTranslationData, bp::bases<context::StageFunctionData>>(
127 "FrameTranslationData", "Data struct for FrameTranslationResidual.",
128 bp::no_init)
129 .def_readonly("fJf", &FrameTranslationData::fJf_)
130 .def_readonly("pin_data", &FrameTranslationData::pin_data_,
131 "Pinocchio data struct.");
132
133 bp::class_<FrameCollision, bp::bases<UnaryFunction>>(
134 "FrameCollisionResidual", "Frame collision residual function.",
135 bp::init<int, int, const PinModel &, const GeometryModel &,
136 pinocchio::PairIndex>(
137 ("self"_a, "ndx", "nu", "model", "geom_model", "frame_pair_id")))
139 .def(unary_visitor);
140
141 bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
142
143 bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
144 "FrameCollisionData", "Data struct for FrameCollisionResidual.",
145 bp::no_init)
146 .def_readonly("pin_data", &FrameCollisionData::pin_data_,
147 "Pinocchio data struct.")
148 .def_readonly("geom_data", &FrameCollisionData::geom_data,
149 "Geometry data struct.");
150}
151
153 const PinModel &model, PinData &data, const ConstVectorRef &q,
154 const ConstVectorRef &v, const ConstMatrixRef &actMatrix,
155 const StdVectorEigenAligned<context::RCM> &constraint_models,
156 StdVectorEigenAligned<context::RCD> &constraint_datas) {
157 long nu = actMatrix.cols();
158 int d = 0;
159 for (size_t k = 0; k < constraint_models.size(); ++k) {
160 d += (int)constraint_models[k].size();
161 }
162 context::VectorXs out(nu + d);
164 model, data, q, v, actMatrix, constraint_models, constraint_datas, out);
165
166 return bp::make_tuple((context::VectorXs)out.head(nu),
167 (context::VectorXs)out.tail(d));
168}
169
176
177 bp::def("underactuatedConstrainedInverseDynamics",
179 ("model"_a, "data", "q", "v", "actMatrix", "constraint_model",
180 "constraint_data"),
181 "Compute the gravity-compensating torque for a pinocchio Model under "
182 "a rigid constraint.");
183}
184} // namespace python
185} // namespace aligator
186
187#endif
Forward declarations.
ManifoldAbstractTpl< Scalar > Manifold
Definition context.hpp:14
pinocchio::ModelTpl< Scalar, Options > PinModel
Definition fwd.hpp:21
StageFunctionTpl< Scalar > StageFunction
Definition context.hpp:17
UnaryFunctionTpl< Scalar > UnaryFunction
Definition context.hpp:18
pinocchio::DataTpl< Scalar, Options > PinData
Definition fwd.hpp:22
The Python bindings.
Definition blk-matrix.hpp:5
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)
const PolymorphicMultiBaseVisitor< UnaryFunction, StageFunction > unary_visitor
Main package namespace.
std::vector< T, Eigen::aligned_allocator< T > > StdVectorEigenAligned
Typedef for vector with Eigen::aligned_allocator allocator type.
Definition fwd.hpp:173
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_)