GCC Code Coverage Report

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