GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/center-of-mass-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 19 19 100.0%
Branches: 28 84 33.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__
6 #define __pinocchio_algorithm_center_of_mass_derivatives_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10
11 namespace pinocchio
12 {
13 template<
14 typename Scalar,
15 int Options,
16 template<typename, int> class JointCollectionTpl,
17 typename Matrix3xOut>
18 struct CoMVelocityDerivativesForwardStep
19 : public fusion::JointUnaryVisitorBase<
20 CoMVelocityDerivativesForwardStep<Scalar, Options, JointCollectionTpl, Matrix3xOut>>
21 {
22 /* Assumes that both computeForwardKinematicsDerivatives and centerOfMass (or
23 * equivalent methods like computeAllTerms) have been computed beforehand. */
24
25 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
26 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
27
28 typedef boost::fusion::vector<const Model &, Data &, Matrix3xOut &> ArgsType;
29
30 template<typename JointModel>
31 174 static void algo(
32 const JointModelBase<JointModel> & jmodel,
33 JointDataBase<typename JointModel::JointDataDerived> & jdata,
34 const Model & model,
35 Data & data,
36 const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq)
37 {
38 typedef typename Model::JointIndex JointIndex;
39 typedef typename Data::Motion Motion;
40
41 typedef Eigen::Matrix<
42 Scalar, 6, JointModel::NV, Options, 6,
43 JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV>
44 Matrix6NV;
45
46
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 const JointIndex i = jmodel.id();
47 174 const JointIndex parent = model.parents[i];
48
49 174 Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq);
50 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix3x>::Type
51
1/2
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
 174 dvcom_dqi = jmodel.jointCols(dvcom_dq);
52
53
6/14
✓ Branch 0 taken 84 times.
✓ Branch 1 taken 3 times.
✓ Branch 3 taken 84 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 84 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 84 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 174 Motion vpc = (parent > 0) ? (data.v[i] - (Motion)jdata.v()) : Motion::Zero();
54
2/4
✓ Branch 2 taken 87 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 87 times.
✗ Branch 6 not taken.
 174 vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ]
55
56
2/4
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
 174 Matrix6NV vxS(6, jmodel.nv());
57
2/6
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 174 vxS = vpc.cross(jdata.S());
58
59
5/14
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 87 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 87 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 522 dvcom_dqi.noalias() =
60
2/4
✓ Branch 2 taken 87 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 87 times.
✗ Branch 8 not taken.
 174 (data.mass[i] / data.mass[0]) * data.oMi[i].rotation()
61
2/4
✓ Branch 1 taken 87 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 87 times.
✗ Branch 6 not taken.
 348 * (vxS.template middleRows<3>(Motion::LINEAR) - cross(data.com[i], vxS.template middleRows<3>(Motion::ANGULAR)));
62 }
63 };
64
65 template<
66 typename Scalar,
67 int Options,
68 template<typename, int> class JointCollectionTpl,
69 typename Matrix3xOut>
70 3 void getCenterOfMassVelocityDerivatives(
71 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
72 DataTpl<Scalar, Options, JointCollectionTpl> & data,
73 const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq)
74 {
75 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut, Data::Matrix3x);
76
77
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(vcom_partial_dq.cols(), model.nv);
78
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
79
80 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
81 typedef typename Model::JointIndex JointIndex;
82
83 3 Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq);
84
85 typedef CoMVelocityDerivativesForwardStep<Scalar, Options, JointCollectionTpl, Matrix3xOut>
86 Pass1;
87
2/2
✓ Branch 0 taken 87 times.
✓ Branch 1 taken 3 times.
 90 for (JointIndex i = 1; i < (JointIndex)model.njoints; i++)
88 {
89
1/2
✓ Branch 4 taken 87 times.
✗ Branch 5 not taken.
 87 Pass1::run(model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, dvcom_dq));
90 }
91 3 }
92
93 } // namespace pinocchio
94
95 #endif // ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__
96