| Directory: | ./ |
|---|---|
| File: | src/differentiable-function.cc |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 77 | 111 | 69.4% |
| Branches: | 80 | 230 | 34.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2015, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 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 <boost/serialization/string.hpp> | ||
| 30 | #include <hpp/constraints/differentiable-function.hh> | ||
| 31 | #include <hpp/pinocchio/configuration.hh> | ||
| 32 | #include <hpp/pinocchio/device.hh> | ||
| 33 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 34 | #include <hpp/pinocchio/joint.hh> | ||
| 35 | #include <hpp/pinocchio/liegroup.hh> | ||
| 36 | #include <hpp/pinocchio/serialization.hh> | ||
| 37 | #include <hpp/util/serialization.hh> | ||
| 38 | #include <pinocchio/multibody/liegroup/liegroup.hpp> | ||
| 39 | |||
| 40 | BOOST_CLASS_EXPORT(hpp::constraints::DifferentiableFunction) | ||
| 41 | |||
| 42 | namespace hpp { | ||
| 43 | namespace constraints { | ||
| 44 | namespace { | ||
| 45 | using hpp::pinocchio::DefaultLieGroupMap; | ||
| 46 | typedef std::vector<pinocchio::JointIndex> JointIndexVector; | ||
| 47 | |||
| 48 | struct FiniteDiffRobotOp { | ||
| 49 | 105 | FiniteDiffRobotOp(const DevicePtr_t& r, const value_type& epsilon) | |
| 50 | 105 | : robot(r), | |
| 51 | 105 | model(robot->model()), | |
| 52 | 105 | epsilon(epsilon), | |
| 53 |
1/2✓ Branch 3 taken 105 times.
✗ Branch 4 not taken.
|
105 | v(robot->numberDof()) {} |
| 54 | |||
| 55 | 1250 | inline value_type step(const size_type& i, const vector_t& x) const { | |
| 56 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1250 times.
|
1250 | assert(i >= 0); |
| 57 | 1250 | value_type r = std::abs(x[i]); | |
| 58 | |||
| 59 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1250 times.
|
1250 | if (r == 0) |
| 60 | ✗ | return epsilon; | |
| 61 | else | ||
| 62 | 1250 | return epsilon * r; | |
| 63 | } | ||
| 64 | |||
| 65 | template <bool forward> | ||
| 66 | 5000 | inline void integrate(const vector_t& x, const vector_t& h, | |
| 67 | const size_type& /*i*/, vector_t& result) const { | ||
| 68 | // Use only the joint corresponding to velocity index i | ||
| 69 | if (forward) | ||
| 70 |
3/6✓ Branch 2 taken 1250 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1250 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1250 times.
✗ Branch 9 not taken.
|
2500 | hpp::pinocchio::integrate<false, DefaultLieGroupMap>(robot, x, h, result); |
| 71 | else | ||
| 72 |
4/8✓ Branch 2 taken 1250 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1250 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1250 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1250 times.
✗ Branch 12 not taken.
|
2500 | hpp::pinocchio::integrate<false, DefaultLieGroupMap>(robot, x, -h, |
| 73 | result); | ||
| 74 | 5000 | } | |
| 75 | |||
| 76 | inline value_type difference(const vector_t& x0, const vector_t& x1, | ||
| 77 | const size_type& i) const { | ||
| 78 | hpp::pinocchio::difference<DefaultLieGroupMap>(robot, x0, x1, v); | ||
| 79 | return v[i]; | ||
| 80 | } | ||
| 81 | |||
| 82 | 2500 | inline void reset(const vector_t& x, const size_type& /*i*/, | |
| 83 | vector_t& result) const { | ||
| 84 | // Use only the joint corresponding to velocity index i | ||
| 85 | 2500 | result = x; | |
| 86 | 2500 | } | |
| 87 | |||
| 88 | const DevicePtr_t& robot; | ||
| 89 | const pinocchio::Model& model; | ||
| 90 | const value_type& epsilon; | ||
| 91 | mutable vector_t v; | ||
| 92 | }; | ||
| 93 | |||
| 94 | struct FiniteDiffVectorSpaceOp { | ||
| 95 | ✗ | FiniteDiffVectorSpaceOp(const value_type& epsilon) : epsilon(epsilon) {} | |
| 96 | |||
| 97 | ✗ | inline value_type step(const size_type i, const vector_t& x) const { | |
| 98 | ✗ | const value_type r = std::abs(x[i]); | |
| 99 | |||
| 100 | ✗ | if (r == 0) | |
| 101 | ✗ | return epsilon; | |
| 102 | else | ||
| 103 | ✗ | return epsilon * r; | |
| 104 | } | ||
| 105 | |||
| 106 | template <bool forward> | ||
| 107 | ✗ | inline void integrate(const vector_t& x, const vector_t& h, | |
| 108 | const size_type& i, vector_t& result) const { | ||
| 109 | ✗ | result[i] = x[i] + (forward ? h[i] : -h[i]); | |
| 110 | } | ||
| 111 | |||
| 112 | inline value_type difference(const vector_t& x0, const vector_t& x1, | ||
| 113 | const size_type& i) const { | ||
| 114 | return x0[i] - x1[i]; | ||
| 115 | } | ||
| 116 | |||
| 117 | ✗ | inline void reset(const vector_t& x, const size_type& i, | |
| 118 | vector_t& result) const { | ||
| 119 | ✗ | result[i] = x[i]; | |
| 120 | } | ||
| 121 | |||
| 122 | const value_type& epsilon; | ||
| 123 | }; | ||
| 124 | |||
| 125 | template <typename FiniteDiffOp, typename Function> | ||
| 126 | 210 | void finiteDiffCentral(matrixOut_t jacobian, vectorIn_t x, | |
| 127 | const FiniteDiffOp& op, const Function& f) { | ||
| 128 | 210 | size_type n = jacobian.cols(); | |
| 129 |
1/2✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
|
210 | vector_t x_pdx = x; |
| 130 |
1/2✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
|
210 | vector_t x_mdx = x; |
| 131 |
2/4✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
|
210 | vector_t h = vector_t::Zero(jacobian.cols()); |
| 132 |
2/4✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 105 times.
✗ Branch 7 not taken.
|
420 | LiegroupElement f_x_mdx(f.outputSpace()), f_x_pdx(f.outputSpace()); |
| 133 | 210 | const ArrayXb& adp = f.activeDerivativeParameters(); | |
| 134 | |||
| 135 |
2/2✓ Branch 0 taken 3360 times.
✓ Branch 1 taken 105 times.
|
6930 | for (size_type j = 0; j < n; ++j) { |
| 136 |
3/4✓ Branch 1 taken 3360 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2110 times.
✓ Branch 4 taken 1250 times.
|
6720 | if (!adp[j]) { |
| 137 |
2/4✓ Branch 1 taken 2110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2110 times.
✗ Branch 5 not taken.
|
4220 | jacobian.col(j).setZero(); |
| 138 | 4220 | continue; | |
| 139 | } | ||
| 140 | |||
| 141 |
3/6✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1250 times.
✗ Branch 8 not taken.
|
2500 | h[j] = op.step(j, x); |
| 142 | |||
| 143 |
2/4✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
|
2500 | op.template integrate<false>(x, h, j, x_mdx); |
| 144 |
3/6✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1250 times.
✗ Branch 8 not taken.
|
2500 | f.value(f_x_mdx, x_mdx); |
| 145 | |||
| 146 |
2/4✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
|
2500 | op.template integrate<true>(x, h, j, x_pdx); |
| 147 |
3/6✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1250 times.
✗ Branch 8 not taken.
|
2500 | f.value(f_x_pdx, x_pdx); |
| 148 | |||
| 149 |
6/12✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1250 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1250 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1250 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1250 times.
✗ Branch 17 not taken.
|
2500 | jacobian.col(j) = ((f_x_pdx - f_x_mdx) / h[j]) / 2; |
| 150 | |||
| 151 |
2/4✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
|
2500 | op.reset(x, j, x_mdx); |
| 152 |
2/4✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1250 times.
✗ Branch 5 not taken.
|
2500 | op.reset(x, j, x_pdx); |
| 153 |
1/2✓ Branch 1 taken 1250 times.
✗ Branch 2 not taken.
|
2500 | h[j] = 0; |
| 154 | } | ||
| 155 |
1/2✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
|
210 | if (jacobian.hasNaN()) { |
| 156 | hppDout(error, "Central finite difference: NaN"); | ||
| 157 | } | ||
| 158 | } | ||
| 159 | |||
| 160 | template <typename FiniteDiffOp, typename Function> | ||
| 161 | ✗ | void finiteDiffForward(matrixOut_t jacobian, vectorIn_t x, | |
| 162 | const FiniteDiffOp& op, const Function& f) { | ||
| 163 | ✗ | size_type n = jacobian.cols(); | |
| 164 | ✗ | vector_t x_dx = x; | |
| 165 | ✗ | vector_t h = vector_t::Zero(jacobian.cols()); | |
| 166 | ✗ | LiegroupElement f_x(f.outputSpace()), f_x_pdx(f.outputSpace()); | |
| 167 | ✗ | const ArrayXb& adp = f.activeDerivativeParameters(); | |
| 168 | |||
| 169 | ✗ | f.value(f_x, x); | |
| 170 | |||
| 171 | ✗ | for (size_type j = 0; j < n; ++j) { | |
| 172 | ✗ | if (!adp[j]) { | |
| 173 | ✗ | jacobian.col(j).setZero(); | |
| 174 | ✗ | continue; | |
| 175 | } | ||
| 176 | |||
| 177 | ✗ | h[j] = op.step(j, x); | |
| 178 | |||
| 179 | ✗ | op.template integrate<true>(x, h, j, x_dx); | |
| 180 | ✗ | f.value(f_x_pdx, x_dx); | |
| 181 | |||
| 182 | ✗ | jacobian.col(j) = (f_x_pdx - f_x) / h[j]; | |
| 183 | |||
| 184 | ✗ | op.reset(x, j, x_dx); | |
| 185 | ✗ | h[j] = 0; | |
| 186 | } | ||
| 187 | ✗ | if (jacobian.hasNaN()) { | |
| 188 | hppDout(warning, | ||
| 189 | "Finite difference of \"" << f.name() << "\" has NaN values."); | ||
| 190 | } | ||
| 191 | } | ||
| 192 | } // namespace | ||
| 193 | |||
| 194 | ✗ | void DifferentiableFunction::finiteDifferenceForward(matrixOut_t jacobian, | |
| 195 | vectorIn_t x, | ||
| 196 | DevicePtr_t robot, | ||
| 197 | value_type eps) const { | ||
| 198 | ✗ | if (robot) | |
| 199 | ✗ | finiteDiffForward(jacobian, x, FiniteDiffRobotOp(robot, eps), *this); | |
| 200 | else | ||
| 201 | ✗ | finiteDiffForward(jacobian, x, FiniteDiffVectorSpaceOp(eps), *this); | |
| 202 | } | ||
| 203 | |||
| 204 | 105 | void DifferentiableFunction::finiteDifferenceCentral(matrixOut_t jacobian, | |
| 205 | vectorIn_t x, | ||
| 206 | DevicePtr_t robot, | ||
| 207 | value_type eps) const { | ||
| 208 |
1/2✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
|
105 | if (robot) |
| 209 |
3/6✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 105 times.
✗ Branch 9 not taken.
|
105 | finiteDiffCentral(jacobian, x, FiniteDiffRobotOp(robot, eps), *this); |
| 210 | else | ||
| 211 | ✗ | finiteDiffCentral(jacobian, x, FiniteDiffVectorSpaceOp(eps), *this); | |
| 212 | 105 | } | |
| 213 | |||
| 214 | 19703 | bool DifferentiableFunction::operator==( | |
| 215 | DifferentiableFunction const& other) const { | ||
| 216 | try { | ||
| 217 |
6/8✓ Branch 1 taken 14658 times.
✓ Branch 2 taken 5045 times.
✓ Branch 3 taken 14630 times.
✓ Branch 4 taken 28 times.
✓ Branch 6 taken 14630 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 14630 times.
✗ Branch 9 not taken.
|
19703 | return isEqual(other) && other.isEqual(*this); |
| 218 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 5045 times.
|
5045 | } catch (const std::bad_cast& exc) { |
| 219 | 5045 | return false; | |
| 220 | 5045 | } | |
| 221 | } | ||
| 222 | 23 | bool DifferentiableFunction::operator!=(DifferentiableFunction const& b) const { | |
| 223 | 23 | return !(*this == b); | |
| 224 | } | ||
| 225 | |||
| 226 | 2084 | DifferentiableFunction::DifferentiableFunction(size_type sizeInput, | |
| 227 | size_type sizeInputDerivative, | ||
| 228 | size_type sizeOutput, | ||
| 229 | 2084 | std::string name) | |
| 230 | 2084 | : inputSize_(sizeInput), | |
| 231 | 2084 | inputDerivativeSize_(sizeInputDerivative), | |
| 232 | 2084 | outputSpace_(LiegroupSpace::Rn(sizeOutput)), | |
| 233 |
2/4✓ Branch 1 taken 2084 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2084 times.
✗ Branch 5 not taken.
|
2084 | activeParameters_(ArrayXb::Constant(sizeInput, true)), |
| 234 |
2/4✓ Branch 1 taken 2084 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2084 times.
✗ Branch 5 not taken.
|
2084 | activeDerivativeParameters_(ArrayXb::Constant(sizeInputDerivative, true)), |
| 235 |
1/2✓ Branch 2 taken 2084 times.
✗ Branch 3 not taken.
|
4168 | name_(name) {} |
| 236 | |||
| 237 | 1239 | DifferentiableFunction::DifferentiableFunction( | |
| 238 | size_type sizeInput, size_type sizeInputDerivative, | ||
| 239 | 1239 | const LiegroupSpacePtr_t& outputSpace, std::string name) | |
| 240 | 1239 | : inputSize_(sizeInput), | |
| 241 | 1239 | inputDerivativeSize_(sizeInputDerivative), | |
| 242 | 1239 | outputSpace_(outputSpace), | |
| 243 |
2/4✓ Branch 1 taken 1239 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1239 times.
✗ Branch 5 not taken.
|
1239 | activeParameters_(ArrayXb::Constant(sizeInput, true)), |
| 244 |
2/4✓ Branch 1 taken 1239 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1239 times.
✗ Branch 5 not taken.
|
1239 | activeDerivativeParameters_(ArrayXb::Constant(sizeInputDerivative, true)), |
| 245 |
1/2✓ Branch 1 taken 1239 times.
✗ Branch 2 not taken.
|
1239 | name_(name), |
| 246 | 2478 | context_() {} | |
| 247 | |||
| 248 | 3 | std::ostream& DifferentiableFunction::print(std::ostream& o) const { | |
| 249 | 3 | return o << "Differentiable function: " << name(); | |
| 250 | } | ||
| 251 | |||
| 252 | template <class Archive> | ||
| 253 | 52 | void DifferentiableFunction::serialize(Archive& ar, | |
| 254 | const unsigned int version) { | ||
| 255 | (void)version; | ||
| 256 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | ar& BOOST_SERIALIZATION_NVP(inputSize_); |
| 257 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | ar& BOOST_SERIALIZATION_NVP(inputDerivativeSize_); |
| 258 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | ar& BOOST_SERIALIZATION_NVP(outputSpace_); |
| 259 |
3/4✓ Branch 0 taken 24 times.
✓ Branch 1 taken 2 times.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
|
52 | if (inputSize_ > 0) ar& BOOST_SERIALIZATION_NVP(activeParameters_); |
| 260 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 2 times.
|
52 | if (inputDerivativeSize_ > 0) |
| 261 |
1/2✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
|
48 | ar& BOOST_SERIALIZATION_NVP(activeDerivativeParameters_); |
| 262 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | ar& BOOST_SERIALIZATION_NVP(name_); |
| 263 |
1/2✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
|
52 | ar& BOOST_SERIALIZATION_NVP(context_); |
| 264 | } | ||
| 265 | |||
| 266 | HPP_SERIALIZATION_IMPLEMENT(DifferentiableFunction); | ||
| 267 | } // namespace constraints | ||
| 268 | } // namespace hpp | ||
| 269 |