GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/data.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 177 233 76.0%
Branches: 288 832 34.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_data_hxx__
7 #define __pinocchio_multibody_data_hxx__
8
9 #include "pinocchio/spatial/fwd.hpp"
10 #include "pinocchio/multibody/model.hpp"
11 #include "pinocchio/utils/string-generator.hpp"
12 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
13
14 /// @cond DEV
15
16 namespace pinocchio
17 {
18
19 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
20 697 DataTpl<Scalar, Options, JointCollectionTpl>::DataTpl(const Model & model)
21
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 : joints(0)
22
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , a((std::size_t)model.njoints, Motion::Zero())
23
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oa((std::size_t)model.njoints, Motion::Zero())
24
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oa_drift((std::size_t)model.njoints, Motion::Zero())
25
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oa_augmented((std::size_t)model.njoints, Motion::Zero())
26
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , a_gf((std::size_t)model.njoints, Motion::Zero())
27
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oa_gf((std::size_t)model.njoints, Motion::Zero())
28
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , v((std::size_t)model.njoints, Motion::Zero())
29
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , ov((std::size_t)model.njoints, Motion::Zero())
30
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , f((std::size_t)model.njoints, Force::Zero())
31
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , of((std::size_t)model.njoints, Force::Zero())
32
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , of_augmented((std::size_t)model.njoints, Force::Zero())
33
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , h((std::size_t)model.njoints, Force::Zero())
34
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oh((std::size_t)model.njoints, Force::Zero())
35
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oMi((std::size_t)model.njoints, SE3::Identity())
36
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , liMi((std::size_t)model.njoints, SE3::Identity())
37
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , tau(VectorXs::Zero(model.nv))
38
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , nle(VectorXs::Zero(model.nv))
39
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , g(VectorXs::Zero(model.nv))
40
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oMf((std::size_t)model.nframes, SE3::Identity())
41
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , Ycrb((std::size_t)model.njoints, Inertia::Zero())
42
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , dYcrb((std::size_t)model.njoints, Inertia::Zero())
43
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , M(MatrixXs::Zero(model.nv, model.nv))
44
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , Minv(MatrixXs::Zero(model.nv, model.nv))
45
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , C(MatrixXs::Zero(model.nv, model.nv))
46
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dHdq(Matrix6x::Zero(6, model.nv))
47
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dFdq(Matrix6x::Zero(6, model.nv))
48
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dFdv(Matrix6x::Zero(6, model.nv))
49
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dFda(Matrix6x::Zero(6, model.nv))
50
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , SDinv(Matrix6x::Zero(6, model.nv))
51
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , UDinv(Matrix6x::Zero(6, model.nv))
52
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , IS(MatrixXs::Zero(6, model.nv))
53
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , vxI((std::size_t)model.njoints, Inertia::Matrix6::Zero())
54
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , Ivx((std::size_t)model.njoints, Inertia::Matrix6::Zero())
55
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , B((std::size_t)model.njoints, Inertia::Matrix6::Zero())
56
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oinertias((std::size_t)model.njoints, Inertia::Zero())
57
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , oYcrb((std::size_t)model.njoints, Inertia::Zero())
58
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , doYcrb((std::size_t)model.njoints, Inertia::Matrix6::Zero())
59
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , ddq(VectorXs::Zero(model.nv))
60
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , Yaba((std::size_t)model.njoints, Inertia::Matrix6::Zero())
61
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , oYaba((std::size_t)model.njoints, Inertia::Matrix6::Zero())
62
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , oL((std::size_t)model.njoints, Inertia::Matrix6::Zero())
63
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , oK((std::size_t)model.njoints, Inertia::Matrix6::Zero())
64
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , u(VectorXs::Zero(model.nv))
65
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , Ag(Matrix6x::Zero(6, model.nv))
66
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dAg(Matrix6x::Zero(6, model.nv))
67
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , hg(Force::Zero())
68
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , dhg(Force::Zero())
69
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , Ig(Inertia::Zero())
70
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , Fcrb((std::size_t)model.njoints, Matrix6x::Zero(6, model.nv))
71
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , lastChild((std::size_t)model.njoints, -1)
72
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , nvSubtree((std::size_t)model.njoints, -1)
73
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , start_idx_v_fromRow((std::size_t)model.nv, -1)
74
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , end_idx_v_fromRow((std::size_t)model.nv, -1)
75
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , U(MatrixXs::Identity(model.nv, model.nv))
76
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , D(VectorXs::Zero(model.nv))
77
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , Dinv(VectorXs::Zero(model.nv))
78
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , tmp(VectorXs::Zero(model.nv))
79
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , parents_fromRow((std::size_t)model.nv, -1)
80
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , supports_fromRow((std::size_t)model.nv)
81
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , nvSubtree_fromRow((std::size_t)model.nv, -1)
82
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , J(Matrix6x::Zero(6, model.nv))
83
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dJ(Matrix6x::Zero(6, model.nv))
84
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , ddJ(Matrix6x::Zero(6, model.nv))
85
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , psid(Matrix6x::Zero(6, model.nv))
86
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , psidd(Matrix6x::Zero(6, model.nv))
87
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dVdq(Matrix6x::Zero(6, model.nv))
88
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dAdq(Matrix6x::Zero(6, model.nv))
89
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dAdv(Matrix6x::Zero(6, model.nv))
90
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dtau_dq(RowMatrixXs::Zero(model.nv, model.nv))
91
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dtau_dv(RowMatrixXs::Zero(model.nv, model.nv))
92
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , ddq_dq(RowMatrixXs::Zero(model.nv, model.nv))
93
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , ddq_dv(RowMatrixXs::Zero(model.nv, model.nv))
94
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , ddq_dtau(RowMatrixXs::Zero(model.nv, model.nv))
95
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , iMf((std::size_t)model.njoints, SE3::Identity())
96
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , com((std::size_t)model.njoints, Vector3::Zero())
97
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , vcom((std::size_t)model.njoints, Vector3::Zero())
98
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , acom((std::size_t)model.njoints, Vector3::Zero())
99
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , mass((std::size_t)model.njoints, (Scalar)(-1))
100
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , Jcom(Matrix3x::Zero(3, model.nv))
101 697 , kinetic_energy(Scalar(0))
102 697 , potential_energy(Scalar(0))
103 697 , mechanical_energy(Scalar(0))
104
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , JMinvJt()
105
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , llt_JMinvJt()
106
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , lambda_c()
107
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , lambda_c_prox()
108
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , diff_lambda_c()
109
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , sDUiJt(MatrixXs::Zero(model.nv, model.nv))
110
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , torque_residual(VectorXs::Zero(model.nv))
111
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , dq_after(VectorXs::Zero(model.nv))
112
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , impulse_c()
113
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , staticRegressor(Matrix3x::Zero(3, 4 * (model.njoints - 1)))
114
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , bodyRegressor(BodyRegressorType::Zero())
115
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , jointTorqueRegressor(MatrixXs::Zero(model.nv, 10 * (model.njoints - 1)))
116
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , kineticEnergyRegressor(RowVectorXs::Zero(10 * (model.njoints - 1)))
117
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
 697 , potentialEnergyRegressor(RowVectorXs::Zero(10 * (model.njoints - 1)))
118
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , KA((std::size_t)model.njoints, Matrix6x::Zero(6, 0))
119
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , LA((std::size_t)model.njoints, MatrixXs::Zero(0, 0))
120
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , lA((std::size_t)model.njoints, VectorXs::Zero(0))
121
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , lambdaA((std::size_t)model.njoints, VectorXs::Zero(0))
122
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , par_cons_ind((std::size_t)model.njoints, 0)
123
2/4
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 , a_bias((std::size_t)model.njoints, Motion::Zero())
124
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , KAS((std::size_t)model.njoints, MatrixXs::Zero(0, 0))
125 #if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93)
126 , kinematic_hessians(
127 6,
128 std::max(1, model.nv),
129 std::max(1, model.nv)) // the minimum size should be 1 for compatibility reasons
130 , d2tau_dqdq(
131 std::max(1, model.nv),
132 std::max(1, model.nv),
133 std::max(1, model.nv)) // the minimum size should be 1 for compatibility reasons
134 , d2tau_dvdv(
135 std::max(1, model.nv),
136 std::max(1, model.nv),
137 std::max(1, model.nv)) // the minimum size should be 1 for compatibility reasons
138 , d2tau_dqdv(
139 std::max(1, model.nv),
140 std::max(1, model.nv),
141 std::max(1, model.nv)) // the minimum size should be 1 for compatibility reasons
142 , d2tau_dadq(
143 std::max(1, model.nv),
144 std::max(1, model.nv),
145 std::max(1, model.nv)) // the minimum size should be 1 for compatibility reasons
146 #else
147
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , kinematic_hessians(6, model.nv, model.nv)
148
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , d2tau_dqdq(model.nv, model.nv, model.nv)
149
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , d2tau_dvdv(model.nv, model.nv, model.nv)
150
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , d2tau_dqdv(model.nv, model.nv, model.nv)
151
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 , d2tau_dadq(model.nv, model.nv, model.nv)
152 #endif
153
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , extended_motion_propagator((std::size_t)model.njoints, Matrix6::Zero())
154
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , extended_motion_propagator2((std::size_t)model.njoints, Matrix6::Zero())
155
3/6
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 697 times.
✗ Branch 9 not taken.
 697 , spatial_inv_inertia((std::size_t)model.njoints, Matrix6::Zero())
156
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , accumulation_descendant((std::size_t)model.njoints, 0)
157
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , accumulation_ancestor((std::size_t)model.njoints, 0)
158
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , constraints_supported_dim((std::size_t)model.njoints, 0)
159
1/2
✓ Branch 2 taken 697 times.
✗ Branch 3 not taken.
 697 , constraints_supported((std::size_t)model.njoints)
160
23/46
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 697 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 697 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 697 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 697 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 697 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 697 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 697 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 697 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 697 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 697 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 697 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 697 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 697 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 697 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 697 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 697 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 697 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 697 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 697 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 697 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 697 times.
✗ Branch 65 not taken.
✓ Branch 68 taken 697 times.
✗ Branch 69 not taken.
 4182 , constraints_on_joint((std::size_t)model.njoints)
161 {
162 typedef typename Model::JointIndex JointIndex;
163
164 /* Create data structure associated to the joints */
165
2/2
✓ Branch 0 taken 18033 times.
✓ Branch 1 taken 697 times.
 18730 for (JointIndex i = 0; i < (JointIndex)(model.njoints); ++i)
166
2/4
✓ Branch 2 taken 18033 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18033 times.
✗ Branch 6 not taken.
 18033 joints.push_back(CreateJointData<Scalar, Options, JointCollectionTpl>::run(model.joints[i]));
167
168 /* Init for CRBA */
169
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 M.setZero();
170
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 Minv.setZero();
171
2/2
✓ Branch 0 taken 18033 times.
✓ Branch 1 taken 697 times.
 18730 for (JointIndex i = 0; i < (JointIndex)(model.njoints); ++i)
172 {
173
1/2
✓ Branch 2 taken 18033 times.
✗ Branch 3 not taken.
 18033 Fcrb[i].resize(6, model.nv);
174 }
175
176
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 computeLastChild(model);
177
178 /* Init for Cholesky */
179
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 computeParents_fromRow(model);
180
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 computeSupports_fromRow(model);
181
182 /* Init universe states relatively to itself */
183
2/4
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 697 times.
✗ Branch 6 not taken.
 697 a_gf[0] = -model.gravity;
184
185
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 kinematic_hessians.setZero();
186
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 d2tau_dqdq.setZero();
187
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 d2tau_dvdv.setZero();
188
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 d2tau_dqdv.setZero();
189
1/2
✓ Branch 1 taken 697 times.
✗ Branch 2 not taken.
 697 d2tau_dadq.setZero();
190 697 }
191
192 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
193 697 inline void DataTpl<Scalar, Options, JointCollectionTpl>::computeLastChild(const Model & model)
194 {
195 typedef typename Model::Index Index;
196
197
1/2
✓ Branch 3 taken 697 times.
✗ Branch 4 not taken.
 697 std::fill(lastChild.begin(), lastChild.end(), -1);
198
2/2
✓ Branch 0 taken 18033 times.
✓ Branch 1 taken 697 times.
 18730 for (int i = model.njoints - 1; i >= 0; --i)
199 {
200
2/2
✓ Branch 1 taken 2616 times.
✓ Branch 2 taken 15417 times.
 18033 if (lastChild[(Index)i] == -1)
201 2616 lastChild[(Index)i] = i;
202 18033 const Index & parent = model.parents[(Index)i];
203 18033 lastChild[parent] = std::max<int>(lastChild[(Index)i], lastChild[parent]);
204
205 36066 nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_v()
206 18033 + model.joints[(Index)lastChild[(Index)i]].nv()
207 18033 - model.joints[(Index)i].idx_v();
208 }
209 697 }
210
211 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
212 inline void
213 697 DataTpl<Scalar, Options, JointCollectionTpl>::computeParents_fromRow(const Model & model)
214 {
215 typedef typename Model::Index Index;
216
217
2/2
✓ Branch 0 taken 17336 times.
✓ Branch 1 taken 697 times.
 18033 for (Index joint = 1; joint < (Index)(model.njoints); joint++)
218 {
219 17336 const Index & parent = model.parents[joint];
220 17336 const int nvj = model.joints[joint].nv();
221 17336 const int idx_vj = model.joints[joint].idx_v();
222
223
2/4
✓ Branch 0 taken 17336 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 17336 times.
✗ Branch 3 not taken.
 17336 assert(idx_vj >= 0 && idx_vj < model.nv);
224
2/2
✓ Branch 0 taken 16640 times.
✓ Branch 1 taken 696 times.
 17336 if (parent > 0)
225 16640 parents_fromRow[(Index)idx_vj] =
226 16640 model.joints[parent].idx_v() + model.joints[parent].nv() - 1;
227 else
228 696 parents_fromRow[(Index)idx_vj] = -1;
229 17336 nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint];
230
231 17336 start_idx_v_fromRow[(size_t)idx_vj] = idx_vj;
232 17336 end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvj - 1;
233
2/2
✓ Branch 0 taken 3298 times.
✓ Branch 1 taken 17336 times.
 20634 for (int row = 1; row < nvj; ++row)
234 {
235 3298 parents_fromRow[(size_t)(idx_vj + row)] = idx_vj + row - 1;
236 3298 nvSubtree_fromRow[(size_t)(idx_vj + row)] = nvSubtree[joint] - row;
237 3298 start_idx_v_fromRow[(size_t)(idx_vj + row)] = start_idx_v_fromRow[(size_t)idx_vj];
238 3298 end_idx_v_fromRow[(size_t)(idx_vj + row)] = end_idx_v_fromRow[(size_t)idx_vj];
239 }
240 }
241 697 }
242
243 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
244 inline void
245 697 DataTpl<Scalar, Options, JointCollectionTpl>::computeSupports_fromRow(const Model & model)
246 {
247 typedef typename Model::JointIndex JointIndex;
248
249
2/2
✓ Branch 0 taken 17336 times.
✓ Branch 1 taken 697 times.
 18033 for (JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); joint_id++)
