GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/joint-configuration.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 166 167 99.4%
Branches: 127 1128 11.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_joint_configuration_hxx__
6 #define __pinocchio_algorithm_joint_configuration_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
10
11 /* --- Details -------------------------------------------------------------------- */
12 namespace pinocchio
13 {
14
15 // --------------- API with return value as argument ---------------------- //
16
17 template<
18 typename LieGroup_t,
19 typename Scalar,
20 int Options,
21 template<typename, int>
22 class JointCollectionTpl,
23 typename ConfigVectorType,
24 typename TangentVectorType,
25 typename ReturnType>
26 2954 void integrate(
27 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
28 const Eigen::MatrixBase<ConfigVectorType> & q,
29 const Eigen::MatrixBase<TangentVectorType> & v,
30 const Eigen::MatrixBase<ReturnType> & qout)
31 {
32
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2954 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.
 2954 PINOCCHIO_CHECK_ARGUMENT_SIZE(
33 q.size(), model.nq, "The configuration vector is not of the right size");
34
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2954 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.
 2954 PINOCCHIO_CHECK_ARGUMENT_SIZE(
35 v.size(), model.nv, "The joint velocity vector is not of the right size");
36
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2954 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.
 2954 PINOCCHIO_CHECK_ARGUMENT_SIZE(
37 qout.size(), model.nq, "The output argument is not of the right size");
38
39 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
40 typedef typename Model::JointIndex JointIndex;
41 2954 ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
42
43 typedef IntegrateStep<LieGroup_t, ConfigVectorType, TangentVectorType, ReturnType> Algo;
44
1/2
✓ Branch 3 taken 2954 times.
✗ Branch 4 not taken.
 2954 typename Algo::ArgsType args(q.derived(), v.derived(), res);
45
2/2
✓ Branch 0 taken 65165 times.
✓ Branch 1 taken 2954 times.
 68119 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
46 {
47
2/4
✓ Branch 1 taken 65165 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 65165 times.
✗ Branch 6 not taken.
 65165 Algo::run(model.joints[i], args);
48 }
49 2954 }
50
51 template<
52 typename LieGroup_t,
53 typename Scalar,
54 int Options,
55 template<typename, int>
56 class JointCollectionTpl,
57 typename ConfigVectorIn1,
58 typename ConfigVectorIn2,
59 typename ReturnType>
60 2 void interpolate(
61 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
62 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
63 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
64 const Scalar & u,
65 const Eigen::MatrixBase<ReturnType> & qout)
66 {
67
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(
68 q0.size(), model.nq, "The first configuration vector is not of the right size");
69
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(
70 q1.size(), model.nq, "The second configuration vector is not of the right size");
71
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(
72 qout.size(), model.nq, "The output argument is not of the right size");
73
74 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
75 typedef typename Model::JointIndex JointIndex;
76 2 ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
77
78 typedef InterpolateStep<LieGroup_t, ConfigVectorIn1, ConfigVectorIn2, Scalar, ReturnType> Algo;
79
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 2 times.
 24 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
80 {
81
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 Algo::run(
82 44 model.joints[i], typename Algo::ArgsType(q0.derived(), q1.derived(), u, res.derived()));
83 }
84 2 }
85
86 template<
87 typename LieGroup_t,
88 typename Scalar,
89 int Options,
90 template<typename, int>
91 class JointCollectionTpl,
92 typename ConfigVectorIn1,
93 typename ConfigVectorIn2,
94 typename ReturnType>
95 125 void difference(
96 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
97 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
98 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
99 const Eigen::MatrixBase<ReturnType> & dvout)
100 {
101
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 125 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.
 125 PINOCCHIO_CHECK_ARGUMENT_SIZE(
102 q0.size(), model.nq, "The first configuration vector is not of the right size");
103
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 125 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.
 125 PINOCCHIO_CHECK_ARGUMENT_SIZE(
104 q1.size(), model.nq, "The second configuration vector is not of the right size");
105
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 125 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.
 125 PINOCCHIO_CHECK_ARGUMENT_SIZE(
106 dvout.size(), model.nv, "The output argument is not of the right size");
107
108 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
109 typedef typename Model::JointIndex JointIndex;
110 125 ReturnType & res = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout);
111
112 typedef DifferenceStep<LieGroup_t, ConfigVectorIn1, ConfigVectorIn2, ReturnType> Algo;
113
1/2
✓ Branch 3 taken 125 times.
✗ Branch 4 not taken.
 125 typename Algo::ArgsType args(q0.derived(), q1.derived(), res);
114
2/2
✓ Branch 0 taken 1375 times.
✓ Branch 1 taken 125 times.
 1500 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
115 {
116
2/4
✓ Branch 1 taken 1375 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1375 times.
✗ Branch 6 not taken.
 1375 Algo::run(model.joints[i], args);
117 }
118 125 }
119
120 template<
121 typename LieGroup_t,
122 typename Scalar,
123 int Options,
124 template<typename, int>
125 class JointCollectionTpl,
126 typename ConfigVectorIn1,
127 typename ConfigVectorIn2,
128 typename ReturnType>
129 1 void squaredDistance(
130 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
131 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
132 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
133 const Eigen::MatrixBase<ReturnType> & out)
134 {
135
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
136 q0.size(), model.nq, "The first configuration vector is not of the right size");
137
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
138 q1.size(), model.nq, "The second configuration vector is not of the right size");
139
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
140 out.size(), (model.njoints - 1), "The output argument is not of the right size");
141
142 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
143 typedef typename Model::JointIndex JointIndex;
144 1 ReturnType & distances = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out);
145
146 typedef SquaredDistanceStep<LieGroup_t, ConfigVectorIn1, ConfigVectorIn2, ReturnType> Algo;
147
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 1 times.
 12 for (JointIndex i = 0; i < (JointIndex)model.njoints - 1; ++i)
