| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, University of Edinburgh, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #ifndef CROCODDYL_PINOCCHIO_MODEL_FACTORY_HXX_ | ||
| 10 | #define CROCODDYL_PINOCCHIO_MODEL_FACTORY_HXX_ | ||
| 11 | |||
| 12 | #include <pinocchio/algorithm/center-of-mass.hpp> | ||
| 13 | #include <pinocchio/algorithm/centroidal-derivatives.hpp> | ||
| 14 | #include <pinocchio/algorithm/centroidal.hpp> | ||
| 15 | #include <pinocchio/algorithm/frames.hpp> | ||
| 16 | #include <pinocchio/algorithm/jacobian.hpp> | ||
| 17 | #include <pinocchio/algorithm/kinematics-derivatives.hpp> | ||
| 18 | #include <pinocchio/algorithm/kinematics.hpp> | ||
| 19 | #include <pinocchio/algorithm/rnea-derivatives.hpp> | ||
| 20 | #include <pinocchio/fwd.hpp> | ||
| 21 | #include <pinocchio/multibody/data.hpp> | ||
| 22 | #include <pinocchio/multibody/geometry.hpp> | ||
| 23 | #include <pinocchio/multibody/model.hpp> | ||
| 24 | |||
| 25 | namespace crocoddyl { | ||
| 26 | namespace unittest { | ||
| 27 | /** | ||
| 28 | * @brief Compute all the pinocchio data needed for the numerical | ||
| 29 | * differentiation. We use the address of the object to avoid a copy from the | ||
| 30 | * "boost::bind". | ||
| 31 | * | ||
| 32 | * @param model is the rigid body robot model. | ||
| 33 | * @param data contains the results of the computations. | ||
| 34 | * @param x is the state vector. | ||
| 35 | */ | ||
| 36 | template <typename Scalar, int Options, | ||
| 37 | template <typename, int> class JointCollectionTpl> | ||
| 38 | ✗ | void updateAllPinocchio( | |
| 39 | pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl>* const model, | ||
| 40 | pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>* data, | ||
| 41 | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& x, | ||
| 42 | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>&) { | ||
| 43 | ✗ | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& q = x.segment(0, model->nq); | |
| 44 | ✗ | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& v = | |
| 45 | ✗ | x.segment(model->nq, model->nv); | |
| 46 | ✗ | Eigen::Matrix<Scalar, Eigen::Dynamic, 1> a = | |
| 47 | ✗ | Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(model->nv); | |
| 48 | ✗ | Eigen::Matrix<Scalar, 6, Eigen::Dynamic> tmp; | |
| 49 | ✗ | tmp.resize(6, model->nv); | |
| 50 | ✗ | pinocchio::forwardKinematics(*model, *data, q, v, a); | |
| 51 | ✗ | pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); | |
| 52 | ✗ | pinocchio::computeJointJacobians(*model, *data, q); | |
| 53 | ✗ | pinocchio::updateFramePlacements(*model, *data); | |
| 54 | ✗ | pinocchio::centerOfMass(*model, *data, q, v, a); | |
| 55 | ✗ | pinocchio::jacobianCenterOfMass(*model, *data, q); | |
| 56 | ✗ | pinocchio::computeCentroidalMomentum(*model, *data, q, v); | |
| 57 | ✗ | pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp, | |
| 58 | tmp, tmp, tmp); | ||
| 59 | ✗ | pinocchio::computeRNEADerivatives(*model, *data, q, v, a); | |
| 60 | ✗ | } | |
| 61 | |||
| 62 | } // namespace unittest | ||
| 63 | } // namespace crocoddyl | ||
| 64 | |||
| 65 | #endif // CROCODDYL_PINOCCHIO_MODEL_FACTORY_HXX_ | ||
| 66 |