| Directory: | ./ |
|---|---|
| File: | unittest/factory/pinocchio_model.hxx |
| Date: | 2025-03-26 19:23:43 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 18 | 18 | 100.0% |
| Branches: | 17 | 34 | 50.0% |
| 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 | 62363 | 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 |
2/4✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62363 times.
✗ Branch 5 not taken.
|
62363 | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& q = x.segment(0, model->nq); |
| 44 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& v = |
| 45 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | x.segment(model->nq, model->nv); |
| 46 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | Eigen::Matrix<Scalar, Eigen::Dynamic, 1> a = |
| 47 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(model->nv); |
| 48 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | Eigen::Matrix<Scalar, 6, Eigen::Dynamic> tmp; |
| 49 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | tmp.resize(6, model->nv); |
| 50 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::forwardKinematics(*model, *data, q, v, a); |
| 51 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a); |
| 52 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::computeJointJacobians(*model, *data, q); |
| 53 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::updateFramePlacements(*model, *data); |
| 54 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::centerOfMass(*model, *data, q, v, a); |
| 55 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::jacobianCenterOfMass(*model, *data, q); |
| 56 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::computeCentroidalMomentum(*model, *data, q, v); |
| 57 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp, |
| 58 | tmp, tmp, tmp); | ||
| 59 |
1/2✓ Branch 1 taken 62363 times.
✗ Branch 2 not taken.
|
62363 | pinocchio::computeRNEADerivatives(*model, *data, q, v, a); |
| 60 | 62363 | } | |
| 61 | |||
| 62 | } // namespace unittest | ||
| 63 | } // namespace crocoddyl | ||
| 64 | |||
| 65 | #endif // CROCODDYL_PINOCCHIO_MODEL_FACTORY_HXX_ | ||
| 66 |