148 {
149
1/2
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 typename Algo::ArgsType args(i, q0.derived(), q1.derived(), distances.derived());
150
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 Algo::run(model.joints[i + 1], args);
151 }
152 1 }
153
154 template<
155 typename LieGroup_t,
156 typename Scalar,
157 int Options,
158 template<typename, int>
159 class JointCollectionTpl,
160 typename ConfigVectorIn1,
161 typename ConfigVectorIn2,
162 typename ReturnType>
163 2376 void randomConfiguration(
164 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
165 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
166 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
167 const Eigen::MatrixBase<ReturnType> & qout)
168 {
169
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2376 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.
 2376 PINOCCHIO_CHECK_ARGUMENT_SIZE(
170 lowerLimits.size(), model.nq, "The lower limits vector is not of the right size");
171
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2376 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.
 2376 PINOCCHIO_CHECK_ARGUMENT_SIZE(
172 upperLimits.size(), model.nq, "The upper limits vector is not of the right size");
173
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2376 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.
 2376 PINOCCHIO_CHECK_ARGUMENT_SIZE(
174 qout.size(), model.nq, "The output argument is not of the right size");
175
176 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
177 typedef typename Model::JointIndex JointIndex;
178 2376 ReturnType & q = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
179
180 typedef RandomConfigurationStep<LieGroup_t, ReturnType, ConfigVectorIn1, ConfigVectorIn2> Algo;
181
1/2
✓ Branch 4 taken 2376 times.
✗ Branch 5 not taken.
 2376 typename Algo::ArgsType args(
182 PINOCCHIO_EIGEN_CONST_CAST(ReturnType, q), lowerLimits.derived(), upperLimits.derived());
183
2/2
✓ Branch 0 taken 73188 times.
✓ Branch 1 taken 2376 times.
 75564 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
184 {
185
2/4
✓ Branch 1 taken 73188 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 73188 times.
✗ Branch 6 not taken.
 73188 Algo::run(model.joints[i], args);
186 }
187 2376 }
188
189 template<
190 typename LieGroup_t,
191 typename Scalar,
192 int Options,
193 template<typename, int>
194 class JointCollectionTpl,
195 typename ReturnType>
196 45 void neutral(
197 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
198 const Eigen::MatrixBase<ReturnType> & qout)
199 {
200
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 45 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.
 45 PINOCCHIO_CHECK_ARGUMENT_SIZE(
201 qout.size(), model.nq, "The output argument is not of the right size");
202
203 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
204 typedef typename Model::JointIndex JointIndex;
205 45 ReturnType & neutral_elt = PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout);
206
207
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 typename NeutralStep<LieGroup_t, ReturnType>::ArgsType args(neutral_elt.derived());
208
2/2
✓ Branch 0 taken 984 times.
✓ Branch 1 taken 45 times.
 1029 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
