GCC Code Coverage Report

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