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 |