209 {
210
2/4
✓ Branch 1 taken 984 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 984 times.
✗ Branch 6 not taken.
 984 NeutralStep<LieGroup_t, ReturnType>::run(model.joints[i], args);
211 }
212 45 }
213
214 template<
215 typename LieGroup_t,
216 typename Scalar,
217 int Options,
218 template<typename, int>
219 class JointCollectionTpl,
220 typename ConfigVectorType,
221 typename TangentVectorType,
222 typename JacobianMatrixType>
223 22 void dIntegrate(
224 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
225 const Eigen::MatrixBase<ConfigVectorType> & q,
226 const Eigen::MatrixBase<TangentVectorType> & v,
227 const Eigen::MatrixBase<JacobianMatrixType> & J,
228 const ArgumentPosition arg,
229 const AssignmentOperatorType op)
230 {
231
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 22 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.
 22 PINOCCHIO_CHECK_ARGUMENT_SIZE(
232 q.size(), model.nq, "The configuration vector is not of the right size");
233
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 22 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.
 22 PINOCCHIO_CHECK_ARGUMENT_SIZE(
234 v.size(), model.nv, "The joint velocity vector is not of the right size");
235
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 22 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.
 22 PINOCCHIO_CHECK_ARGUMENT_SIZE(
236 J.rows(), model.nv, "The output argument is not of the right size");
237
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 22 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.
 22 PINOCCHIO_CHECK_ARGUMENT_SIZE(
238 J.cols(), model.nv, "The output argument is not of the right size");
239
240 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
241 typedef typename Model::JointIndex JointIndex;
242
243 typedef dIntegrateStep<LieGroup_t, ConfigVectorType, TangentVectorType, JacobianMatrixType>
244 Algo;
245
1/2
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
 22 typename Algo::ArgsType args(
246 22 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
247
2/2
✓ Branch 0 taken 242 times.
✓ Branch 1 taken 22 times.
 264 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
248 {
249
2/4
✓ Branch 1 taken 242 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 242 times.
✗ Branch 6 not taken.
 242 Algo::run(model.joints[i], args);
250 }
251 22 }
252
253 template<
254 typename LieGroup_t,
255 typename Scalar,
256 int Options,
257 template<typename, int>
258 class JointCollectionTpl,
259 typename ConfigVectorType,
260 typename TangentVectorType,
261 typename JacobianMatrixType1,
262 typename JacobianMatrixType2>
263 2 void dIntegrateTransport(
264 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
265 const Eigen::MatrixBase<ConfigVectorType> & q,
266 const Eigen::MatrixBase<TangentVectorType> & v,
267 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
268 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
269 const ArgumentPosition arg)
270 {
271
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(
272 q.size(), model.nq, "The configuration vector is not of the right size");
273
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(
274 v.size(), model.nv, "The joint velocity vector is not of the right size");
275
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(
276 Jin.rows(), model.nv, "The input matrix is not of the right size");
277
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
278 Jout.rows(), Jin.rows(), "The output argument should be the same size as input matrix");
279
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
280 Jout.cols(), Jin.cols(), "The output argument should be the same size as input matrix");
281
282 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
283 typedef typename Model::JointIndex JointIndex;
284
285 typedef dIntegrateTransportStep<
286 LieGroup_t, ConfigVectorType, TangentVectorType, JacobianMatrixType1, JacobianMatrixType2>
287 Algo;
288
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 typename Algo::ArgsType args(
289 q.derived(), v.derived(), Jin.derived(),
290 2 PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
291
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 2 times.
 24 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
292 {
293
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 22 Algo::run(model.joints[i], args);
294 }
295 2 }
296
297 template<
298 typename LieGroup_t,
299 typename Scalar,
300 int Options,
301 template<typename, int>
302 class JointCollectionTpl,
303 typename ConfigVectorType,
304 typename TangentVectorType,
305 typename JacobianMatrixType>
306 2 void dIntegrateTransport(
307 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
308 const Eigen::MatrixBase<ConfigVectorType> & q,
309 const Eigen::MatrixBase<TangentVectorType> & v,
310 const Eigen::MatrixBase<JacobianMatrixType> & J,
311 const ArgumentPosition arg)
312 {
313
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(
314 q.size(), model.nq, "The configuration vector is not of the right size");
315
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(
316 v.size(), model.nv, "The joint velocity vector is not of the right size");
317
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(J.rows(), model.nv, "The input matrix is not of the right size");
318
319 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
320 typedef typename Model::JointIndex JointIndex;
321
322 typedef dIntegrateTransportInPlaceStep<
323 LieGroup_t, ConfigVectorType, TangentVectorType, JacobianMatrixType>
324 Algo;
325
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 typename Algo::ArgsType args(
326 2 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
327
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 2 times.
 24 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
328 {
329
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 22 Algo::run(model.joints[i], args);
330 }
331 2 }
332
333 template<
334 typename LieGroup_t,
335 typename Scalar,
336 int Options,
337 template<typename, int>
338 class JointCollectionTpl,
339 typename ConfigVector1,
340 typename ConfigVector2,
341 typename JacobianMatrix>
342 6 void dDifference(
343 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
344 const Eigen::MatrixBase<ConfigVector1> & q0,
345 const Eigen::MatrixBase<ConfigVector2> & q1,
346 const Eigen::MatrixBase<JacobianMatrix> & J,
347 const ArgumentPosition arg)
348 {
349
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 6 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.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(
350 q0.size(), model.nq, "The configuration vector q0 is not of the right size");
351
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 6 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.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(
352 q1.size(), model.nq, "The configuration vector q1 is not of the right size");
353
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 6 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.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(
354 J.rows(), model.nv, "The output argument is not of the right size");
355
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 6 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.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(
356 J.cols(), model.nv, "The output argument is not of the right size");
357
358 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
359 typedef typename Model::JointIndex JointIndex;
360
361 typedef dDifferenceStep<LieGroup_t, ConfigVector1, ConfigVector2, JacobianMatrix> Algo;
362
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 typename Algo::ArgsType args(
363 6 q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
364
2/2
✓ Branch 0 taken 66 times.
✓ Branch 1 taken 6 times.
 72 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
365 {
366
2/4
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
 66 Algo::run(model.joints[i], args);
367 }
368 6 }
369
370 template<
371 typename LieGroup_t,
372 typename Scalar,
373 int Options,
374 template<typename, int>
375 class JointCollectionTpl,
376 typename ConfigVectorIn1,
377 typename ConfigVectorIn2>
378 2 Scalar squaredDistanceSum(
379 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
380 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
381 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
382 {
383
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(
384 q0.size(), model.nq, "The first configuration vector is not of the right size");
385
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(
386 q1.size(), model.nq, "The second configuration vector is not of the right size");
387
388 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
389 typedef typename Model::JointIndex JointIndex;
390 2 typename ConfigVectorIn1::Scalar squaredDistance = Scalar(0.0);
391
392 typedef SquaredDistanceSumStep<LieGroup_t, ConfigVectorIn1, ConfigVectorIn2, Scalar> Algo;
393
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 2 times.
 24 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
394 {
395
1/2
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
 22 typename Algo::ArgsType args(q0.derived(), q1.derived(), squaredDistance);
396
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 22 Algo::run(model.joints[i], args);
397 }
398
399 2 return squaredDistance;
400 }
401
402 template<
403 typename LieGroup_t,
404 typename Scalar,
405 int Options,
406 template<typename, int>
407 class JointCollectionTpl,
408 typename ConfigVectorIn1,
409 typename ConfigVectorIn2>
410 2 Scalar distance(
411 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
412 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
413 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
414 {
415 2 const Scalar & squaredDistance = squaredDistanceSum<
416
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
417 2 model, q0.derived(), q1.derived());
418 4 return math::sqrt(squaredDistance);
419 }
420
421 template<
422 typename LieGroup_t,
423 typename Scalar,
424 int Options,
425 template<typename, int>
426 class JointCollectionTpl,
427 typename ConfigVectorType>
428 11 void normalize(
429 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
430 const Eigen::MatrixBase<ConfigVectorType> & qout)
431 {
432
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 11 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.
 11 PINOCCHIO_CHECK_ARGUMENT_SIZE(
433 qout.size(), model.nq, "The output argument is not of the right size");
434
435 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
436 typedef typename Model::JointIndex JointIndex;
437
438 typedef NormalizeStep<LieGroup_t, ConfigVectorType> Algo;
439
2/2
✓ Branch 0 taken 169 times.
✓ Branch 1 taken 11 times.
 180 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
440 {
441
1/2
✓ Branch 1 taken 169 times.
✗ Branch 2 not taken.
 169 Algo::run(
442 169 model.joints[i],
443 338 typename Algo::ArgsType(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout)));
444 }
445 11 }
446
447 template<
448 typename LieGroup_t,
449 typename Scalar,
450 int Options,
451 template<typename, int>
452 class JointCollectionTpl,
453 typename ConfigVectorIn>
454 9 bool isNormalized(
455 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
456 const Eigen::MatrixBase<ConfigVectorIn> & q,
457 const Scalar & prec)
458 {
459
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 9 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.
 9 PINOCCHIO_CHECK_ARGUMENT_SIZE(
460 q.size(), model.nq, "The configuration vector is not of the right size");
461
2/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 9 PINOCCHIO_CHECK_INPUT_ARGUMENT(
462 (check_expression_if_real<Scalar, true>(prec >= 0)), "The precision should be positive");
463
464 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
465 typedef typename Model::JointIndex JointIndex;
466
467 9 bool result = true;
468 typedef IsNormalizedStep<LieGroup_t, ConfigVectorIn, Scalar> Algo;
469
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 typename Algo::ArgsType args(q.derived(), prec, result);
470
2/2
✓ Branch 0 taken 79 times.
✓ Branch 1 taken 7 times.
 86 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
471 {
472
2/4
✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 79 times.
✗ Branch 6 not taken.
 79 Algo::run(model.joints[i], args);
473
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 77 times.
 79 if (!result)
474 2 return false;
475 }
476
477 7 return true;
478 }
479
480 template<
481 typename LieGroup_t,
482 typename Scalar,
483 int Options,
484 template<typename, int>
485 class JointCollectionTpl,
486 typename ConfigVectorIn1,
487 typename ConfigVectorIn2>
488 3 bool isSameConfiguration(
489 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
490 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
491 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
492 const Scalar & prec)
493 {
494
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(
495 q1.size(), model.nq, "The first configuration vector is not of the right size");
496
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(
497 q2.size(), model.nq, "The second configuration vector is not of the right size");
498
2/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
499 (check_expression_if_real<Scalar, true>(prec >= 0)), "The precision should be positive");
500
501 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
502 typedef typename Model::JointIndex JointIndex;
503
504 3 bool result = true;
505 typedef IsSameConfigurationStep<LieGroup_t, ConfigVectorIn1, ConfigVectorIn2, Scalar> Algo;
506
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 typename Algo::ArgsType args(result, q1.derived(), q2.derived(), prec);
507
2/2
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 3 times.
 36 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
508 {
509
2/4
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 33 times.
✗ Branch 6 not taken.
 33 Algo::run(model.joints[i], args);
510
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33 times.
 33 if (!result)
511 ✗ return false;
512 }
513
514 3 return true;
515 }
516
517 template<
518 typename LieGroup_t,
519 typename Scalar,
520 int Options,
521 template<typename, int>
522 class JointCollectionTpl,
523 typename ConfigVector,
524 typename JacobianMatrix>
525 1 void integrateCoeffWiseJacobian(
526 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
527 const Eigen::MatrixBase<ConfigVector> & q,
528 const Eigen::MatrixBase<JacobianMatrix> & jacobian)
529 {
530
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
531 q.size(), model.nq, "The configuration vector is not of the right size");
532
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(jacobian.rows(), model.nq);
533
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
534 jacobian.cols(), model.nv, "The jacobian does not have the right dimension");
535
536 typedef IntegrateCoeffWiseJacobianStep<LieGroup_t, ConfigVector, JacobianMatrix> Algo;
537
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 typename Algo::ArgsType args(q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
538
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
539 {
540
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
 27 Algo::run(model.joints[i], args);
541 }
542 1 }
543
544 // ----------------- API that allocates memory ---------------------------- //
545
546 template<
547 typename LieGroup_t,
548 typename Scalar,
549 int Options,
550 template<typename, int>
551 class JointCollectionTpl,
552 typename ConfigVectorType,
553 typename TangentVectorType>
554 2954 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
555 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
556 const Eigen::MatrixBase<ConfigVectorType> & q,
557 const Eigen::MatrixBase<TangentVectorType> & v)
558 {
559 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) ReturnType;
560 2954 ReturnType res(model.nq);
561 integrate<
562 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
563
1/2
✓ Branch 3 taken 2954 times.
✗ Branch 4 not taken.
 2954 ReturnType>(model, q.derived(), v.derived(), res);
