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 |