| 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 |