564 2954 return res;
565 }
566
567 template<
568 typename LieGroup_t,
569 typename Scalar,
570 int Options,
571 template<typename, int>
572 class JointCollectionTpl,
573 typename ConfigVectorIn1,
574 typename ConfigVectorIn2>
575 2 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
576 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
577 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
578 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
579 const Scalar & u)
580 {
581 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
582 2 ReturnType res(model.nq);
583 interpolate<
584 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
585
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 ReturnType>(model, q0.derived(), q1.derived(), u, res);
586 2 return res;
587 }
588
589 template<
590 typename LieGroup_t,
591 typename Scalar,
592 int Options,
593 template<typename, int>
594 class JointCollectionTpl,
595 typename ConfigVectorIn1,
596 typename ConfigVectorIn2>
597 125 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
598 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
599 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
600 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
601 {
602 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
603 125 ReturnType res(model.nv);
604 difference<
605 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
606
1/2
✓ Branch 3 taken 125 times.
✗ Branch 4 not taken.
 125 ReturnType>(model, q0.derived(), q1.derived(), res);
607 125 return res;
608 }
609
610 template<
611 typename LieGroup_t,
612 typename Scalar,
613 int Options,
614 template<typename, int>
615 class JointCollectionTpl,
616 typename ConfigVectorIn1,
617 typename ConfigVectorIn2>
618 1 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
619 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
620 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
621 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
622 {
623 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
624
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ReturnType distances(ReturnType::Zero(model.njoints - 1));
625 squaredDistance<
626 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
627
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ReturnType>(model, q0.derived(), q1.derived(), distances);
628 1 return distances;
629 }
630
631 template<
632 typename LieGroup_t,
633 typename Scalar,
634 int Options,
635 template<typename, int>
636 class JointCollectionTpl,
637 typename ConfigVectorIn1,
638 typename ConfigVectorIn2>
639 typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
640 (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
641 2376 randomConfiguration(
642 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
643 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
644 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
645 {
646 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
647 (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType)) ReturnType;
648 2376 ReturnType q(model.nq);
649 randomConfiguration<
650 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
651
1/2
✓ Branch 3 taken 2376 times.
✗ Branch 4 not taken.
 2376 ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), q);
652 2376 return q;
653 }
654
655 template<
656 typename LieGroup_t,
657 typename Scalar,
658 int Options,
659 template<typename, int>
660 class JointCollectionTpl>
661 typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
662 (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
663 2373 randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
664 {
665 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
666 typedef typename Model::ConfigVectorType ConfigVectorType;
667 return randomConfiguration<
668 LieGroup_t, Scalar, Options, JointCollectionTpl, ConfigVectorType, ConfigVectorType>(
669 2373 model, model.lowerPositionLimit, model.upperPositionLimit);
670 }
671
672 template<
673 typename LieGroup_t,
674 typename Scalar,
675 int Options,
676 template<typename, int>
677 class JointCollectionTpl>
678 Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
679 23 neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
680 {
681 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ReturnType;
682 23 ReturnType q(model.nq);
683
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 neutral<LieGroup_t, Scalar, Options, JointCollectionTpl, ReturnType>(model, q);
684 23 return q;
685 }
686
687 } // namespace pinocchio
688
689 #endif // ifndef __pinocchio_algorithm_joint_configuration_hxx__
690