GCC Code Coverage Report

Directory: ./
File: include/sot/core/feature-pose.hxx
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 135 163 82.8%
Branches: 223 488 45.7%
Line Branch Exec Source
1 /*
2 * Copyright 2019
3 * Joseph Mirabel
4 *
5 * LAAS-CNRS
6 *
7 */
8
9 /* --------------------------------------------------------------------- */
10 /* --- INCLUDE --------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12
13 /* --- SOT --- */
14 #include <dynamic-graph/command-bind.h>
15 #include <dynamic-graph/command-getter.h>
16 #include <dynamic-graph/command-setter.h>
17 #include <dynamic-graph/command.h>
18
19 #include <Eigen/LU>
20 #include <boost/mpl/if.hpp>
21 #include <boost/type_traits/is_same.hpp>
22 #include <pinocchio/multibody/liegroup/liegroup.hpp>
23 #include <sot/core/debug.hh>
24 #include <sot/core/factory.hh>
25 #include <sot/core/feature-pose.hh>
26
27 namespace dynamicgraph {
28 namespace sot {
29
30 typedef pinocchio::CartesianProductOperation<
31 pinocchio::VectorSpaceOperationTpl<3, double>,
32 pinocchio::SpecialOrthogonalOperationTpl<3, double> >
33 R3xSO3_t;
34 typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
35
36 namespace internal {
37 template <Representation_t representation>
38 struct LG_t {
39 typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
40 R3xSO3_t>::type type;
41 };
42 } // namespace internal
43
44 /* --------------------------------------------------------------------- */
45 /* --- CLASS ----------------------------------------------------------- */
46 /* --------------------------------------------------------------------- */
47
48 static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());
49
50 template <Representation_t representation>
51 8 FeaturePose<representation>::FeaturePose(const std::string &pointName)
52 : FeatureAbstract(pointName),
53
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
54
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
55
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
56
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
57
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
58
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"),
59
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 faMfbDes(NULL,
60
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
61
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 faNufafbDes(NULL,
62
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"),
63
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 24 faMfb(
64 boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
65
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 16 oMja << jaMfa << oMjb << jbMfb,
66
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"),
67
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
68 _2),
69
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
70
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
71 this, _1, _2),
72 faMfbDes,
73
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 16 CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
74
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 oMja.setConstant(Id);
75
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 jaMfa.setConstant(Id);
76
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 jbMfb.setConstant(Id);
77
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 faMfbDes.setConstant(Id);
78
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 faNufafbDes.setConstant(Vector::Zero(6));
79
80
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);
81
82
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 errorSOUT.addDependencies(q_faMfbDes << q_faMfb);
83
84
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
85
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes);
86
87
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 errordotSOUT.setFunction(
88 boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
89
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);
90
91 // Commands
92 //
93 {
94 using namespace dynamicgraph::command;
95
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 addCommand("keep",
96
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
 16 makeCommandVoid1(
97 *this, &FeaturePose<representation>::servoCurrentPosition,
98 docCommandVoid1(
99 "modify the desired position to servo at current pos.",
100 "time")));
101 }
102 8 }
103
104 template <Representation_t representation>
105 8 FeaturePose<representation>::~FeaturePose() {}
106
107 /* --------------------------------------------------------------------- */
108 /* --------------------------------------------------------------------- */
109 /* --------------------------------------------------------------------- */
110
111 template <Representation_t representation>
112 3936 static inline void check(const FeaturePose<representation> &ft) {
113 (void)ft;
114
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.oMja.isPlugged());
115
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.jaMfa.isPlugged());
116
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.oMjb.isPlugged());
117
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.jbMfb.isPlugged());
118
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.faMfbDes.isPlugged());
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1968 times.
 3936 assert(ft.faNufafbDes.isPlugged());
