bindings/python/multibody/data.hpp
Go to the documentation of this file.
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)
23 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)
24 #endif
25 
26 namespace 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::python::DataPythonVisitor
Definition: bindings/python/multibody/data.hpp:77
boost::python
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::python::PickleData::getinitargs
static bp::tuple getinitargs(const Data &)
Definition: bindings/python/multibody/data.hpp:35
eigenpy::StdVectorPythonVisitor
B
B
pinocchio::python::DataPythonVisitor::Vector3
Data::Vector3 Vector3
Definition: bindings/python/multibody/data.hpp:82
pinocchio::python::PickleData::setstate
static void setstate(Data &data, bp::tuple tup)
Definition: bindings/python/multibody/data.hpp:46
setup.data
data
Definition: cmake/cython/setup.in.py:48
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
eigen-to-python.hpp
exception.hpp
pinocchio::python::PINOCCHIO_ALIGNED_STD_VECTOR
typedef PINOCCHIO_ALIGNED_STD_VECTOR(context::Force) ForceAlignedVector
data.hpp
pinocchio.shortcuts.nle
nle
Definition: shortcuts.py:14
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
pinocchio::python::DataPythonVisitor::Matrix6x
Data::Matrix6x Matrix6x
Definition: bindings/python/multibody/data.hpp:80
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
forward-dynamics-derivatives.ddq_dtau
ddq_dtau
Definition: forward-dynamics-derivatives.py:35
pinocchio::python::DataPythonVisitor::expose
static void expose()
Definition: bindings/python/multibody/data.hpp:288
dcrba.C
C
Definition: dcrba.py:491
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
#define EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(...)
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
eigenpy::details::overload_base_get_item_for_std_vector
serializable.hpp
pinocchio::DataTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: multibody/data.hpp:77
autodiff-rnea.dtau_dv
dtau_dv
Definition: autodiff-rnea.py:29
python
pinocchio::python::v
const Vector3Like & v
Definition: bindings/python/spatial/explog.hpp:66
data.hpp
D
D
pinocchio::python::CopyableVisitor
Add the Python method copy to allow a copy of this by calling the copy constructor.
Definition: copyable.hpp:21
M
M
eigenpy::Exception
pinocchio::python::PickleData::getstate
static bp::tuple getstate(const Data &data)
Definition: bindings/python/multibody/data.hpp:40
copyable.hpp
anymal-simulation.mass
mass
Definition: anymal-simulation.py:62
pinocchio::python::StdAlignedVectorPythonVisitor::expose
static void expose(const std::string &class_name, const std::string &doc_string="")
Definition: std-aligned-vector.hpp:43
ur5x4.h
h
Definition: ur5x4.py:48
forward-dynamics-derivatives.ddq_dq
ddq_dq
Definition: forward-dynamics-derivatives.py:33
a
Vec3f a
pinocchio::python::SerializableVisitor
Definition: bindings/python/serialization/serializable.hpp:19
macros.hpp
pinocchio::python::DataPythonVisitor::Matrix6
Data::Matrix6 Matrix6
Definition: bindings/python/multibody/data.hpp:79
forward-dynamics-derivatives.ddq_dv
ddq_dv
Definition: forward-dynamics-derivatives.py:34
ocp.U
U
Definition: ocp.py:81
pinocchio::python::DataPythonVisitor::Matrix3x
Data::Matrix3x Matrix3x
Definition: bindings/python/multibody/data.hpp:81
autodiff-rnea.dtau_dq
dtau_dq
Definition: autodiff-rnea.py:29
std-aligned-vector.hpp
cl
cl
pinocchio::DataTpl::Matrix6
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: multibody/data.hpp:96
memory.hpp
fwd.hpp
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
str
str
std-vector.hpp
pinocchio::python::DataPythonVisitor::visit
void visit(PyClass &cl) const
Definition: bindings/python/multibody/data.hpp:94
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
pinocchio::python::PickleData
Definition: bindings/python/multibody/data.hpp:33
pinocchio::python::PickleData::getstate_manages_dict
static bool getstate_manages_dict()
Definition: bindings/python/multibody/data.hpp:70

pinocchio
Author(s):
autogenerated on Thu Apr 10 2025 02:42:17