250 {
251
1/2
✓ Branch 2 taken 17336 times.
✗ Branch 3 not taken.
 17336 const int nvj = nv(model.joints[joint_id]);
252
1/2
✓ Branch 2 taken 17336 times.
✗ Branch 3 not taken.
 17336 const int idx_vj = idx_v(model.joints[joint_id]);
253
254
2/4
✓ Branch 0 taken 17336 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 17336 times.
✗ Branch 3 not taken.
 17336 assert(idx_vj >= 0 && idx_vj < model.nv);
255
256 17336 const int parent_fromRow = parents_fromRow[(size_t)idx_vj];
257
258
2/2
✓ Branch 0 taken 16638 times.
✓ Branch 1 taken 698 times.
 17336 if (parent_fromRow >= 0)
259
1/2
✓ Branch 3 taken 16638 times.
✗ Branch 4 not taken.
 16638 supports_fromRow[(size_t)idx_vj] = supports_fromRow[(size_t)parent_fromRow];
260
261
1/2
✓ Branch 2 taken 17336 times.
✗ Branch 3 not taken.
 17336 supports_fromRow[(size_t)idx_vj].push_back(idx_vj);
262
263
2/2
✓ Branch 0 taken 3298 times.
✓ Branch 1 taken 17336 times.
 20634 for (int row = 1; row < nvj; ++row)
264 {
265
1/2
✓ Branch 3 taken 3298 times.
✗ Branch 4 not taken.
 3298 supports_fromRow[(size_t)(idx_vj + row)] = supports_fromRow[(size_t)(idx_vj + row - 1)];
266
1/2
✓ Branch 2 taken 3298 times.
✗ Branch 3 not taken.
 3298 supports_fromRow[(size_t)(idx_vj + row)].push_back(idx_vj + row);
267 }
268 }
269 697 }
270
271 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
272 ✗ bool operator==(
273 const DataTpl<Scalar, Options, JointCollectionTpl> & data1,
274 const DataTpl<Scalar, Options, JointCollectionTpl> & data2)
275 {
276 ✗ bool value =
277 ✗ data1.joints == data2.joints && data1.a == data2.a && data1.oa == data2.oa
278 ✗ && data1.oa_drift == data2.oa_drift && data1.oa_augmented == data2.oa_augmented
279 ✗ && data1.a_gf == data2.a_gf && data1.oa_gf == data2.oa_gf && data1.v == data2.v
280 ✗ && data1.ov == data2.ov && data1.f == data2.f && data1.of == data2.of
281 ✗ && data1.of_augmented == data2.of_augmented && data1.h == data2.h && data1.oh == data2.oh
282 ✗ && data1.oMi == data2.oMi && data1.liMi == data2.liMi && data1.tau == data2.tau
283 ✗ && data1.nle == data2.nle && data1.g == data2.g && data1.oMf == data2.oMf
284 ✗ && data1.Ycrb == data2.Ycrb && data1.dYcrb == data2.dYcrb && data1.M == data2.M
285 ✗ && data1.Minv == data2.Minv && data1.C == data2.C && data1.dHdq == data2.dHdq
286 ✗ && data1.dFdq == data2.dFdq && data1.dFdv == data2.dFdv && data1.dFda == data2.dFda
287 ✗ && data1.SDinv == data2.SDinv && data1.UDinv == data2.UDinv && data1.IS == data2.IS
288 ✗ && data1.vxI == data2.vxI && data1.Ivx == data2.Ivx && data1.oinertias == data2.oinertias
289 ✗ && data1.oYcrb == data2.oYcrb && data1.doYcrb == data2.doYcrb && data1.ddq == data2.ddq
290 ✗ && data1.Yaba == data2.Yaba && data1.oYaba == data2.oYaba
291 ✗ && data1.oYaba_contact == data2.oYaba_contact && data1.oL == data2.oL && data1.oK == data2.oK
292 ✗ && data1.u == data2.u && data1.Ag == data2.Ag && data1.dAg == data2.dAg
293 ✗ && data1.hg == data2.hg && data1.dhg == data2.dhg && data1.Ig == data2.Ig
294 ✗ && data1.Fcrb == data2.Fcrb && data1.lastChild == data2.lastChild
295 ✗ && data1.nvSubtree == data2.nvSubtree
296 ✗ && data1.start_idx_v_fromRow == data2.start_idx_v_fromRow
297 ✗ && data1.end_idx_v_fromRow == data2.end_idx_v_fromRow && data1.U == data2.U
298 ✗ && data1.D == data2.D && data1.Dinv == data2.Dinv
299 ✗ && data1.parents_fromRow == data2.parents_fromRow
300 ✗ && data1.supports_fromRow == data2.supports_fromRow
301 ✗ && data1.nvSubtree_fromRow == data2.nvSubtree_fromRow && data1.J == data2.J
302 ✗ && data1.dJ == data2.dJ && data1.ddJ == data2.ddJ && data1.psid == data2.psid
303 ✗ && data1.psidd == data2.psidd && data1.dVdq == data2.dVdq && data1.dAdq == data2.dAdq
304 ✗ && data1.dAdv == data2.dAdv && data1.dtau_dq == data2.dtau_dq
305 ✗ && data1.dtau_dv == data2.dtau_dv && data1.ddq_dq == data2.ddq_dq
306 ✗ && data1.ddq_dv == data2.ddq_dv && data1.dvc_dq == data2.dvc_dq
307 ✗ && data1.dac_dq == data2.dac_dq && data1.dac_dv == data2.dac_dv
308 ✗ && data1.dac_da == data2.dac_da && data1.osim == data2.osim
309 ✗ && data1.dlambda_dq == data2.dlambda_dq && data1.dlambda_dv == data2.dlambda_dv
310 ✗ && data1.dlambda_dtau == data2.dlambda_dtau && data1.dlambda_dx_prox == data2.dlambda_dx_prox
311 ✗ && data1.drhs_prox == data2.drhs_prox && data1.iMf == data2.iMf && data1.com == data2.com
312 ✗ && data1.vcom == data2.vcom && data1.acom == data2.acom && data1.mass == data2.mass
313 ✗ && data1.Jcom == data2.Jcom && data1.kinetic_energy == data2.kinetic_energy
314 ✗ && data1.potential_energy == data2.potential_energy
315 ✗ && data1.mechanical_energy == data2.mechanical_energy && data1.JMinvJt == data2.JMinvJt
316 ✗ && data1.lambda_c == data2.lambda_c && data1.lambda_c_prox == data2.lambda_c_prox
317 ✗ && data1.diff_lambda_c == data2.diff_lambda_c && data1.sDUiJt == data2.sDUiJt
318 ✗ && data1.torque_residual == data2.torque_residual && data1.dq_after == data2.dq_after
319 ✗ && data1.impulse_c == data2.impulse_c && data1.staticRegressor == data2.staticRegressor
320 ✗ && data1.bodyRegressor == data2.bodyRegressor
321 ✗ && data1.jointTorqueRegressor == data2.jointTorqueRegressor
322 // && data1.contact_chol == data2.contact_chol
323 ✗ && data1.primal_dual_contact_solution == data2.primal_dual_contact_solution;
324
325 // operator== for Eigen::Tensor provides an Expression which might be not evaluated as a boolean
326 ✗ value &= Tensor<bool, 0>((data1.kinematic_hessians == data2.kinematic_hessians).all())(0)
327 ✗ && Tensor<bool, 0>((data1.d2tau_dqdq == data2.d2tau_dqdq).all())(0)
328 ✗ && Tensor<bool, 0>((data1.d2tau_dvdv == data2.d2tau_dvdv).all())(0)
329 ✗ && Tensor<bool, 0>((data1.d2tau_dqdv == data2.d2tau_dqdv).all())(0)
330 ✗ && Tensor<bool, 0>((data1.d2tau_dadq == data2.d2tau_dadq).all())(0);
331
332 ✗ return value;
333 }
334
335 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
336 ✗ bool operator!=(
337 const DataTpl<Scalar, Options, JointCollectionTpl> & data1,
338 const DataTpl<Scalar, Options, JointCollectionTpl> & data2)
339 {
340 ✗ return !(data1 == data2);
341 }
342
343 } // namespace pinocchio
344
345 /// @endcond
346
347 #endif // ifndef __pinocchio_multibody_data_hxx__
348