120 3936 }
121
122 template <Representation_t representation>
123 8 unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
124 int time) {
125 sotDEBUG(25) << "# In {" << std::endl;
126
127 8 const Flags &fl = selectionSIN.access(time);
128
129 8 dim = 0;
130
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 4 times.
 56 for (int i = 0; i < 6; ++i)
131
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 24 times.
✗ Branch 4 not taken.
 48 if (fl(i)) dim++;
132
133 sotDEBUG(25) << "# Out }" << std::endl;
134 8 return dim;
135 }
136
137 640 void toVector(const MatrixHomogeneous &M, Vector7 &v) {
138
2/4
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 640 times.
✗ Branch 6 not taken.
 640 v.head<3>() = M.translation();
139
3/6
✓ Branch 2 taken 640 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 640 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 640 times.
✗ Branch 10 not taken.
 640 QuaternionMap(v.tail<4>().data()) = M.linear();
140 640 }
141
142 ✗ Vector7 toVector(const MatrixHomogeneous &M) {
143 ✗ Vector7 ret;
144 ✗ toVector(M, ret);
145 ✗ return ret;
146 }
147
148 template <Representation_t representation>
149 64 Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
150 typedef typename internal::LG_t<representation>::type LieGroup_t;
151
152
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 64 check(*this);
153
154
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_faMfb.recompute(time);
155
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_faMfbDes.recompute(time);
156
157
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const unsigned int &dim = dimensionSOUT(time);
158
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const Flags &fl = selectionSIN(time);
159
160
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const Matrix &_jbJjb = jbJjb(time);
161
162 64 const MatrixHomogeneous &_jbMfb =
163
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
164
165 64 const Matrix::Index cJ = _jbJjb.cols();
166
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 J.resize(dim, cJ);
167
168
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 MatrixTwist X;
169
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
170
171
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 buildFrom(_jbMfb.inverse(Eigen::Affine), X);
172
5/10
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
 128 MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
173
4/8
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 128 oMja.access(time).rotation().transpose() *
174
4/8
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 160 oMjb.access(time).rotation() * _jbMfb.rotation();
175 if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
176
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 X.topRows<3>().applyOnTheLeft(faRfb);
177
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 128 LieGroup_t().template dDifference<pinocchio::ARG1>(
178 64 q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
179
180 // Contribution of b:
181 // J = Jminus * X * jbJjb;
182 64 unsigned int rJ = 0;
183
2/2
✓ Branch 0 taken 192 times.
✓ Branch 1 taken 32 times.
 448 for (unsigned int r = 0; r < 6; ++r)
184
7/14
✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 192 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 192 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 192 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 192 times.
✗ Branch 19 not taken.
 384 if (fl((int)r)) J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
185
186
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 16 times.
 64 if (jaJja.isPlugged()) {
187
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 const Matrix &_jaJja = jaJja(time);
188 32 const MatrixHomogeneous &_jaMfa =
189
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
190
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 _faMfb = faMfb.accessCopy();
191
192
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 32 buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
193 if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
194
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 X.topRows<3>().applyOnTheLeft(faRfb);
195
196 // J -= (Jminus * X) * jaJja(time);
197 32 rJ = 0;
198
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 16 times.
 224 for (unsigned int r = 0; r < 6; ++r)
199
8/16
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 96 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 96 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 96 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 96 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 96 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 96 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 96 times.
✗ Branch 22 not taken.
 192 if (fl((int)r)) J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
200 }
201
202 64 return J;
203 }
204
205 template <Representation_t representation>
206 1184 MatrixHomogeneous &FeaturePose<representation>::computefaMfb(
207 MatrixHomogeneous &res, int time) {
208 1184 check(*this);
209
210
4/8
✓ Branch 6 taken 592 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 592 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 592 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 592 times.
✗ Branch 16 not taken.
 1184 res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
211 jbMfb(time);
212 1184 return res;
213 }
214
215 template <Representation_t representation>
216 1184 Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
217 1184 check(*this);
218
219 1184 toVector(faMfb(time), res);
220 1184 return res;
221 }
222
223 template <Representation_t representation>
224 96 Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
225 96 check(*this);
226
227 96 toVector(faMfbDes(time), res);
228 96 return res;
229 }
230
231 template <Representation_t representation>
232 1184 Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
233 typedef typename internal::LG_t<representation>::type LieGroup_t;
234
1/2
✓ Branch 1 taken 296 times.
✗ Branch 2 not taken.
 1184 check(*this);
235
236
1/2
✓ Branch 1 taken 592 times.
✗ Branch 2 not taken.
 1184 const Flags &fl = selectionSIN(time);
237
238
1/2
✓ Branch 1 taken 592 times.
✗ Branch 2 not taken.
 1184 Eigen::Matrix<double, 6, 1> v;
239
4/8
✓ Branch 1 taken 592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 592 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 592 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 592 times.
✗ Branch 11 not taken.
 1184 LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);
