model.hpp 21.2 KB
 Florian Valenza committed Apr 29, 2015 1 //  jcarpent committed Mar 30, 2017 2 // Copyright (c) 2015-2017 CNRS  Antonio El Khoury committed Oct 14, 2015 3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.  Florian Valenza committed Apr 29, 2015 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 // // 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 // .  Nicolas Mansard committed Mar 30, 2015 19 20 21 22 23 24 #ifndef __se3_model_hpp__ #define __se3_model_hpp__ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3.hpp" #include "pinocchio/spatial/force.hpp"  25 26 #include "pinocchio/spatial/motion.hpp" #include "pinocchio/spatial/inertia.hpp"  jcarpent committed Feb 16, 2016 27 #include "pinocchio/multibody/fwd.hpp"  jcarpent committed Jul 07, 2016 28 #include "pinocchio/multibody/frame.hpp"  jcarpent committed Sep 26, 2016 29 #include "pinocchio/multibody/joint/joint.hpp"  jcarpent committed Aug 08, 2016 30 #include "pinocchio/deprecated.hh"  Valenza Florian committed Sep 16, 2016 31 #include "pinocchio/tools/string-generator.hpp"  jcarpent committed Sep 30, 2016 32 #include "pinocchio/container/aligned-vector.hpp"  jcarpent committed Aug 08, 2016 33   34 #include  jcarpent committed Mar 17, 2016 35 #include  Nicolas Mansard committed Mar 30, 2015 36 37 38  namespace se3 {  jcarpent committed Aug 02, 2016 39  struct Model  Nicolas Mansard committed Mar 30, 2015 40  {  mnaveau committed May 20, 2016 41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  jcarpent committed Feb 17, 2016 42 43 44 45  typedef se3::Index Index; typedef se3::JointIndex JointIndex; typedef se3::GeomIndex GeomIndex; typedef se3::FrameIndex FrameIndex;  jcarpent committed Jul 23, 2016 46  typedef std::vector IndexVector;  Nicolas Mansard committed Mar 30, 2015 47   jcarpent committed Mar 07, 2016 48 49 50 51 52 53  /// \brief Dimension of the configuration vector representation. int nq; /// \brief Dimension of the velocity vector space. int nv;  jcarpent committed Jun 28, 2016 54  /// \brief Number of joints.  Valenza Florian committed Sep 19, 2016 55  int njoints;  Valenza Florian committed Jun 10, 2016 56   jcarpent committed Jun 28, 2016 57  /// \brief Number of bodies.  Valenza Florian committed Sep 19, 2016 58  int nbodies;  jcarpent committed Mar 07, 2016 59 60  /// \brief Number of operational frames.  Valenza Florian committed Sep 19, 2016 61  int nframes;  Florian Valenza committed Jun 12, 2015 62   jcarpent committed Mar 07, 2016 63  /// \brief Spatial inertias of the body expressed in the supporting joint frame .  jcarpent committed Sep 30, 2016 64  container::aligned_vector inertias;  jcarpent committed Mar 07, 2016 65 66  /// \brief Placement (SE3) of the input of joint regarding to the parent joint output
