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(parents_fromRow, "First previous non-zero row in M (used in Cholesky)")
154  .ADD_DATA_PROPERTY(
155  nvSubtree_fromRow, "Subtree of the current row index (used in Cholesky)")
156  .ADD_DATA_PROPERTY(J, "Jacobian of joint placement")
157  .ADD_DATA_PROPERTY(dJ, "Time variation of the Jacobian of joint placement (data.J).")
158  .ADD_DATA_PROPERTY(iMf, "Body placement wrt to algorithm end effector.")
159 
160  .ADD_DATA_PROPERTY(Ivx, "Right variation of the inertia matrix.")
161  .ADD_DATA_PROPERTY(vxI, "Left variation of the inertia matrix.")
162  .ADD_DATA_PROPERTY(
163  B, "Combined variations of the inertia matrix consistent with Christoffel symbols.")
164 
165  .ADD_DATA_PROPERTY(
166  Ag, "Centroidal matrix which maps from joint velocity to the centroidal momentum.")
167  .ADD_DATA_PROPERTY(dAg, "Time derivative of the centroidal momentum matrix Ag.")
168  .ADD_DATA_PROPERTY(
169  hg, "Centroidal momentum (expressed in the frame centered at the CoM "
170  "and aligned with the world frame).")
171  .ADD_DATA_PROPERTY(
172  dhg, "Centroidal momentum time derivative (expressed in the frame "
173  "centered at the CoM and aligned with the world frame).")
174  .ADD_DATA_PROPERTY(Ig, "Centroidal Composite Rigid Body Inertia.")
175 
176  .ADD_DATA_PROPERTY(com, "CoM position of the subtree starting at joint index i.")
177  .ADD_DATA_PROPERTY(vcom, "CoM velocity of the subtree starting at joint index i.")
178  .ADD_DATA_PROPERTY(acom, "CoM acceleration of the subtree starting at joint index i.")
179  .ADD_DATA_PROPERTY(mass, "Mass of the subtree starting at joint index i.")
180  .ADD_DATA_PROPERTY(Jcom, "Jacobian of center of mass.")
181 
182  .ADD_DATA_PROPERTY(
183  dAdq,
184  "Variation of the spatial acceleration set with respect to the joint configuration.")
185  .ADD_DATA_PROPERTY(
186  dAdv, "Variation of the spatial acceleration set with respect to the joint velocity.")
187  .ADD_DATA_PROPERTY(
188  dHdq, "Variation of the spatial momenta set with respect to the joint configuration.")
189  .ADD_DATA_PROPERTY(
190  dFdq, "Variation of the force set with respect to the joint configuration.")
191  .ADD_DATA_PROPERTY(dFdv, "Variation of the force set with respect to the joint velocity.")
192  .ADD_DATA_PROPERTY(
193  dFda, "Variation of the force set with respect to the joint acceleration.")
194 
195  .ADD_DATA_PROPERTY(
196  dtau_dq, "Partial derivative of the joint torque vector with respect "
197  "to the joint configuration.")
198  .ADD_DATA_PROPERTY(
199  dtau_dv,
200  "Partial derivative of the joint torque vector with respect to the joint velocity.")
201  .ADD_DATA_PROPERTY(
202  ddq_dq, "Partial derivative of the joint acceleration vector with "
203  "respect to the joint configuration.")
204  .ADD_DATA_PROPERTY(
205  ddq_dv, "Partial derivative of the joint acceleration vector with "
206  "respect to the joint velocity.")
207  .ADD_DATA_PROPERTY(
208  ddq_dtau,
209  "Partial derivative of the joint acceleration vector with respect to the joint torque.")
210  .ADD_DATA_PROPERTY(
211  dvc_dq, "Partial derivative of the constraint velocity vector with "
212  "respect to the joint configuration.")
213 
214  .ADD_DATA_PROPERTY(
215  dac_dq, "Partial derivative of the contact acceleration vector with "
216  "respect to the joint configuration.")
217  .ADD_DATA_PROPERTY(
218  dac_dv, "Partial derivative of the contact acceleration vector vector "
219  "with respect to the joint velocity.")
220  .ADD_DATA_PROPERTY(
221  dac_da, "Partial derivative of the contact acceleration vector vector "
222  "with respect to the joint acceleration.")
223 
224  .ADD_DATA_PROPERTY(osim, "Operational space inertia matrix.")
225 
226  .ADD_DATA_PROPERTY_READONLY_BYVALUE(
227  dlambda_dq, "Partial derivative of the contact force vector with "
228  "respect to the joint configuration.")
229  .ADD_DATA_PROPERTY_READONLY_BYVALUE(
230  dlambda_dv,
231  "Partial derivative of the contact force vector with respect to the joint velocity.")
232  .ADD_DATA_PROPERTY_READONLY_BYVALUE(
233  dlambda_dtau,
234  "Partial derivative of the contact force vector with respect to the torque.")
235  .ADD_DATA_PROPERTY(
236  kinetic_energy, "Kinetic energy in [J] computed by computeKineticEnergy")
237  .ADD_DATA_PROPERTY(
238  potential_energy, "Potential energy in [J] computed by computePotentialEnergy")
239  .ADD_DATA_PROPERTY(
240  mechanical_energy,
241  "Mechanical energy in [J] of the system computed by computeMechanicalEnergy")
242 
243  .ADD_DATA_PROPERTY(lambda_c, "Lagrange Multipliers linked to contact forces")
244  .ADD_DATA_PROPERTY(impulse_c, "Lagrange Multipliers linked to contact impulses")
245  .ADD_DATA_PROPERTY(contact_chol, "Contact Cholesky decomposition.")
246  .ADD_DATA_PROPERTY(
247  primal_dual_contact_solution,
248  "Right hand side vector when solving the contact dynamics KKT problem.")
249  .ADD_DATA_PROPERTY(
250  lambda_c_prox, "Proximal Lagrange Multipliers used in the computation "
251  "of the Forward Dynamics computations.")
252  .ADD_DATA_PROPERTY(primal_rhs_contact, "Primal RHS in contact dynamic equations.")
253 
254  .ADD_DATA_PROPERTY(dq_after, "Generalized velocity after the impact.")
255  .ADD_DATA_PROPERTY(staticRegressor, "Static regressor.")
256  .ADD_DATA_PROPERTY(jointTorqueRegressor, "Joint torque regressor.")
257  .ADD_DATA_PROPERTY(kineticEnergyRegressor, "Kinetic energy regressor.")
258  .ADD_DATA_PROPERTY(potentialEnergyRegressor, "Potential energy regressor.")
259 
260 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
261  .def(bp::self == bp::self)
262  .def(bp::self != bp::self)
263 #endif
264  ;
265  }
266 
267  /* --- Expose --------------------------------------------------------- */
268  static void expose()
269  {
270  bp::class_<Data>(
271  "Data",
272  "Articulated rigid body data related to a Model.\n"
273  "It contains all the data that can be modified by the Pinocchio algorithms.",
274  bp::no_init)
275  .def(DataPythonVisitor())
276  .def(CopyableVisitor<Data>())
277 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
278  .def(SerializableVisitor<Data>())
279  .def_pickle(PickleData<Data>())
280 #endif
281  ;
282 
283  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3;
284  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x;
285  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) StdVec_Matrix6;
286 
287  StdAlignedVectorPythonVisitor<Vector3, false>::expose(
288  "StdVec_Vector3",
289  eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Vector3>());
290 
291  StdAlignedVectorPythonVisitor<Matrix6x, false>::expose(
292  "StdVec_Matrix6x",
293  eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Matrix6x>());
294  StdAlignedVectorPythonVisitor<Matrix6, false>::expose(
295  "StdVec_Matrix6",
296  eigenpy::details::overload_base_get_item_for_std_vector<StdVec_Matrix6>());
297  StdVectorPythonVisitor<std::vector<int>, true>::expose("StdVec_int");
298 #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION
299  serialize<typename StdAlignedVectorPythonVisitor<Vector3, false>::vector_type>();
300  serialize<typename StdAlignedVectorPythonVisitor<Matrix6x, false>::vector_type>();
301 #endif
302  serialize<std::vector<int>>();
303  }
304  };
305 
306  } // namespace python
307 } // namespace pinocchio
308 
309 #undef ADD_DATA_PROPERTY
310 #undef ADD_DATA_PROPERTY_READONLY
311 #undef ADD_DATA_PROPERTY_READONLY_BYVALUE
312 
313 #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:13
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:268
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:50
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:80
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:27
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 Sun Dec 22 2024 03:41:08