240
241
2/4
✓ Branch 1 taken 592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 592 times.
✗ Branch 5 not taken.
 1184 error.resize(dimensionSOUT(time));
242 1184 unsigned int cursor = 0;
243
2/2
✓ Branch 0 taken 3552 times.
✓ Branch 1 taken 592 times.
 8288 for (unsigned int i = 0; i < 6; ++i)
244
4/8
✓ Branch 1 taken 3552 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3552 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3552 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3552 times.
✗ Branch 10 not taken.
 7104 if (fl((int)i)) error(cursor++) = v(i);
245
246 1184 return error;
247 }
248
249 // This function is responsible of converting the input velocity expressed with
250 // SE(3) convention onto a velocity expressed with the convention of this
251 // feature (R^3xSO(3) or SE(3)), in the right frame.
252 template <>
253 56 Vector6d convertVelocity<SE3_t>(const MatrixHomogeneous &M,
254 const MatrixHomogeneous &Mdes,
255 const Vector &faNufafbDes) {
256 (void)M;
257
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 MatrixTwist X;
258
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 buildFrom(Mdes.inverse(Eigen::Affine), X);
259
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 112 return X * faNufafbDes;
260 }
261 template <>
262 56 Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
263 const MatrixHomogeneous &Mdes,
264 const Vector &faNufafbDes) {
265 56 Vector6d nu;
266
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 nu.head<3>() =
267
5/10
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 56 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 56 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 56 times.
✗ Branch 15 not taken.
 112 faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
268
5/10
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 56 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 56 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 56 times.
✗ Branch 15 not taken.
 56 nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
269 56 return nu;
270 }
271
272 template <Representation_t representation>
273 224 Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
274 int time) {
275 typedef typename internal::LG_t<representation>::type LieGroup_t;
276
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 224 check(*this);
277
278
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 224 errordot.resize(dimensionSOUT(time));
279
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 const Flags &fl = selectionSIN(time);
280
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 112 times.
 224 if (!faNufafbDes.isPlugged()) {
281 ✗ errordot.setZero();
282 ✗ return errordot;
283 }
284
285
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 q_faMfb.recompute(time);
286
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 q_faMfbDes.recompute(time);
287
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 faNufafbDes.recompute(time);
288
289
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 const MatrixHomogeneous &_faMfbDes = faMfbDes(time);
290
291
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
292
293
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 448 LieGroup_t().template dDifference<pinocchio::ARG0>(
294 224 q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
295
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
296
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 224 faNufafbDes.accessCopy());
297 224 unsigned int cursor = 0;
298
2/2
✓ Branch 0 taken 672 times.
✓ Branch 1 taken 112 times.
 1568 for (unsigned int i = 0; i < 6; ++i)
299
6/12
✓ Branch 1 taken 672 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 672 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 672 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 672 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 672 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 672 times.
✗ Branch 16 not taken.
 1344 if (fl((int)i)) errordot(cursor++) = Jminus.row(i) * nu;
300
301 224 return errordot;
302 }
303
304 /* Modify the value of the reference (sdes) so that it corresponds
305 * to the current position. The effect on the servo is to maintain the
306 * current position and correct any drift. */
307 template <Representation_t representation>
308 ✗ void FeaturePose<representation>::servoCurrentPosition(const int &time) {
309 ✗ check(*this);
310
311 ✗ const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
312 ✗ _jaMfa =
313 ✗ (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
314 ✗ _oMjb = oMjb.access(time),
315 ✗ _jbMfb =
316 ✗ (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
317 ✗ faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
318 }
319
320 static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
321 template <Representation_t representation>
322 ✗ void FeaturePose<representation>::display(std::ostream &os) const {
323 ✗ os << CLASS_NAME << "<" << name << ">: (";
324
325 try {
326 ✗ const Flags &fl = selectionSIN.accessCopy();
327 ✗ bool first = true;
328 ✗ for (int i = 0; i < 6; ++i)
329 ✗ if (fl(i)) {
330 ✗ if (first) {
331 ✗ first = false;
332 } else {
333 ✗ os << ",";
334 }
335 ✗ os << featureNames[i];
336 }
337 ✗ os << ") ";
338 ✗ } catch (ExceptionAbstract e) {
339 ✗ os << " selectSIN not set.";
340 }
341 }
342
343 } // namespace sot
344 } // namespace dynamicgraph
345