GCC Code Coverage Report


Directory: ./
File: src/explicit/implicit-function.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 88 166 53.0%
Branches: 75 382 19.6%

Line Branch Exec Source
1 // Copyright (c) 2015 - 2018 LAAS-CNRS
2 // Authors: Florent Lamiraux, Joseph Mirabel
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <Eigen/Geometry>
30 #include <hpp/constraints/differentiable-function.hh>
31 #include <hpp/constraints/explicit/implicit-function.hh>
32 #include <hpp/constraints/matrix-view.hh>
33 #include <hpp/constraints/serialization.hh>
34 #include <hpp/constraints/tools.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/joint.hh>
37 #include <hpp/pinocchio/liegroup-space.hh>
38 #include <hpp/pinocchio/serialization.hh>
39 #include <hpp/util/serialization.hh>
40
41 namespace hpp {
42 namespace constraints {
43 namespace explicit_ {
44 typedef Eigen::Map<Quaternion_t> QuatMap_t;
45 typedef Eigen::Map<const Quaternion_t> QuatConstMap_t;
46 typedef hpp::pinocchio::liegroup::VectorSpaceOperation<3, false> R3;
47 typedef hpp::pinocchio::liegroup::SpecialOrthogonalOperation<3> SO3;
48 typedef hpp::pinocchio::liegroup::CartesianProductOperation<R3, SO3> R3xSO3;
49 typedef pinocchio::liegroup::SpecialEuclideanOperation<3> SE3;
50 typedef hpp::pinocchio::LiegroupType LiegroupType;
51
52 struct JacobianVisitor : public boost::static_visitor<> {
53 JacobianVisitor(vectorIn_t qOut, vectorIn_t f_qIn, matrixIn_t Jf,
54 const Eigen::MatrixBlocks<false, false>& outJacobian,
55 const Eigen::MatrixBlocks<false, false>& inJacobian,
56 matrixOut_t result)
57 : qOut_(qOut),
58 f_qIn_(f_qIn),
59 Jf_(Jf),
60 outJacobian_(outJacobian),
61 inJacobian_(inJacobian),
62 result_(result) {}
63
64 template <typename LgT>
65 void operator()(const LgT&);
66
67 vectorIn_t qOut_, f_qIn_;
68 matrixIn_t Jf_;
69 const Eigen::MatrixBlocks<false, false>& outJacobian_;
70 const Eigen::MatrixBlocks<false, false>& inJacobian_;
71 matrixOut_t result_;
72 }; // struct JacobianVisitor
73
74 template <>
75 inline void JacobianVisitor::operator()<R3xSO3>(const R3xSO3&) {
76 using Eigen::BlockIndex;
77 using Eigen::MatrixBlocks;
78 typedef hpp::constraints::BlockIndex BlockIndex;
79 hppDout(info, "result_ = " << std::endl << result_);
80 // Fill R^3 part
81 assert(outJacobian_.nbRows() == 6);
82 Eigen::MatrixBlocks<false, false> tmp(outJacobian_.block(0, 0, 3, 3));
83 tmp.lview(result_) = matrix3_t::Identity();
84 hppDout(info, "result_ = " << std::endl << result_);
85 // extract 3 top rows of inJacobian_
86 segments_t cols(inJacobian_.cols());
87 MatrixBlocks<false, false> inJacobian(
88 inJacobian_.block(0, 0, 3, BlockIndex::cardinal(cols)));
89 inJacobian.lview(result_) = -Jf_.topRows<3>();
90 hppDout(info, "result_ = " << std::endl << result_);
91 // Fill SO(3) part
92 // extract 3 bottom rows of inJacobian_
93 inJacobian = inJacobian_.block(3, 0, 3, BlockIndex::cardinal(cols));
94 // extract 3x3 bottom left part of outJacobian_
95 MatrixBlocks<false, false> outJacobian(outJacobian_.block(3, 3, 3, 3));
96 assert(qOut_.size() == 7);
97 assert(f_qIn_.size() == 7);
98 matrix3_t R_out(Eigen::Quaterniond(qOut_.tail<4>()).toRotationMatrix());
99 matrix3_t R_f(Eigen::Quaterniond(f_qIn_.tail<4>()).toRotationMatrix());
100 // \f$R_f^T R_{out}\f$
101 matrix3_t R_f_T_R_out(R_f.transpose() * R_out);
102 matrix3_t Jlog_R_f_T_R_out;
103 vector3_t r;
104 value_type theta;
105 constraints::logSO3(R_f_T_R_out, theta, r);
106 constraints::JlogSO3(theta, r, Jlog_R_f_T_R_out);
107 outJacobian.lview(result_) = Jlog_R_f_T_R_out;
108 hppDout(info, "result_ = " << std::endl << result_);
109 inJacobian.lview(result_) =
110 -Jlog_R_f_T_R_out * R_out.transpose() * R_f * Jf_.bottomRows<3>();
111 hppDout(info, "result_ = " << std::endl << result_);
112 }
113
114 template <>
115 inline void JacobianVisitor::operator()<SE3>(const SE3&) {
116 assert(outJacobian_.nbRows() == 6);
117 // extract 3 top rows of inJacobian_
118 assert(qOut_.size() == 7);
119 assert(f_qIn_.size() == 7);
120 matrix3_t Rout(Eigen::Quaterniond(qOut_.tail<4>()).toRotationMatrix());
121 vector3_t pOut(qOut_.head<3>());
122 Transform3s Mout(Rout, pOut);
123 matrix3_t Rf(Eigen::Quaterniond(f_qIn_.tail<4>()).toRotationMatrix());
124 vector3_t pf(f_qIn_.head<3>());
125 Transform3s Mf(Rf, pf);
126 // \f$Mf^{-1} M_{out}\f$
127 Transform3s Mf_inverse_Mout(Mf.inverse() * Mout);
128 matrix6_t Jlog_Mf_inverse_Mout;
129 constraints::JlogSE3(Mf_inverse_Mout, Jlog_Mf_inverse_Mout);
130 outJacobian_.lview(result_) = Jlog_Mf_inverse_Mout;
131 JointJacobian_t inJ(6, Jf_.cols());
132 inJ.topRows<3>().noalias() =
133 (Rout.transpose() * ::pinocchio::skew(pOut - pf) * Rf) *
134 Jf_.bottomRows<3>();
135 inJ.topRows<3>().noalias() -= (Rout.transpose() * Rf) * Jf_.topRows<3>();
136 inJ.bottomRows<3>().noalias() =
137 (-Rout.transpose() * Rf) * Jf_.bottomRows<3>();
138 inJacobian_.lview(result_) = Jlog_Mf_inverse_Mout * inJ;
139 }
140
141 template <typename LgT>
142 void JacobianVisitor::operator()(const LgT&) {
143 outJacobian_.lview(result_).setIdentity();
144 inJacobian_.lview(result_) = -Jf_;
145 }
146
147 53 ImplicitFunctionPtr_t ImplicitFunction::create(
148 const LiegroupSpacePtr_t& configSpace,
149 const DifferentiableFunctionPtr_t& function, const segments_t& inputConf,
150 const segments_t& outputConf, const segments_t& inputVelocity,
151 const segments_t& outputVelocity) {
152 ImplicitFunction* ptr =
153 new ImplicitFunction(configSpace, function, inputConf, outputConf,
154
1/2
✓ Branch 2 taken 53 times.
✗ Branch 3 not taken.
53 inputVelocity, outputVelocity);
155 53 return ImplicitFunctionPtr_t(ptr);
156 }
157
158 const DifferentiableFunctionPtr_t& ImplicitFunction::inputToOutput() const {
159 return inputToOutput_;
160 }
161
162 53 ImplicitFunction::ImplicitFunction(const LiegroupSpacePtr_t& configSpace,
163 const DifferentiableFunctionPtr_t& function,
164 const segments_t& inputConf,
165 const segments_t& outputConf,
166 const segments_t& inputVelocity,
167 53 const segments_t& outputVelocity)
168 : DifferentiableFunction(configSpace->nq(), configSpace->nv(),
169
1/2
✓ Branch 5 taken 53 times.
✗ Branch 6 not taken.
106 LiegroupSpace::Rn(function->outputSpace()->nv()),
170 106 "implicit " + function->name()),
171 53 robot_(),
172 53 inputToOutput_(function),
173
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 inputConfIntervals_(inputConf),
174
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 outputConfIntervals_(outputConf),
175
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 inputDerivIntervals_(inputVelocity),
176
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 outputDerivIntervals_(outputVelocity),
177 53 outJacobian_(),
178 53 inJacobian_(),
179
1/2
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
53 f_qIn_(function->outputSpace()),
180
1/2
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
53 qOut_(function->outputSpace()),
181
4/8
✓ Branch 6 taken 53 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 53 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 53 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 53 times.
✗ Branch 19 not taken.
265 result_(outputSpace()) {
182 // Check input consistency
183 // Each configuration variable is either input or output
184
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 53 times.
53 assert(function->inputSize() + function->outputSize() <= configSpace->nq());
185 // Each velocity variable is either input or output
186
1/2
✗ Branch 6 not taken.
✓ Branch 7 taken 53 times.
53 assert(function->inputDerivativeSize() + function->outputDerivativeSize() <=
187 configSpace->nv());
188
1/2
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
53 qIn_.resize(function->inputSize());
189
1/2
✓ Branch 5 taken 53 times.
✗ Branch 6 not taken.
53 Jf_.resize(function->outputDerivativeSize(), function->inputDerivativeSize());
190
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 53 times.
53 assert(BlockIndex::cardinal(outputConf) == function->outputSize());
191 // Sum of velocity output interval sizes equal function output
192 // derivative size
193
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 53 times.
53 assert(BlockIndex::cardinal(outputVelocity) ==
194 function->outputDerivativeSize());
195
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 computeJacobianBlocks();
196
197
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 activeParameters_.setConstant(false);
198
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 activeDerivativeParameters_.setConstant(false);
199
3/6
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 53 times.
✗ Branch 8 not taken.
53 inputConfIntervals_.lview(activeParameters_.matrix()).setConstant(true);
200
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 inputDerivIntervals_.lview(activeDerivativeParameters_.matrix())
201
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 .setConstant(true);
202
3/6
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 53 times.
✗ Branch 8 not taken.
53 outputConfIntervals_.lview(activeParameters_.matrix()).setConstant(true);
203
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 outputDerivIntervals_.lview(activeDerivativeParameters_.matrix())
204
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 .setConstant(true);
205 53 }
206
207 14 void ImplicitFunction::impl_compute(LiegroupElementRef result,
208 vectorIn_t argument) const {
209 using Eigen::MatrixBlocks;
210 hppDout(info, "argument=" << argument.transpose());
211 // Store q_{output} in result
212
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
14 qOut_.vector() = outputConfIntervals_.rview(argument);
213 hppDout(info, "qOut_=" << qOut_);
214 // fill in q_{input}
215
1/2
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
14 qIn_ = inputConfIntervals_.rview(argument);
216 hppDout(info, "qIn_=" << qIn_);
217 // compute f (q_{input}) -> output_
218
2/4
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14 times.
✗ Branch 7 not taken.
14 inputToOutput_->value(f_qIn_, qIn_);
219 hppDout(info, "f_qIn_=" << f_qIn_);
220
1/2
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
14 result.vector() = qOut_ - f_qIn_;
221 hppDout(info, "result=" << result);
222 14 }
223
224 void ImplicitFunction::impl_jacobian(matrixOut_t jacobian,
225 vectorIn_t arg) const {
226 jacobian.setZero();
227 size_type iq = 0, iv = 0, nq, nv;
228 std::size_t rank = 0;
229 impl_compute(result_, arg);
230 inputToOutput_->jacobian(Jf_, qIn_);
231 hppDout(info, "Jf_=" << std::endl << Jf_);
232 // Fill Jacobian by set of lines corresponding to the types of Lie group
233 // that compose the outputspace of input to output function.
234 for (std::vector<LiegroupType>::const_iterator it =
235 inputToOutput_->outputSpace()->liegroupTypes().begin();
236 it != inputToOutput_->outputSpace()->liegroupTypes().end(); ++it) {
237 nq = inputToOutput_->outputSpace()->nq(rank);
238 nv = inputToOutput_->outputSpace()->nv(rank);
239 JacobianVisitor v(qOut_.vector().segment(iq, nq),
240 f_qIn_.vector().segment(iq, nq), Jf_.middleRows(iv, nv),
241 outJacobian_[rank], inJacobian_[rank],
242 jacobian.middleRows(iv, nv));
243 boost::apply_visitor(v, *it);
244 iq += nq;
245 iv += nv;
246 ++rank;
247 }
248 }
249
250 53 void ImplicitFunction::computeJacobianBlocks() {
251
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 segments_t remainingCols(outputDerivIntervals_.indices());
252 53 segments_t cols;
253 53 size_type iv = 0, nv;
254 53 std::size_t rank = 0;
255 53 for (std::vector<LiegroupType>::const_iterator it =
256 53 inputToOutput_->outputSpace()->liegroupTypes().begin();
257
2/2
✓ Branch 8 taken 53 times.
✓ Branch 9 taken 53 times.
106 it != inputToOutput_->outputSpace()->liegroupTypes().end(); ++it) {
258
1/2
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 nv = inputToOutput_->outputSpace()->nv(rank);
259
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 cols = BlockIndex::split(remainingCols, nv);
260
2/4
✓ Branch 2 taken 53 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 53 times.
✗ Branch 6 not taken.
53 segments_t rows(1, std::make_pair(iv, nv));
261
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 outJacobian_.push_back(Eigen::MatrixBlocks<false, false>(rows, cols));
262
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
53 inJacobian_.push_back(Eigen::MatrixBlocks<false, false>(
263
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
53 rows, inputDerivIntervals_.indices()));
264 //
265 53 iv += nv;
266 53 ++rank;
267 53 }
268 53 }
269
270 template <class Archive>
271 4 void ImplicitFunction::serialize(Archive& ar, const unsigned int version) {
272 (void)version;
273
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
4 ar& boost::serialization::make_nvp(
274 4 "base", boost::serialization::base_object<DifferentiableFunction>(*this));
275 // ar & BOOST_SERIALIZATION_NVP(robot_);
276
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(inputToOutput_);
277
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(inputConfIntervals_);
278
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(outputConfIntervals_);
279
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(inputDerivIntervals_);
280
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(outputDerivIntervals_);
281
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(outJacobian_);
282
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
4 ar& BOOST_SERIALIZATION_NVP(inJacobian_);
283
284 if (!Archive::is_saving::value) { // loading
285
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 f_qIn_ = LiegroupElement(inputToOutput_->outputSpace());
286
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 qOut_ = LiegroupElement(inputToOutput_->outputSpace());
287
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 result_ = LiegroupElement(outputSpace());
288 2 qIn_.resize(inputToOutput_->inputSize());
289 2 Jf_.resize(inputToOutput_->outputDerivativeSize(),
290 2 inputToOutput_->inputDerivativeSize());
291 // should we recompute them instead of storing them ?
292 // computeJacobianBlocks ();
293 }
294 }
295
296 std::pair<JointConstPtr_t, JointConstPtr_t>
297 1 ImplicitFunction::dependsOnRelPoseBetween(DeviceConstPtr_t robot) const {
298
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
1 assert(robot);
299 // check that this is a constant function
300
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
1 if (inputToOutput_->inputSize() != 0) {
301 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
302 }
303
304 // get the joints involved in the output config
305 1 JointConstPtr_t j1;
306 // check that output interval matches with the config range of one joint
307
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
1 if (outputConfIntervals_.rows().size() != 1) {
308 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
309 }
310 1 segment_t row = outputConfIntervals_.rows()[0];
311
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 j1 = robot->getJointAtConfigRank(row.first);
312
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
1 if (!j1 || row.second != j1->configSize()) {
313 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
314 }
315
316
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 JointConstPtr_t j2 = j1->parentJoint();
317 1 size_type index1 = j1->index();
318
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
1 size_type index2 = (j2 ? j2->index() : 0);
319
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
1 if (index1 <= index2) {
320 return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
321 } else {
322 1 return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
323 }
324 1 }
325
326 HPP_SERIALIZATION_IMPLEMENT(ImplicitFunction);
327 } // namespace explicit_
328 } // namespace constraints
329 } // namespace hpp
330
331 BOOST_CLASS_EXPORT_IMPLEMENT(hpp::constraints::explicit_::ImplicitFunction)
332