pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
data.hpp
1//
2// Copyright (c) 2015-2024 CNRS INRIA
3//
4
5#ifndef __pinocchio_python_multibody_data_hpp__
6#define __pinocchio_python_multibody_data_hpp__
7
8#include "pinocchio/multibody/data.hpp"
9#include "pinocchio/serialization/data.hpp"
10
11#include <eigenpy/memory.hpp>
12#include <eigenpy/eigen-to-python.hpp>
13#include <eigenpy/exception.hpp>
14
15#include "pinocchio/bindings/python/fwd.hpp"
16#include "pinocchio/bindings/python/utils/macros.hpp"
17#include "pinocchio/bindings/python/serialization/serializable.hpp"
18#include "pinocchio/bindings/python/utils/std-vector.hpp"
19#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
20#include "pinocchio/bindings/python/utils/copyable.hpp"
21
22#if EIGENPY_VERSION_AT_MOST(2, 8, 1)
23EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)
24#endif
25
26namespace pinocchio
27{
28 namespace python
29 {
30 namespace bp = boost::python;
31
32 template<typename Data>
33 struct PickleData : bp::pickle_suite
34 {
35 static bp::tuple getinitargs(const Data &)
36 {
37 return bp::make_tuple();
38 }
39
40 static bp::tuple getstate(const Data & data)
41 {
42 const std::string str(data.saveToString());
43 return bp::make_tuple(bp::str(str));
44 }
45
46 static void setstate(Data & data, bp::tuple tup)
47 {
48 if (bp::len(tup) == 0 || bp::len(tup) > 1)
49 {
50 throw eigenpy::Exception(
51 "Pickle was not able to reconstruct the model from the loaded data.\n"
52 "The pickle data structure contains too many elements.");
53 }
54
55 bp::object py_obj = tup[0];
56 boost::python::extract<std::string> obj_as_string(py_obj.ptr());
57 if (obj_as_string.check())
58 {
59 const std::string str = obj_as_string;
60 data.loadFromString(str);
61 }
62 else
63 {
64 throw eigenpy::Exception(
65 "Pickle was not able to reconstruct the model from the loaded data.\n"
66 "The entry is not a string.");
67 }
68 }
69
70 static bool getstate_manages_dict()
71 {
72 return true;
73 }
74 };
75
76 template<typename Data>
77 struct DataPythonVisitor : public boost::python::def_visitor<DataPythonVisitor<Data>>
78 {
79 typedef typename Data::Matrix6 Matrix6;
80 typedef typename Data::Matrix6x Matrix6x;
81 typedef typename Data::Matrix3x Matrix3x;
82 typedef typename Data::Vector3 Vector3;
83
84 public:
85#define ADD_DATA_PROPERTY(NAME, DOC) PINOCCHIO_ADD_PROPERTY(Data, NAME, DOC)
86
87#define ADD_DATA_PROPERTY_READONLY(NAME, DOC) PINOCCHIO_ADD_PROPERTY_READONLY(Data, NAME, DOC)
88
89#define ADD_DATA_PROPERTY_READONLY_BYVALUE(NAME, DOC) \
90 PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Data, NAME, DOC)
91
92 /* --- Exposing C++ API to python through the handler ----------------- */
93 template<class PyClass>
94 void visit(PyClass & cl) const
95 {
96 cl.def(bp::init<>(bp::arg("self"), "Default constructor."))
97 .def(bp::init<const context::Model &>(
98 bp::args("self", "model"), "Constructs a data structure from a given model."))
99
100 .ADD_DATA_PROPERTY(
101 joints,
102 "Vector of JointData associated to each JointModel stored in the related model.")
103 .ADD_DATA_PROPERTY(
104 a, "Vector of joint accelerations expressed in the local frame of the joint.")
105 .ADD_DATA_PROPERTY(
106 oa, "Joint spatial acceleration expressed at the origin of the world frame.")
107 .ADD_DATA_PROPERTY(
108 a_gf, "Joint spatial acceleration containing also the contribution of "
109 "the gravity acceleration")
110 .ADD_DATA_PROPERTY(
111 oa_gf, "Joint spatial acceleration containing also the contribution of the gravity "
112 "acceleration, but expressed at the origin of the world frame.")
113
114 .ADD_DATA_PROPERTY(
115 v, "Vector of joint velocities expressed in the local frame of the joint.")
116 .ADD_DATA_PROPERTY(ov, "Vector of joint velocities expressed at the origin of the world.")
117
118 .ADD_DATA_PROPERTY(f, "Vector of body forces expressed in the local frame of the joint.")
119 .ADD_DATA_PROPERTY(of, "Vector of body forces expressed at the origin of the world.")
120 .ADD_DATA_PROPERTY(
121 of_augmented, "Vector of body forces expressed at the origin of the "
122 "world, in the context of lagrangian formulation")
123 .ADD_DATA_PROPERTY(
124 h, "Vector of spatial momenta expressed in the local frame of the joint.")
125 .ADD_DATA_PROPERTY(oh, "Vector of spatial momenta expressed at the origin of the world.")
126 .ADD_DATA_PROPERTY(oMi, "Body absolute placement (wrt world)")
127 .ADD_DATA_PROPERTY(oMf, "frames absolute placement (wrt world)")
128 .ADD_DATA_PROPERTY(liMi, "Body relative placement (wrt parent)")
129 .ADD_DATA_PROPERTY(tau, "Joint torques (output of RNEA)")
130 .ADD_DATA_PROPERTY(nle, "Non Linear Effects (output of nle algorithm)")
131 .ADD_DATA_PROPERTY(ddq, "Joint accelerations (output of ABA)")
132 .ADD_DATA_PROPERTY(Ycrb, "Inertia of the sub-tree composit rigid body")
133 .ADD_DATA_PROPERTY(
134 oYcrb, "Composite Rigid Body Inertia of the sub-tree expressed in the "
135 "WORLD coordinate system.")
136 .ADD_DATA_PROPERTY(Yaba, "Articulated Body Inertia of the sub-tree")
137 .ADD_DATA_PROPERTY(
138 oYaba,
139 "Articulated Body Inertia of the sub-tree expressed in the WORLD coordinate system.")
140 .ADD_DATA_PROPERTY(oL, "Acceleration propagator.")
141 .ADD_DATA_PROPERTY(oK, "Inverse articulated inertia.")
142 .ADD_DATA_PROPERTY(M, "The joint space inertia matrix")
143 .ADD_DATA_PROPERTY(Minv, "The inverse of the joint space inertia matrix")
144 .ADD_DATA_PROPERTY(
145 C, "The Coriolis C(q,v) matrix such that the Coriolis effects are "
146 "given by c(q,v) = C(q,v)v")
147 .ADD_DATA_PROPERTY(g, "Vector of generalized gravity (dim model.nv).")
148 .ADD_DATA_PROPERTY(Fcrb, "Spatial forces set, used in CRBA")
149 .ADD_DATA_PROPERTY(lastChild, "Index of the last child (for CRBA)")
150 .ADD_DATA_PROPERTY(nvSubtree, "Dimension of the subtree motion space (for CRBA)")
151 .ADD_DATA_PROPERTY(U, "Joint Inertia square root (upper triangle)")
152 .ADD_DATA_PROPERTY(D, "Diagonal of UDUT inertia decomposition")
153 .ADD_DATA_PROPERTY(
154 supports_fromRow,
155 "Each element of this vector corresponds to the ordered list of indexes "
156 "belonging to the supporting tree of the given index at the row level. "
157 "It may be helpful to retrieve the sparsity pattern through it.")
158 .ADD_DATA_PROPERTY(parents_fromRow, "First previous non-zero row in M (used in Cholesky)")
159 .ADD_DATA_PROPERTY(
160 mimic_parents_fromRow,
161 "First previous non-zero row belonging to a mimic joint in M (used in Jacobian).")
162 .ADD_DATA_PROPERTY(
163 non_mimic_parents_fromRow,
164 "First previous non-zero row belonging to a non mimic joint in M (used in Jacobian).")
165 .ADD_DATA_PROPERTY(
166 idx_vExtended_to_idx_v_fromRow,
167 "Extended model mapping of the joint rows "
168 "(idx_vExtended_to_idx_v_fromRow[idx_vExtended] = idx_v)")
169 .ADD_DATA_PROPERTY(
170 nvSubtree_fromRow, "Subtree of the current row index (used in Cholesky)")
171 .ADD_DATA_PROPERTY(
172 mimic_subtree_joint, "When mimic joints are in the tree, this will store the index of "
173 "the first non mimic child of each mimic joint. (useful in crba)")
174 .ADD_DATA_PROPERTY(J, "Jacobian of joint placement")
175 .ADD_DATA_PROPERTY(dJ, "Time variation of the Jacobian of joint placement (data.J).")
176 .ADD_DATA_PROPERTY(iMf, "Body placement wrt to algorithm end effector.")
177
178 .ADD_DATA_PROPERTY(Ivx, "Right variation of the inertia matrix.")
179 .ADD_DATA_PROPERTY(vxI, "Left variation of the inertia matrix.")
180 .ADD_DATA_PROPERTY(
181 B, "Combined variations of the inertia matrix consistent with Christoffel symbols.")
182
183 .ADD_DATA_PROPERTY(
184 Ag, "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
185 .ADD_DATA_PROPERTY(dAg, "Time derivative of the centroidal momentum matrix Ag.")
186 .ADD_DATA_PROPERTY(
187 hg, "Centroidal momentum (expressed in the frame centered at the CoM "
188 "and aligned with the world frame).")
189 .ADD_DATA_PROPERTY(
190 dhg, "Centroidal momentum time derivative (expressed in the frame "
191 "centered at the CoM and aligned with the world frame).")
192 .ADD_DATA_PROPERTY(Ig, "Centroidal Composite Rigid Body Inertia.")
193
194 .ADD_DATA_PROPERTY(com, "CoM position of the subtree starting at joint index i.")
195 .ADD_DATA_PROPERTY(vcom, "CoM velocity of the subtree starting at joint index i.")
196 .ADD_DATA_PROPERTY(acom, "CoM acceleration of the subtree starting at joint index i.")
197 .ADD_DATA_PROPERTY(mass, "Mass of the subtree starting at joint index i.")
198 .ADD_DATA_PROPERTY(Jcom, "Jacobian of center of mass.")
199
200 .ADD_DATA_PROPERTY(
201 dAdq,
202 "Variation of the spatial acceleration set with respect to the joint configuration.")
203 .ADD_DATA_PROPERTY(
204 dAdv, "Variation of the spatial acceleration set with respect to the joint velocity.")
205 .ADD_DATA_PROPERTY(
206 dHdq, "Variation of the spatial momenta set with respect to the joint configuration.")
207 .ADD_DATA_PROPERTY(
208 dFdq, "Variation of the force set with respect to the joint configuration.")
209 .ADD_DATA_PROPERTY(dFdv, "Variation of the force set with respect to the joint velocity.")
210 .ADD_DATA_PROPERTY(
211 dFda, "Variation of the force set with respect to the joint acceleration.")
212
213 .ADD_DATA_PROPERTY(
214 dtau_dq, "Partial derivative of the joint torque vector with respect "
215 "to the joint configuration.")
216 .ADD_DATA_PROPERTY(
217 dtau_dv,
218 "Partial derivative of the joint torque vector with respect to the joint velocity.")
219 .ADD_DATA_PROPERTY(
220 ddq_dq, "Partial derivative of the joint acceleration vector with "
221 "respect to the joint configuration.")
222 .ADD_DATA_PROPERTY(
223 ddq_dv, "Partial derivative of the joint acceleration vector with "
224 "respect to the joint velocity.")
225 .ADD_DATA_PROPERTY(
226 ddq_dtau,
227 "Partial derivative of the joint acceleration vector with respect to the joint torque.")
228 .ADD_DATA_PROPERTY(
229 dvc_dq, "Partial derivative of the constraint velocity vector with "
230 "respect to the joint configuration.")
231
232 .ADD_DATA_PROPERTY(
233 dac_dq, "Partial derivative of the contact acceleration vector with "
234 "respect to the joint configuration.")
235 .ADD_DATA_PROPERTY(
236 dac_dv, "Partial derivative of the contact acceleration vector vector "
237 "with respect to the joint velocity.")
238 .ADD_DATA_PROPERTY(
239 dac_da, "Partial derivative of the contact acceleration vector vector "
240 "with respect to the joint acceleration.")
241
242 .ADD_DATA_PROPERTY(osim, "Operational space inertia matrix.")
243
244 .ADD_DATA_PROPERTY_READONLY_BYVALUE(
245 dlambda_dq, "Partial derivative of the contact force vector with "
246 "respect to the joint configuration.")
247 .ADD_DATA_PROPERTY_READONLY_BYVALUE(
248 dlambda_dv,
249 "Partial derivative of the contact force vector with respect to the joint velocity.")
250 .ADD_DATA_PROPERTY_READONLY_BYVALUE(
251 dlambda_dtau,
252 "Partial derivative of the contact force vector with respect to the torque.")
253 .ADD_DATA_PROPERTY(
254 kinetic_energy, "Kinetic energy in [J] computed by computeKineticEnergy")
255 .ADD_DATA_PROPERTY(
256 potential_energy, "Potential energy in [J] computed by computePotentialEnergy")
257 .ADD_DATA_PROPERTY(
258 mechanical_energy,
259 "Mechanical energy in [J] of the system computed by computeMechanicalEnergy")
260
261 .ADD_DATA_PROPERTY(lambda_c, "Lagrange Multipliers linked to contact forces")
262 .ADD_DATA_PROPERTY(impulse_c, "Lagrange Multipliers linked to contact impulses")
263 .ADD_DATA_PROPERTY(contact_chol, "Contact Cholesky decomposition.")
264 .ADD_DATA_PROPERTY(
265 primal_dual_contact_solution,
266 "Right hand side vector when solving the contact dynamics KKT problem.")
267 .ADD_DATA_PROPERTY(
268 lambda_c_prox, "Proximal Lagrange Multipliers used in the computation "
269 "of the Forward Dynamics computations.")
270 .ADD_DATA_PROPERTY(primal_rhs_contact, "Primal RHS in contact dynamic equations.")
271
272 .ADD_DATA_PROPERTY(dq_after, "Generalized velocity after the impact.")
273 .ADD_DATA_PROPERTY(staticRegressor, "Static regressor.")
274 .ADD_DATA_PROPERTY(jointTorqueRegressor, "Joint torque regressor.")
275 .ADD_DATA_PROPERTY(kineticEnergyRegressor, "Kinetic energy regressor.")
276 .ADD_DATA_PROPERTY(potentialEnergyRegressor, "Potential energy regressor.")
277
278#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
279 .def(bp::self == bp::self)
280 .def(bp::self != bp::self)
281#endif
282 ;
283
284 bp::register_ptr_to_python<std::shared_ptr<Data>>();
285 }
286
287 /* --- Expose --------------------------------------------------------- */
288 static void expose()
289 {
290 bp::class_<Data>(
291 "Data",
292 "Articulated rigid body data related to a Model.\n"
293 "It contains all the data that can be modified by the Pinocchio algorithms.",
294 bp::no_init)
295 .def(DataPythonVisitor())
296 .def(CopyableVisitor<Data>())
297#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
298 .def(SerializableVisitor<Data>())
299 .def_pickle(PickleData<Data>())
300#endif
301 ;
302
303 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3;
304 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x;
305 typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) StdVec_Matrix6;
306
307 StdVectorPythonVisitor<std::vector<std::vector<int>>>::expose("StdVec_StdVec_Int");
308 StdAlignedVectorPythonVisitor<Vector3, false>::expose(
309 "StdVec_Vector3",
310 eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Vector3>());
311
312 StdAlignedVectorPythonVisitor<Matrix6x, false>::expose(
313 "StdVec_Matrix6x",
314 eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Matrix6x>());
315 StdAlignedVectorPythonVisitor<Matrix6, false>::expose(
316 "StdVec_Matrix6",
317 eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Matrix6>());
318 StdVectorPythonVisitor<std::vector<int>, true>::expose("StdVec_int");
319#ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
320 serialize<typename StdAlignedVectorPythonVisitor<Vector3, false>::vector_type>();
321 serialize<typename StdAlignedVectorPythonVisitor<Matrix6x, false>::vector_type>();
322#endif
323 serialize<std::vector<int>>();
324 }
325 };
326
327 } // namespace python
328} // namespace pinocchio
329
330#undef ADD_DATA_PROPERTY
331#undef ADD_DATA_PROPERTY_READONLY
332#undef ADD_DATA_PROPERTY_READONLY_BYVALUE
333
334#endif // ifndef __pinocchio_python_multibody_data_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition data.hpp:92
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition data.hpp:94
pinocchio::serialization::Serializable::saveToString
std::string saveToString() const
Saves a Derived object to a string.
Definition serializable.hpp:60
pinocchio::serialization::Serializable::loadFromString
void loadFromString(const std::string &str)
Loads a Derived object from a string.
Definition serializable.hpp:54