data.hpp 15.2 KB
 jcarpent committed Jun 01, 2018 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 // // Copyright (c) 2015-2018 CNRS // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // // Pinocchio is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with // Pinocchio If not, see // . #ifndef __se3_data_hpp__ #define __se3_data_hpp__ #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" #include "pinocchio/multibody/joint/joint.hpp" #include "pinocchio/deprecated.hh" #include "pinocchio/container/aligned-vector.hpp" #include #include namespace se3 {  jcarpent committed Oct 29, 2018 37 38 39  template struct DataTpl  jcarpent committed Jun 01, 2018 40 41 42  { EIGEN_MAKE_ALIGNED_OPERATOR_NEW  jcarpent committed Oct 29, 2018 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69  enum { Options = JointCollection::Options }; typedef ModelTpl Model; typedef typename JointCollection::Scalar Scalar; typedef SE3Tpl SE3; typedef MotionTpl Motion; typedef ForceTpl Force; typedef InertiaTpl Inertia; typedef FrameTpl Frame; typedef se3::Index Index; typedef se3::JointIndex JointIndex; typedef se3::GeomIndex GeomIndex; typedef se3::FrameIndex FrameIndex; typedef std::vector IndexVector; typedef JointModelTpl JointModel; typedef JointDataTpl JointData; 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 70 71 72 73 74 75 76  /// \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 77  /// \brief The 6d jacobian type (temporary)  jcarpent committed Oct 29, 2018 78  typedef Eigen::Matrix Matrix6x;  jcarpent committed Jun 01, 2018 79  /// \brief The 3d jacobian type (temporary)  jcarpent committed Oct 29, 2018 80  typedef Eigen::Matrix Matrix3x;  jcarpent committed Jun 01, 2018 81   jcarpent committed Oct 29, 2018 82 83  typedef Eigen::Matrix RowMatrix6; typedef Eigen::Matrix RowMatrixXd;  jcarpent committed Jun 01, 2018 84 85 86 87 88 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 116 117 118 119 120 121 122 123 124  /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, /// encapsulated in JointDataAccessor. JointDataVector joints; /// \brief Vector of joint accelerations expressed at the centers of the joints. container::aligned_vector a; /// \brief Vector of joint accelerations expressed at the origin. container::aligned_vector oa; /// \brief Vector of joint accelerations due to the gravity field. container::aligned_vector a_gf; /// \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 125  TangentVectorType tau;  jcarpent committed Jun 01, 2018 126 127 128 129  /// \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 130  VectorXs nle;  jcarpent committed Jun 01, 2018 131 132 133 134  /// \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 135  VectorXs g;  jcarpent committed Jun 01, 2018 136 137 138 139 140 141 142 143  /// \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 144  /// \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 145  container::aligned_vector dYcrb; // TODO: change with dense symmetric matrix6  jcarpent committed Jun 01, 2018 146 147  /// \brief The joint space inertia matrix (a square matrix of dim model.nv).  jcarpent committed Oct 29, 2018 148  MatrixXs M;  jcarpent committed Jun 01, 2018 149 150 151 152 153  /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv). RowMatrixXd Minv; /// \brief The Coriolis matrix (a square matrix of dim model.nv).  jcarpent committed Oct 29, 2018 154  MatrixXs C;  jcarpent committed Jun 01, 2018 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  /// \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 175  container::aligned_vector vxI;  jcarpent committed Jun 01, 2018 176 177  /// \brief Left variation of the inertia matrix  jcarpent committed Oct 29, 2018 178  container::aligned_vector Ivx;  jcarpent committed Jun 01, 2018 179 180 181 182 183  /// \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 184  container::aligned_vector doYcrb;  jcarpent committed Jun 01, 2018 185 186  /// \brief Temporary for derivative algorithms  jcarpent committed Oct 29, 2018 187  typename Inertia::Matrix6 Itmp;  jcarpent committed Jun 01, 2018 188 189 190 191 192  /// \brief Temporary for derivative algorithms RowMatrix6 M6tmpR; /// \brief The joint accelerations computed from ABA  jcarpent committed Oct 29, 2018 193  TangentVectorType ddq;  jcarpent committed Jun 01, 2018 194 195 196  // ABA internal data /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]  jcarpent committed Oct 29, 2018 197  container::aligned_vector Yaba; // TODO: change with dense symmetric matrix6  jcarpent committed Jun 01, 2018 198 199  /// \brief Intermediate quantity corresponding to apparent torque [ABA]  jcarpent committed Oct 29, 2018 200  TangentVectorType u; // Joint Inertia  jcarpent committed Jun 01, 2018 201 202 203 204 205 206 207 208  // 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 209  /// \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 210 211 212 213  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 214  /// \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 215 216 217  /// Force hg;  jcarpent committed Oct 29, 2018 218 219 220 221 222 223  /// \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 224 225 226 227 228 229 230 231 232 233 234 235 236  /// \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 237  MatrixXs U;  jcarpent committed Jun 01, 2018 238 239  /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.  jcarpent committed Oct 29, 2018 240  VectorXs D;  jcarpent committed Jun 01, 2018 241 242  /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.  jcarpent committed Oct 29, 2018 243  VectorXs Dinv;  jcarpent committed Jun 01, 2018 244 245  /// \brief Temporary of size NV used in Cholesky Decomposition.  jcarpent committed Oct 29, 2018 246  VectorXs tmp;  jcarpent committed Jun 01, 2018 247 248 249 250 251 252 253 254  /// \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.  jcarpent committed Jul 04, 2018 255  /// \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 se3::getJointJacobian  jcarpent committed Jun 01, 2018 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270  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 271  MatrixXs dtau_dq;  jcarpent committed Jun 01, 2018 272 273  /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.  jcarpent committed Oct 29, 2018 274  MatrixXs dtau_dv;  jcarpent committed Jun 01, 2018 275 276  /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.  jcarpent committed Oct 29, 2018 277  MatrixXs ddq_dq;  jcarpent committed Jun 01, 2018 278 279  /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.  jcarpent committed Oct 29, 2018 280  MatrixXs ddq_dv;  jcarpent committed Jun 01, 2018 281 282 283 284 285  /// \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 286  container::aligned_vector com;  jcarpent committed Jun 01, 2018 287 288  /// \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 289  container::aligned_vector vcom;  jcarpent committed Jun 01, 2018 290 291  /// \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 292  container::aligned_vector acom;  jcarpent committed Jun 01, 2018 293 294  /// \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] corrresponds to the total mass of the model.  jcarpent committed Oct 29, 2018 295  std::vector mass;  jcarpent committed Jun 01, 2018 296 297 298 299 300 301 302  /// \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 303  Scalar kinetic_energy;  jcarpent committed Jun 01, 2018 304 305  /// \brief Potential energy of the model.  jcarpent committed Oct 29, 2018 306  Scalar potential_energy;  jcarpent committed Jun 01, 2018 307 308 309 310  // Temporary variables used in forward dynamics /// \brief Inverse of the operational-space inertia matrix  jcarpent committed Oct 29, 2018 311  MatrixXs JMinvJt;  jcarpent committed Jun 01, 2018 312   Guilhem Saurel committed Jun 07, 2018 313  /// \brief Cholesky decompostion of \f$\JMinvJt\f$.  jcarpent committed Oct 29, 2018 314  Eigen::LLT llt_JMinvJt;  jcarpent committed Jun 01, 2018 315 316  /// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.  jcarpent committed Oct 29, 2018 317  VectorXs lambda_c;  jcarpent committed Jun 01, 2018 318 319  /// \brief Temporary corresponding to \f$\sqrt{D} U^{-1} J^{\top} \f$.  jcarpent committed Oct 29, 2018 320  MatrixXs sDUiJt;  jcarpent committed Jun 01, 2018 321 322  /// \brief Temporary corresponding to the residual torque \f$\tau - b(q,\dot{q}) \f$.  jcarpent committed Oct 29, 2018 323  VectorXs torque_residual;  jcarpent committed Jun 01, 2018 324 325  /// \brief Generalized velocity after impact.  jcarpent committed Oct 29, 2018 326  TangentVectorType dq_after;  jcarpent committed Jun 01, 2018 327 328  /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.  jcarpent committed Oct 29, 2018 329  VectorXs impulse_c;  jcarpent committed Jun 01, 2018 330 331 332 333 334 335 336 337 338  // data related to regressor Matrix3x staticRegressor; /// /// \brief Default constructor of se3::Data from a se3::Model. /// /// \param[in] model The model structure of the rigid body system. ///  jcarpent committed Oct 29, 2018 339  explicit DataTpl(const Model & model);  jcarpent committed Jun 01, 2018 340 341  private:  jcarpent committed Oct 29, 2018 342 343  void computeLastChild(const Model & model); void computeParents_fromRow(const Model & model);  jcarpent committed Jun 01, 2018 344 345 346 347 348 349 350 351 352 353 354 355  }; } // namespace se3 /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ #include "pinocchio/multibody/data.hxx" #endif // ifndef __se3_data_hpp__