GCC Code Coverage Report


Directory: ./
File: src/path-optimization/spline-gradient-based/cost.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 40 49 81.6%
Branches: 34 84 40.5%

Line Branch Exec Source
1 // Copyright (c) 2017, 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 #ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COST_HH
30 #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COST_HH
31
32 #include <hpp/core/path-optimization/cost.hh>
33 #include <hpp/core/path/spline.hh>
34 #include <hpp/util/debug.hh>
35 #include <hpp/util/exception-factory.hh>
36
37 namespace hpp {
38 namespace core {
39 namespace pathOptimization {
40 /// TODO
41 /// The derivative of the cost is wrong when for freeflyer and planar
42 /// joints. It lacks the derivative of the difference operator. The issue
43 /// is that it is not a quadratic cost anymore.
44 template <typename _Spline>
45 struct HPP_CORE_LOCAL L2NormSquaredOfDerivative {
46 typedef _Spline Spline;
47 typedef typename Spline::Ptr_t SplinePtr_t;
48 typedef std::vector<SplinePtr_t> Splines_t;
49
50 30 L2NormSquaredOfDerivative(const Splines_t& splines, size_type paramSize,
51 size_type paramDerivativeSize,
52 size_type derivativeOrder)
53
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
30 : lambda_(splines.size()),
54 30 nSplines_(splines.size()),
55 30 paramSize_(paramSize),
56 30 paramDerivativeSize_(paramDerivativeSize),
57 30 inputSize_(nSplines_ * Spline::NbCoeffs * paramSize),
58 30 inputDerivativeSize_(nSplines_ * Spline::NbCoeffs *
59 30 paramDerivativeSize),
60 30 derivativeOrder_(derivativeOrder) {
61
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
30 assert(derivativeOrder_ > 0);
62 // Spline::NbPowerOfT = 2 * Spline::Order + 3
63
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
30 if (2 * derivativeOrder_ - 1 >= Spline::NbPowerOfT) {
64 HPP_THROW(std::invalid_argument,
65 "Cannot compute the squared norm of the "
66 << derivativeOrder_
67 << "th order derivative with splines of order "
68 << Spline::Order);
69 }
70
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
30 lambda_.setOnes();
71 30 }
72
73 void computeLambdasFromSplineLength(const Splines_t& splines) {
74 for (std::size_t i = 0; i < nSplines_; ++i)
75 lambda_[i] = splines[i]->squaredNormIntegral(derivativeOrder_);
76 value_type lMax = lambda_.maxCoeff();
77 // Make sure there is no too relatively small values in lambda_.
78 lambda_ = (lambda_.array() > 1e-6 * lMax)
79 .select(lambda_.cwiseInverse(), 1e6 / lMax);
80 }
81
82 132 void value(value_type& result, const Splines_t& splines) const {
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 66 times.
132 assert(nSplines_ == splines.size());
84 132 result = 0;
85
2/2
✓ Branch 0 taken 228 times.
✓ Branch 1 taken 66 times.
588 for (std::size_t i = 0; i < nSplines_; ++i)
86 456 result += lambda_[i] * splines[i]->squaredNormIntegral(derivativeOrder_);
87 132 }
88
89 void jacobian(vectorOut_t J, const Splines_t& splines) const {
90 assert(nSplines_ == splines.size());
91 assert(J.size() == inputDerivativeSize_);
92 size_type col = 0;
93 size_type size = Spline::NbCoeffs * paramDerivativeSize_;
94 for (std::size_t i = 0; i < nSplines_; ++i) {
95 splines[i]->squaredNormIntegralDerivative(derivativeOrder_,
96 J.segment(col, size));
97 J.segment(col, size) *= lambda_[i];
98 col += size;
99 }
100 }
101
102 30 void hessian(matrixOut_t H, const Splines_t& splines) const {
103
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
30 assert(H.rows() == inputDerivativeSize_);
104
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
30 assert(H.cols() == inputDerivativeSize_);
105
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
30 typename Spline::BasisFunctionIntegralMatrix_t Ic;
106
107
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
30 H.setZero();
108
109
2/2
✓ Branch 0 taken 51 times.
✓ Branch 1 taken 15 times.
132 for (std::size_t k = 0; k < nSplines_; ++k) {
110
1/2
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
102 splines[k]->squaredNormBasisFunctionIntegral(derivativeOrder_, Ic);
111
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
102 Ic *= 2;
112 102 const size_type shift = k * Spline::NbCoeffs * paramSize_;
113
2/2
✓ Branch 0 taken 258 times.
✓ Branch 1 taken 51 times.
618 for (size_type i = 0; i < Spline::NbCoeffs; ++i) {
114
2/2
✓ Branch 0 taken 1492 times.
✓ Branch 1 taken 258 times.
3500 for (size_type j = 0; j < Spline::NbCoeffs; ++j) {
115 H.block(shift + i * paramSize_, shift + j * paramSize_, paramSize_,
116
1/2
✓ Branch 1 taken 1492 times.
✗ Branch 2 not taken.
2984 paramSize_)
117 .diagonal()
118
4/8
✓ Branch 1 taken 1492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1492 times.
✗ Branch 11 not taken.
2984 .setConstant(Ic(i, j) * lambda_[k]);
119 }
120 }
121
122 #ifndef NDEBUG
123
7/14
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 51 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 51 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 51 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 51 times.
✗ Branch 23 not taken.
204 value_type res1 = 0.5 * splines[k]->rowParameters().transpose() *
124 H.block(shift, shift, Spline::NbCoeffs * paramSize_,
125
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
102 Spline::NbCoeffs * paramSize_) *
126 102 splines[k]->rowParameters();
127
128 102 value_type res2 =
129
2/4
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
102 splines[k]->squaredNormIntegral(derivativeOrder_) * lambda_[k];
130
131 102 value_type diff = res1 - res2;
132
133 102 if (std::fabs(diff) > Eigen::NumTraits<value_type>::dummy_precision()) {
134 hppDout(error,
135 "Hessian seems wrong for spline "
136 << k << ": " << res1 << " - " << res2 << " = "
137 << res1 - res2 << '\n'
138 << H.block(shift, shift, Spline::NbCoeffs * paramSize_,
139 Spline::NbCoeffs * paramSize_));
140 }
141 #endif // NDEBUG
142 }
143 30 }
144
145 vector_t lambda_;
146 const std::size_t nSplines_;
147 const size_type paramSize_, paramDerivativeSize_;
148 const size_type inputSize_, inputDerivativeSize_;
149 size_type derivativeOrder_;
150 };
151 } // namespace pathOptimization
152 } // namespace core
153 } // namespace hpp
154
155 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COST_HH
156