• .  jcarpent committed Sep 30, 2016 67  container::aligned_vector jointPlacements;  Valenza Florian committed Jun 10, 2016 68   69  /// \brief Model of joint , encapsulated in a JointModelAccessor.  Valenza Florian committed Jul 07, 2016 70  JointModelVector joints;  jcarpent committed Mar 07, 2016 71 72 73  /// \brief Joint parent of joint , denoted
• (li==parents[i]). std::vector parents;  Valenza Florian committed Jun 10, 2016 74   jcarpent committed Mar 07, 2016 75 76 77  /// \brief Name of joint std::vector names;  78  /// \brief Vector of joint's neutral configurations  Nicolas Mansard committed Jul 21, 2016 79  Eigen::VectorXd neutralConfiguration;  80   81 82 83 84 85 86 87 88 89 90  /// \brief Vector of maximal joint torques Eigen::VectorXd effortLimit; /// \brief Vector of maximal joint velocities Eigen::VectorXd velocityLimit; /// \brief Lower joint configuration limit Eigen::VectorXd lowerPositionLimit; /// \brief Upper joint configuration limit Eigen::VectorXd upperPositionLimit;  jcarpent committed Mar 07, 2016 91  /// \brief Vector of operational frames registered on the model.  jcarpent committed Sep 30, 2016 92  container::aligned_vector frames;  jcarpent committed Jul 23, 2016 93   jcarpent committed Jul 24, 2016 94  /// \brief Vector of subtrees.  jcarpent committed Jul 23, 2016 95  /// subtree[j] corresponds to the subtree supported by the joint j.  jcarpent committed Jul 24, 2016 96  /// The first element of subtree[j] is the index of the joint j itself.  jcarpent committed Jul 23, 2016 97  std::vector subtrees;  98   jcarpent committed Jul 23, 2016 99  /// \brief Spatial gravity of the model.  jcarpent committed Mar 07, 2016 100  Motion gravity;  jcarpent committed Jun 28, 2016 101   jcarpent committed Mar 07, 2016 102 103  /// \brief Default 3D gravity vector (=(0,0,-9.81)). static const Eigen::Vector3d gravity981;  Nicolas Mansard committed Mar 30, 2015 104   Olivier Stasse committed May 24, 2017 105 106 107  /// \brief Model name; std::string name;  jcarpent committed Jul 23, 2016 108  /// \brief Default constructor. Builds an empty model with no joints.  Nicolas Mansard committed Mar 30, 2015 109 110 111  Model() : nq(0) , nv(0)  Valenza Florian committed Sep 19, 2016 112 113 114  , njoints(1) , nbodies(1) , nframes(0)  Nicolas Mansard committed Mar 30, 2015 115  , inertias(1)  116  , jointPlacements(1, SE3::Identity())  Nicolas Mansard committed Mar 30, 2015 117  , joints(1)  118  , parents(1, 0)  Nicolas Mansard committed Mar 30, 2015 119  , names(1)  jcarpent committed Jul 24, 2016 120  , subtrees(1)  Nicolas Mansard committed Mar 30, 2015 121 122  , gravity( gravity981,Eigen::Vector3d::Zero() ) {  Nicolas Mansard committed Mar 30, 2015 123  names[0] = "universe"; // Should be "universe joint (trivial)"  Joseph Mirabel committed Sep 07, 2016 124 125 126  // FIXME Should the universe joint be a FIXED_JOINT even if it is // in the list of joints ? See comment in definition of // Model::addJointFrame and Model::addBodyFrame  Joseph Mirabel committed Sep 05, 2016 127  addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));  Joseph Mirabel committed Sep 06, 2016 128 129 130 131  // Inertia of universe has no sense. inertias[0].mass() = std::numeric_limits::quiet_NaN(); inertias[0].lever().fill (std::numeric_limits::quiet_NaN()); inertias[0].inertia().fill (std::numeric_limits::quiet_NaN());  Nicolas Mansard committed Mar 30, 2015 132  }  Nicolas Mansard committed Mar 30, 2015 133  ~Model() {} // std::cout << "Destroy model" << std::endl; }  jcarpent committed Mar 07, 2016 134 135  ///  136  /// \brief Add a joint to the kinematic tree with given bounds.  jcarpent committed Mar 07, 2016 137  ///  Joseph Mirabel committed Sep 05, 2016 138 139  /// \remark This method does not add a Frame of same name to the vector of frames. /// Use Model::addJointFrame.  jcarpent committed Jul 23, 2016 140  /// \remark The inertia supported by the joint is set to Zero.  jcarpent committed Mar 07, 2016 141  ///  jcarpent committed Jul 23, 2016 142  /// \tparam JointModelDerived The type of the joint model.  jcarpent committed Mar 07, 2016 143 144  /// /// \param[in] parent Index of the parent joint.  jcarpent committed Jul 23, 2016 145 146 147  /// \param[in] joint_model The joint model. /// \param[in] joint_placement Placement of the joint inside its parent joint. /// \param[in] joint_name Name of the joint. If empty, the name is random.  148 149 150 151  /// \param[in] max_effort Maximal joint torque. /// \param[in] max_velocity Maximal joint velocity. /// \param[in] min_config Lower joint configuration. /// \param[in] max_config Upper joint configuration.  Valenza Florian committed Jun 10, 2016 152  ///  jcarpent committed Jul 23, 2016 153  /// \return The index of the new joint.  Valenza Florian committed Jun 10, 2016 154  ///  Joseph Mirabel committed Sep 05, 2016 155  /// \sa Model::appendBodyToJoint, Model::addJointFrame  Valenza Florian committed Jun 10, 2016 156  ///  jcarpent committed Jul 23, 2016 157 158  template JointIndex addJoint(const JointIndex parent, const JointModelBase & joint_model, const SE3 & joint_placement,  jcarpent committed Sep 27, 2016 159  const std::string & joint_name,  160 161 162  const Eigen::VectorXd & max_effort, const Eigen::VectorXd & max_velocity, const Eigen::VectorXd & min_config,  jcarpent committed Sep 27, 2016 163  const Eigen::VectorXd & max_config  164 165 166 167  ); /// /// \brief Add a joint to the kinematic tree with infinite bounds.  jcarpent committed Mar 07, 2016 168  ///  jcarpent committed Jul 23, 2016 169  /// \remark This method also adds a Frame of same name to the vector of frames.  jcarpent committed Jul 23, 2016 170  /// \remark The inertia supported by the joint is set to Zero.  jcarpent committed Mar 07, 2016 171  ///  jcarpent committed Jul 23, 2016 172  /// \tparam JointModelDerived The type of the joint model.  jcarpent committed Mar 07, 2016 173 174  /// /// \param[in] parent Index of the parent joint.  jcarpent committed Jul 23, 2016 175 176 177  /// \param[in] joint_model The joint model. /// \param[in] joint_placement Placement of the joint inside its parent joint. /// \param[in] joint_name Name of the joint. If empty, the name is random.  Valenza Florian committed Jun 10, 2016 178  ///  jcarpent committed Jul 23, 2016 179  /// \return The index of the new joint.  Valenza Florian committed Jun 10, 2016 180  ///  jcarpent committed Jul 23, 2016 181  /// \sa Model::appendBodyToJoint  Valenza Florian committed Jun 10, 2016 182  ///  jcarpent committed Jul 23, 2016 183 184  template JointIndex addJoint(const JointIndex parent, const JointModelBase & joint_model, const SE3 & joint_placement,  jcarpent committed Sep 27, 2016 185  const std::string & joint_name  jcarpent committed Jul 23, 2016 186  );  Valenza Florian committed Jun 10, 2016 187 188  ///  Joseph Mirabel committed Sep 05, 2016 189  /// \brief Add a joint to the frame tree.  Valenza Florian committed Jun 10, 2016 190  ///  Joseph Mirabel committed Sep 05, 2016 191 192 193 194  /// \param[in] jointIndex Index of the joint. /// \param[in] frameIndex Index of the parent frame. If negative, /// the parent frame is the frame of the parent joint. ///  Joseph Mirabel committed Sep 07, 2016 195  /// \return The index of the new frame or -1 in case of error.  Joseph Mirabel committed Sep 05, 2016 196  ///  Joseph Mirabel committed Sep 07, 2016 197 198  int addJointFrame (const JointIndex& jointIndex, int frameIndex = -1);  Joseph Mirabel committed Sep 05, 2016 199   Valenza Florian committed Jun 10, 2016 200  ///  jcarpent committed Jul 23, 2016 201  /// \brief Append a body to a given joint of the kinematic tree.  Valenza Florian committed Jun 10, 2016 202  ///  jcarpent committed Jul 23, 2016 203  /// \param[in] joint_index Index of the supporting joint.  Valenza Florian committed Jun 10, 2016 204  /// \param[in] Y Spatial inertia of the body.  jcarpent committed Jul 23, 2016 205  /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.  Valenza Florian committed Jun 10, 2016 206  ///  jcarpent committed Jul 23, 2016 207  /// \sa Model::addJoint  Valenza Florian committed Jun 10, 2016 208  ///  jcarpent committed Jul 23, 2016 209  void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,  Joseph Mirabel committed Sep 05, 2016 210 211 212 213 214 215 216 217 218 219 220  const SE3 & body_placement = SE3::Identity()); /// /// \brief Add a body to the frame tree. /// /// \param[in] body_name Name of the body. /// \param[in] parentJoint Index of the parent joint. /// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement. /// \param[in] previousFrame Index of the parent frame. If negative, /// the parent frame is the frame of the parent joint. ///  Joseph Mirabel committed Sep 07, 2016 221  /// \return The index of the new frame or -1 in case of error.  Joseph Mirabel committed Sep 05, 2016 222  ///  Joseph Mirabel committed Sep 07, 2016 223 224 225 226  int addBodyFrame (const std::string & body_name, const JointIndex & parentJoint, const SE3 & body_placement = SE3::Identity(), int previousFrame = -1);  Valenza Florian committed Jun 10, 2016 227   jcarpent committed Mar 07, 2016 228 229  /// /// \brief Return the index of a body given by its name.  230 231 232 233 234  /// /// \warning If no body is found, return the number of elements at time T. /// This can lead to errors if the model is expanded after this method is called /// (for example to get the id of a parent body) ///  jcarpent committed Mar 07, 2016 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253  /// \param[in] name Name of the body. /// /// \return Index of the body. /// JointIndex getBodyId(const std::string & name) const; /// /// \brief Check if a body given by its name exists. /// /// \param[in] name Name of the body. /// /// \return True if the body exists in the kinematic tree. /// bool existBodyName(const std::string & name) const; /// /// \brief Return the index of a joint given by its name. ///  254 255 256  /// \warning If no joint is found, return the number of elements at time T. /// This can lead to errors if the model is expanded after this method is called /// (for example to get the id of a parent joint)  jcarpent committed Mar 07, 2016 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278  /// \param[in] index Index of the joint. /// /// \return Index of the joint. /// JointIndex getJointId(const std::string & name) const; /// /// \brief Check if a joint given by its name exists. /// /// \param[in] name Name of the joint. /// /// \return True if the joint exists in the kinematic tree. /// bool existJointName(const std::string & name) const; /// /// \brief Get the name of a joint given by its index. /// /// \param[in] index Index of the joint. /// /// \return Name of the joint. ///  Valenza Florian committed Sep 19, 2016 279  PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;  280   jcarpent committed Mar 07, 2016 281  ///  jcarpent committed Aug 08, 2016 282 283  /// \brief Returns the index of a frame given by its name. /// \sa Model::existFrame to check if the frame exists or not.  jcarpent committed Mar 07, 2016 284  ///  jcarpent committed Aug 08, 2016 285  /// \warning If no frame is found, returns the size of the vector of Model::frames.  286  /// This can lead to errors if the model is expanded after this method is called  jcarpent committed Aug 08, 2016 287  /// (for example to get the id of a parent frame).  288  ///  jcarpent committed Aug 08, 2016 289  /// \param[in] name Name of the frame.  Joseph Mirabel committed Sep 07, 2016 290  /// \param[in] type Type of the frame.  jcarpent committed Mar 07, 2016 291 292 293  /// /// \return Index of the frame. ///  Joseph Mirabel committed Sep 07, 2016 294  FrameIndex getFrameId (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;  jcarpent committed Mar 07, 2016 295 296  ///  jcarpent committed Aug 08, 2016 297  /// \brief Checks if a frame given by its name exists.  jcarpent committed Mar 07, 2016 298 299  /// /// \param[in] name Name of the frame.  Joseph Mirabel committed Sep 07, 2016 300  /// \param[in] type Type of the frame.  jcarpent committed Mar 07, 2016 301  ///  jcarpent committed Aug 08, 2016 302  /// \return Returns true if the frame exists.  jcarpent committed Mar 07, 2016 303  ///  Joseph Mirabel committed Sep 07, 2016 304  bool existFrame (const std::string & name, const FrameType& type = (FrameType) (JOINT | FIXED_JOINT | BODY | OP_FRAME | SENSOR )) const;  jcarpent committed Mar 07, 2016 305   306   jcarpent committed Mar 07, 2016 307  ///  jcarpent committed Aug 08, 2016 308  /// \brief Adds a frame to the kinematic tree.  jcarpent committed Mar 07, 2016 309 310 311  /// /// \param[in] frame The frame to add to the kinematic tree. ///  Joseph Mirabel committed Sep 07, 2016 312 313  /// \return Returns the index of the frame if it has been successfully added, /// -1 otherwise.  jcarpent committed Mar 07, 2016 314  ///  Joseph Mirabel committed Sep 07, 2016 315  int addFrame(const Frame & frame);  316   317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337  /// Check the validity of the attributes of Model with respect to the specification of some /// algorithms. /// /// The method is a template so that the checkers can be defined in each algorithms. /// \param[in] checker a class, typically defined in the algorithm module, that /// validates the attributes of model. /// \return true if the Model is valid, false otherwise. template inline bool check(const AlgorithmCheckerBase & checker = AlgorithmCheckerBase()) const { return checker.checkModel(*this); } /// Multiple checks for a fusion::vector of AlgorithmCheckerBase. /// /// Run the check test for several conditons. /// \param[in] v fusion::vector of algo checkers. The param is typically initialize with /// boost::fusion::make_list( AlgoChecker1(), AlgoChecker2(), ...) /// make_list is defined in #include /// \warning no more than 10 checkers can be added (or Model API should be extended). /// \note This method is implemented in src/algo/check.hxx. template  jcarpent committed Mar 30, 2017 338  inline bool check( const boost::fusion::list & checkerList ) const;  339 340  /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.  jcarpent committed Mar 30, 2017 341  inline bool check() const;  jcarpent committed Mar 30, 2017 342 343 344 345 346 347 348  /// Run checkData on data and current model. /// /// \param[in] data to be checked wrt *this. /// /// \return true if the data is valid, false otherwise. inline bool check(const Data & data) const;  349   jcarpent committed Jul 24, 2016 350 351 352 353 354 355 356  protected: /// \brief Add the joint_id to its parent subtrees. /// /// \param[in] joint_id The id of the joint to add to the subtrees /// void addJointIndexToParentSubtrees(const JointIndex joint_id);  Nicolas Mansard committed Mar 30, 2015 357 358  };  jcarpent committed Aug 02, 2016 359  struct Data  Nicolas Mansard committed Mar 30, 2015 360  {  mnaveau committed May 20, 2016 361  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  jcarpent committed Mar 07, 2016 362  /// \brief The 6d jacobian type (temporary)  Nicolas Mansard committed Mar 30, 2015 363  typedef Eigen::Matrix Matrix6x;  jcarpent committed Mar 07, 2016 364  /// \brief The 3d jacobian type (temporary)  Nicolas Mansard committed Mar 30, 2015 365  typedef Eigen::Matrix Matrix3x;  jcarpent committed Jan 27, 2016 366  typedef SE3::Vector3 Vector3;  Nicolas Mansard committed Mar 30, 2015 367   368 369  /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, /// encapsulated in JointDataAccessor.  Valenza Florian committed Jul 07, 2016 370  JointDataVector joints;  jcarpent committed Feb 15, 2016 371 372  /// \brief Vector of joint accelerations.  jcarpent committed Sep 30, 2016 373  container::aligned_vector a;  jcarpent committed Feb 15, 2016 374 375  /// \brief Vector of joint accelerations due to the gravity field.  jcarpent committed Sep 30, 2016 376  container::aligned_vector a_gf;  jcarpent committed Feb 15, 2016 377 378  /// \brief Vector of joint velocities.  jcarpent committed Sep 30, 2016 379  container::aligned_vector v;  jcarpent committed Feb 15, 2016 380 381 382  /// \brief Vector of body forces. For each body, the force represents the sum of /// all external forces acting on the body.  jcarpent committed Sep 30, 2016 383  container::aligned_vector f;  jcarpent committed Feb 15, 2016 384 385  /// \brief Vector of absolute joint placements (wrt the world).  jcarpent committed Sep 30, 2016 386  container::aligned_vector oMi;  jcarpent committed Mar 04, 2016 387   jcarpent committed Feb 15, 2016 388  /// \brief Vector of relative joint placements (wrt the body parent).  jcarpent committed Sep 30, 2016 389  container::aligned_vector liMi;  jcarpent committed Feb 15, 2016 390 391 392 393 394 395 396 397  /// \brief Vector of joint torques (dim model.nv). Eigen::VectorXd tau; /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to the Coriolis, centrifugal and gravitational effects. /// \note In the equation \f$M\ddot{q} + b = \tau \f$, /// the non linear effects are associated to the \f$b\f$ term. Eigen::VectorXd nle;  Nicolas Mansard committed Mar 30, 2015 398   jcarpent committed Feb 15, 2016 399  /// \brief Vector of absolute operationnel frame placements (wrt the world).  jcarpent committed Sep 30, 2016 400  container::aligned_vector oMf;  401   jcarpent committed Feb 15, 2016 402  /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint.  jcarpent committed Sep 30, 2016 403  container::aligned_vector Ycrb;  jcarpent committed Feb 15, 2016 404 405 406  /// \brief The joint space inertia matrix (a square matrix of dim model.nv). Eigen::MatrixXd M;  jcarpent committed Mar 04, 2016 407 408 409 410 411 412  /// \brief The joint accelerations computed from ABA Eigen::VectorXd ddq; // ABA internal data /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]  jcarpent committed Sep 30, 2016 413  container::aligned_vector Yaba;  jcarpent committed Mar 04, 2016 414 415  /// \brief Intermediate quantity corresponding to apparent torque [ABA]  jcarpent committed Mar 06, 2016 416 417 418  Eigen::VectorXd u; // Joint Inertia // CCRBA return quantities  jcarpent committed Mar 07, 2016 419 420  /// \brief Centroidal Momentum Matrix /// \note \f$hg = Ag \dot{q}\f$ maps the joint velocity set to the centroidal momentum.  jcarpent committed Mar 06, 2016 421 422  Matrix6x Ag;  jcarpent committed Mar 07, 2016 423 424 425  /// \brief Centroidal momentum quantity. /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame. ///  jcarpent committed Mar 06, 2016 426 427  Force hg;  jcarpent committed Mar 07, 2016 428 429  /// \brief Centroidal Composite Rigid Body Inertia. /// \note \f$hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.  jcarpent committed Mar 06, 2016 430  Inertia Ig;  Nicolas Mansard committed Mar 30, 2015 431   jcarpent committed Mar 07, 2016 432  /// \brief Spatial forces set, used in CRBA and CCRBA  jcarpent committed Sep 30, 2016 433  container::aligned_vector Fcrb;  Nicolas Mansard committed Mar 30, 2015 434   jcarpent committed Mar 07, 2016 435 436 437 438  /// \brief Index of the last child (for CRBA) std::vector lastChild; /// \brief Dimension of the subtree motion space (for CRBA) std::vector nvSubtree;  Nicolas Mansard committed Mar 30, 2015 439   jcarpent committed Mar 07, 2016 440 441 442 443 444 445 446 447 448 449 450 451 452 453  /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition. Eigen::MatrixXd U; /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. Eigen::VectorXd D; /// \brief Temporary of size NV used in Cholesky Decomposition. Eigen::VectorXd tmp; /// \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;  Nicolas Mansard committed Mar 30, 2015 454   jcarpent committed Feb 15, 2016 455 456 457 458  /// \brief Jacobian of joint placements. /// \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::getJacobian Matrix6x J;  jcarpent committed Sep 24, 2017 459 460 461  /// \brief Derivative of the Jacobian with respect to the time. Matrix6x dJ;  jcarpent committed Feb 15, 2016 462  /// \brief Vector of joint placements wrt to algorithm end effector.  jcarpent committed Sep 30, 2016 463  container::aligned_vector iMf;  Nicolas Mansard committed Mar 30, 2015 464   jcarpent committed Feb 15, 2016 465  /// \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 Sep 30, 2016 466  container::aligned_vector com;  jcarpent committed Feb 15, 2016 467 468  /// \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 Sep 30, 2016 469  container::aligned_vector vcom;  jcarpent committed Feb 15, 2016 470 471  /// \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 Sep 30, 2016 472  container::aligned_vector acom;  jcarpent committed Feb 15, 2016 473 474 475 476 477  /// \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. std::vector mass; /// \brief Jacobien of center of mass.  jcarpent committed Jun 17, 2016 478  /// \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$.  jcarpent committed Feb 15, 2016 479  Matrix3x Jcom;  Florian Valenza committed Jun 12, 2015 480   jcarpent committed Jan 27, 2016 481   jcarpent committed Mar 07, 2016 482 483 484 485 486 487  /// \brief Kinetic energy of the model. double kinetic_energy; /// \brief Potential energy of the model. double potential_energy;  jcarpent committed Mar 17, 2016 488 489 490 491 492 493 494 495  // Temporary variables used in forward dynamics /// \brief Inverse of the operational-space inertia matrix Eigen::MatrixXd JMinvJt; /// \brief Cholesky decompostion of \JMinvJt. Eigen::LLT llt_JMinvJt;  jcarpent committed Apr 03, 2016 496  /// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.  Guilhem Saurel committed Mar 25, 2016 497  Eigen::VectorXd lambda_c;  jcarpent committed Mar 17, 2016 498 499 500 501 502 503 504  /// \brief Temporary corresponding to \f$\sqrt{D} U^{-1} J^{\top} \f$. Eigen::MatrixXd sDUiJt; /// \brief Temporary corresponding to the residual torque \f$\tau - b(q,\dot{q}) \f$. Eigen::VectorXd torque_residual;  jcarpent committed Apr 03, 2016 505 506 507 508 509 510  /// \brief Generalized velocity after impact. Eigen::VectorXd dq_after; /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics. Eigen::VectorXd impulse_c;  jcarpent committed Mar 07, 2016 511  ///  jcarpent committed Feb 15, 2016 512 513 514  /// \brief Default constructor of se3::Data from a se3::Model. /// /// \param[in] model The model structure of the rigid body system.  jcarpent committed Mar 07, 2016 515  ///  Joseph Mirabel committed Aug 20, 2016 516  explicit Data (const Model & model);  Nicolas Mansard committed Mar 30, 2015 517   Nicolas Mansard committed Mar 30, 2015 518  private:  Nicolas Mansard committed Mar 30, 2015 519  void computeLastChild(const Model& model);  520  void computeParents_fromRow(const Model& model);  521   Nicolas Mansard committed Mar 30, 2015 522 523 524 525 526 527 528  }; } // namespace se3 /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */  529 #include "pinocchio/multibody/model.hxx"  Nicolas Mansard committed Mar 30, 2015 530 531  #endif // ifndef __se3_model_hpp__