data.hpp 15 KB
 jcarpent committed Jun 01, 2018 1 //  Justin Carpentier committed Mar 31, 2019 2 // Copyright (c) 2015-2019 CNRS INRIA  jcarpent committed Jun 01, 2018 3 4 5 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. //  Justin Carpentier committed Nov 08, 2018 6 7 #ifndef __pinocchio_data_hpp__ #define __pinocchio_data_hpp__  jcarpent committed Jun 01, 2018 8 9 10 11 12 13 14  #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3.hpp" #include "pinocchio/spatial/force.hpp" #include "pinocchio/spatial/motion.hpp" #include "pinocchio/spatial/inertia.hpp" #include "pinocchio/multibody/fwd.hpp"  Justin Carpentier committed Oct 29, 2018 15 #include "pinocchio/multibody/joint/joint-generic.hpp"  jcarpent committed Jun 01, 2018 16 17 18 19 20 #include "pinocchio/container/aligned-vector.hpp" #include #include  Justin Carpentier committed Nov 08, 2018 21 namespace pinocchio  jcarpent committed Jun 01, 2018 22 {  jcarpent committed Oct 29, 2018 23   Justin Carpentier committed Oct 29, 2018 24  template class JointCollectionTpl>  jcarpent committed Oct 29, 2018 25  struct DataTpl  jcarpent committed Jun 01, 2018 26 27 28  { EIGEN_MAKE_ALIGNED_OPERATOR_NEW  Justin Carpentier committed Oct 29, 2018 29 30  typedef _Scalar Scalar; enum { Options = _Options };  jcarpent committed Oct 29, 2018 31   Justin Carpentier committed Oct 29, 2018 32 33 34  typedef JointCollectionTpl JointCollection; typedef ModelTpl Model;  jcarpent committed Oct 29, 2018 35 36 37 38 39 40 41  typedef SE3Tpl SE3; typedef MotionTpl Motion; typedef ForceTpl Force; typedef InertiaTpl Inertia; typedef FrameTpl Frame;  Justin Carpentier committed Nov 08, 2018 42 43 44 45  typedef pinocchio::Index Index; typedef pinocchio::JointIndex JointIndex; typedef pinocchio::GeomIndex GeomIndex; typedef pinocchio::FrameIndex FrameIndex;  jcarpent committed Oct 29, 2018 46 47  typedef std::vector IndexVector;  Justin Carpentier committed Oct 29, 2018 48 49  typedef JointModelTpl JointModel; typedef JointDataTpl JointData;  jcarpent committed Oct 29, 2018 50 51 52 53 54 55 56 57  typedef container::aligned_vector JointModelVector; typedef container::aligned_vector JointDataVector; typedef Eigen::Matrix MatrixXs; typedef Eigen::Matrix VectorXs; typedef Eigen::Matrix Vector3;  jcarpent committed Oct 29, 2018 58 59 60 61 62 63 64  /// \brief Dense vectorized version of a joint configuration vector. typedef VectorXs ConfigVectorType; /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). /// It also handles the notion of co-tangent vector (e.g. torque, etc). typedef VectorXs TangentVectorType;  jcarpent committed Jun 01, 2018 65  /// \brief The 6d jacobian type (temporary)  jcarpent committed Oct 29, 2018 66  typedef Eigen::Matrix Matrix6x;  jcarpent committed Jun 01, 2018 67  /// \brief The 3d jacobian type (temporary)  jcarpent committed Oct 29, 2018 68  typedef Eigen::Matrix Matrix3x;  jcarpent committed Jun 01, 2018 69   jcarpent committed Oct 29, 2018 70  typedef Eigen::Matrix RowMatrix6;  Justin Carpentier committed Oct 29, 2018 71  typedef Eigen::Matrix RowMatrixXs;  jcarpent committed Jun 01, 2018 72   Justin Carpentier committed Nov 08, 2018 73  /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,  jcarpent committed Jun 01, 2018 74 75 76  /// encapsulated in JointDataAccessor. JointDataVector joints;  Justin Carpentier committed Mar 31, 2019 77  /// \brief Vector of joint accelerations expressed at the centers of the joints frames.  jcarpent committed Jun 01, 2018 78 79  container::aligned_vector a;  Justin Carpentier committed Mar 31, 2019 80  /// \brief Vector of joint accelerations expressed at the origin of the world.  jcarpent committed Jun 01, 2018 81 82 83 84 85  container::aligned_vector oa; /// \brief Vector of joint accelerations due to the gravity field. container::aligned_vector a_gf;  Justin Carpentier committed Mar 31, 2019 86 87 88  /// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution. container::aligned_vector oa_gf;  jcarpent committed Jun 01, 2018 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115  /// \brief Vector of joint velocities expressed at the centers of the joints. container::aligned_vector v; /// \brief Vector of joint velocities expressed at the origin. container::aligned_vector ov; /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of /// all external forces acting on the body. container::aligned_vector f; /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of /// all external forces acting on the body. container::aligned_vector of; /// \brief Vector of spatial momenta expressed in the local frame of the joint. container::aligned_vector h; /// \brief Vector of spatial momenta expressed in the world frame. container::aligned_vector oh; /// \brief Vector of absolute joint placements (wrt the world). container::aligned_vector oMi; /// \brief Vector of relative joint placements (wrt the body parent). container::aligned_vector liMi; /// \brief Vector of joint torques (dim model.nv).  jcarpent committed Oct 29, 2018 116  TangentVectorType tau;  jcarpent committed Jun 01, 2018 117 118 119 120  /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects. /// \note In the multibody dynamics equation \f$M\ddot{q} + b(q, \dot{q}) = \tau \f$, /// the non linear effects are associated to the term \f$b\f$.  jcarpent committed Oct 29, 2018 121  VectorXs nle;  jcarpent committed Jun 01, 2018 122 123 124 125  /// \brief Vector of generalized gravity (dim model.nv). /// \note In the multibody dynamics equation \f$M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$, /// the gravity effect is associated to the \f$g\f$ term.  jcarpent committed Oct 29, 2018 126  VectorXs g;  jcarpent committed Jun 01, 2018 127 128 129 130 131 132 133 134  /// \brief Vector of absolute operationnel frame placements (wrt the world). container::aligned_vector oMf; /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint /// and expressed in the local frame of the joint.. container::aligned_vector Ycrb;  Guilhem Saurel committed Jun 07, 2018 135  /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$\dot{Y}_{crb}\f$. See Data::Ycrb for more details.  jcarpent committed Oct 29, 2018 136  container::aligned_vector dYcrb; // TODO: change with dense symmetric matrix6  jcarpent committed Jun 01, 2018 137 138  /// \brief The joint space inertia matrix (a square matrix of dim model.nv).  jcarpent committed Oct 29, 2018 139  MatrixXs M;  jcarpent committed Jun 01, 2018 140 141  /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).  Justin Carpentier committed Oct 29, 2018 142  RowMatrixXs Minv;  jcarpent committed Jun 01, 2018 143 144  /// \brief The Coriolis matrix (a square matrix of dim model.nv).  jcarpent committed Oct 29, 2018 145  MatrixXs C;  jcarpent committed Jun 01, 2018 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165  /// \brief Variation of the forceset with respect to the joint configuration. Matrix6x dFdq; /// \brief Variation of the forceset with respect to the joint velocity. Matrix6x dFdv; /// \brief Variation of the forceset with respect to the joint acceleration. Matrix6x dFda; /// \brief Used in computeMinverse Matrix6x SDinv; /// \brief Used in computeMinverse Matrix6x UDinv; /// \brief Used in computeMinverse Matrix6x IS; /// \brief Right variation of the inertia matrix  jcarpent committed Oct 29, 2018 166  container::aligned_vector vxI;  jcarpent committed Jun 01, 2018 167 168  /// \brief Left variation of the inertia matrix  jcarpent committed Oct 29, 2018 169  container::aligned_vector Ivx;  jcarpent committed Jun 01, 2018 170 171 172 173 174  /// \brief Inertia quantities expressed in the world frame container::aligned_vector oYcrb; /// \brief Time variation of the inertia quantities expressed in the world frame  jcarpent committed Oct 29, 2018 175  container::aligned_vector doYcrb;  jcarpent committed Jun 01, 2018 176 177  /// \brief Temporary for derivative algorithms  jcarpent committed Oct 29, 2018 178  typename Inertia::Matrix6 Itmp;  jcarpent committed Jun 01, 2018 179 180 181  /// \brief Temporary for derivative algorithms RowMatrix6 M6tmpR;  Justin Carpentier committed Oct 29, 2018 182  RowMatrix6 M6tmpR2;  jcarpent committed Jun 01, 2018 183 184  /// \brief The joint accelerations computed from ABA  jcarpent committed Oct 29, 2018 185  TangentVectorType ddq;  jcarpent committed Jun 01, 2018 186 187 188  // ABA internal data /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]  jcarpent committed Oct 29, 2018 189  container::aligned_vector Yaba; // TODO: change with dense symmetric matrix6  jcarpent committed Jun 01, 2018 190 191  /// \brief Intermediate quantity corresponding to apparent torque [ABA]  jcarpent committed Oct 29, 2018 192  TangentVectorType u; // Joint Inertia  jcarpent committed Jun 01, 2018 193 194 195 196 197 198 199 200  // CCRBA return quantities /// \brief Centroidal Momentum Matrix /// \note \f$hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum. Matrix6x Ag; // dCCRBA return quantities /// \brief Centroidal Momentum Matrix Time Variation  Guilhem Saurel committed Jun 07, 2018 201  /// \note \f$\dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum.  jcarpent committed Jun 01, 2018 202 203 204 205  Matrix6x dAg; /// \brief Centroidal momentum quantity. /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).  jcarpent committed Oct 29, 2018 206  /// \note \f$h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.  jcarpent committed Jun 01, 2018 207 208 209  /// Force hg;  jcarpent committed Oct 29, 2018 210 211 212 213 214 215  /// \brief Centroidal momentum time derivative. /// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame). /// \note \f$\dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$\dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation. /// Force dhg;  jcarpent committed Jun 01, 2018 216 217 218 219 220 221 222 223 224 225 226 227 228  /// \brief Centroidal Composite Rigid Body Inertia. /// \note \f$hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity. Inertia Ig; /// \brief Spatial forces set, used in CRBA and CCRBA container::aligned_vector Fcrb; /// \brief Index of the last child (for CRBA) std::vector lastChild; /// \brief Dimension of the subtree motion space (for CRBA) std::vector nvSubtree; /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.  jcarpent committed Oct 29, 2018 229  MatrixXs U;  jcarpent committed Jun 01, 2018 230 231  /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.  jcarpent committed Oct 29, 2018 232  VectorXs D;  jcarpent committed Jun 01, 2018 233 234  /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.  jcarpent committed Oct 29, 2018 235  VectorXs Dinv;  jcarpent committed Jun 01, 2018 236 237  /// \brief Temporary of size NV used in Cholesky Decomposition.  jcarpent committed Oct 29, 2018 238  VectorXs tmp;  jcarpent committed Jun 01, 2018 239 240 241 242 243 244 245 246  /// \brief First previous non-zero row in M (used in Cholesky Decomposition). std::vector parents_fromRow; /// \brief Subtree of the current row index (used in Cholesky Decomposition). std::vector nvSubtree_fromRow; /// \brief Jacobian of joint placements.  Justin Carpentier committed Nov 08, 2018 247  /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call pinocchio::getJointJacobian  jcarpent committed Jun 01, 2018 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262  Matrix6x J; /// \brief Derivative of the Jacobian with respect to the time. Matrix6x dJ; /// \brief Variation of the spatial velocity set with respect to the joint configuration. Matrix6x dVdq; /// \brief Variation of the spatial acceleration set with respect to the joint configuration. Matrix6x dAdq; /// \brief Variation of the spatial acceleration set with respect to the joint velocity. Matrix6x dAdv; /// \brief Partial derivative of the joint torque vector with respect to the joint configuration.  jcarpent committed Oct 29, 2018 263  MatrixXs dtau_dq;  jcarpent committed Jun 01, 2018 264 265  /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.  jcarpent committed Oct 29, 2018 266  MatrixXs dtau_dv;  jcarpent committed Jun 01, 2018 267 268  /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.  jcarpent committed Oct 29, 2018 269  MatrixXs ddq_dq;  jcarpent committed Jun 01, 2018 270 271  /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.  jcarpent committed Oct 29, 2018 272  MatrixXs ddq_dv;  jcarpent committed Jun 01, 2018 273 274 275 276 277  /// \brief Vector of joint placements wrt to algorithm end effector. container::aligned_vector iMf; /// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$j \f$ and expressed in the joint frame \f$j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.  jcarpent committed Oct 29, 2018 278  container::aligned_vector com;  jcarpent committed Jun 01, 2018 279 280  /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$j \f$ and expressed in the joint frame \f$j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.  jcarpent committed Oct 29, 2018 281  container::aligned_vector vcom;  jcarpent committed Jun 01, 2018 282 283  /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$j \f$ and expressed in the joint frame \f$j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.  jcarpent committed Oct 29, 2018 284  container::aligned_vector acom;  jcarpent committed Jun 01, 2018 285   Gabriele Buondonno committed Mar 18, 2019 286  /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$j \f$. The element mass[0] corresponds to the total mass of the model.  jcarpent committed Oct 29, 2018 287  std::vector mass;  jcarpent committed Jun 01, 2018 288 289 290 291 292 293 294  /// \brief Jacobien of center of mass. /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$. Matrix3x Jcom; /// \brief Kinetic energy of the model.  jcarpent committed Oct 29, 2018 295  Scalar kinetic_energy;  jcarpent committed Jun 01, 2018 296 297  /// \brief Potential energy of the model.  jcarpent committed Oct 29, 2018 298  Scalar potential_energy;  jcarpent committed Jun 01, 2018 299 300 301 302  // Temporary variables used in forward dynamics /// \brief Inverse of the operational-space inertia matrix  jcarpent committed Oct 29, 2018 303  MatrixXs JMinvJt;  jcarpent committed Jun 01, 2018 304   Guilhem Saurel committed Jun 07, 2018 305  /// \brief Cholesky decompostion of \f$\JMinvJt\f$.  jcarpent committed Oct 29, 2018 306  Eigen::LLT llt_JMinvJt;  jcarpent committed Jun 01, 2018 307   Justin Carpentier committed Nov 08, 2018 308  /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.  jcarpent committed Oct 29, 2018 309  VectorXs lambda_c;  jcarpent committed Jun 01, 2018 310 311  /// \brief Temporary corresponding to \f$\sqrt{D} U^{-1} J^{\top} \f$.  jcarpent committed Oct 29, 2018 312  MatrixXs sDUiJt;  jcarpent committed Jun 01, 2018 313 314  /// \brief Temporary corresponding to the residual torque \f$\tau - b(q,\dot{q}) \f$.  jcarpent committed Oct 29, 2018 315  VectorXs torque_residual;  jcarpent committed Jun 01, 2018 316 317  /// \brief Generalized velocity after impact.  jcarpent committed Oct 29, 2018 318  TangentVectorType dq_after;  jcarpent committed Jun 01, 2018 319   Justin Carpentier committed Nov 08, 2018 320  /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.  jcarpent committed Oct 29, 2018 321  VectorXs impulse_c;  jcarpent committed Jun 01, 2018 322 323 324 325 326  // data related to regressor Matrix3x staticRegressor; ///  Justin Carpentier committed Nov 08, 2018 327  /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.  jcarpent committed Jun 01, 2018 328 329 330  /// /// \param[in] model The model structure of the rigid body system. ///  jcarpent committed Oct 29, 2018 331  explicit DataTpl(const Model & model);  jcarpent committed Jun 01, 2018 332 333  private:  jcarpent committed Oct 29, 2018 334 335  void computeLastChild(const Model & model); void computeParents_fromRow(const Model & model);  jcarpent committed Jun 01, 2018 336 337 338  };  Justin Carpentier committed Nov 08, 2018 339 } // namespace pinocchio  jcarpent committed Jun 01, 2018 340 341 342 343 344 345  /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ #include "pinocchio/multibody/data.hxx"  Justin Carpentier committed Nov 08, 2018 346 #endif // ifndef __pinocchio_data_hpp__  jcarpent committed Jun 01, 2018 347