GCC Code Coverage Report

Directory: ./
File: src/steering-method/spline.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 40 43 93.0%
Branches: 72 140 51.4%
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 #include <hpp/constraints/svd.hh>
30 #include <hpp/core/distance.hh>
31 #include <hpp/core/path/spline.hh>
32 #include <hpp/core/problem.hh>
33 #include <hpp/core/steering-method/spline.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/liegroup.hh>
37
38 namespace hpp {
39 namespace core {
40 namespace steeringMethod {
41 template <int _PB, int _SO>
42 2 PathPtr_t Spline<_PB, _SO>::impl_compute(ConfigurationIn_t q1,
43 ConfigurationIn_t q2) const {
44 enum { NDerivativeConstraintPerSide = int((SplineOrder + 1 - 2) / 2) };
45 typedef Eigen::Matrix<value_type, Eigen::Dynamic,
46 NDerivativeConstraintPerSide>
47 DerMatrix_t;
48 typedef typename DerMatrix_t::ConstantReturnType DefaultDerivatives_t;
49
50
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 DefaultDerivatives_t defaultDer(DerMatrix_t::Zero(
51 device_.lock()->numberDof(), NDerivativeConstraintPerSide));
52
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<int> orders(NDerivativeConstraintPerSide);
53
0/2
✗ Branch 0 not taken.
✗ Branch 1 not taken.
 2 for (std::size_t i = 0; i < NDerivativeConstraintPerSide; ++i)
54 ✗ orders[i] = int(i + 1);
55
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 return impl_compute(q1, orders, defaultDer, q2, orders, defaultDer, -1);
56 }
57
58 template <int _PB, int _SO>
59 130 PathPtr_t Spline<_PB, _SO>::steer(ConfigurationIn_t q1, std::vector<int> order1,
60 matrixIn_t derivatives1, ConfigurationIn_t q2,
61 std::vector<int> order2,
62 matrixIn_t derivatives2,
63 value_type length) const {
64 // Check the size of the derivatives.
65
2/4
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 65 times.
 130 assert(q1.size() == device_.lock()->configSize());
66
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 65 times.
 130 assert(q1.size() == q2.size());
67
2/4
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 65 times.
 130 assert(derivatives1.rows() == device_.lock()->numberDof());
68
2/4
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 65 times.
 130 assert(derivatives2.rows() == device_.lock()->numberDof());
69 return impl_compute(q1, order1, derivatives1, q2, order2, derivatives2,
70
4/8
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 62 times.
✗ Branch 12 not taken.
 130 length);
71 }
72
73 template <int _PB, int _SO>
74 template <typename Derived>
75 132 PathPtr_t Spline<_PB, _SO>::impl_compute(
76 ConfigurationIn_t q1, std::vector<int> order1,
77 const Eigen::MatrixBase<Derived>& derivatives1, ConfigurationIn_t q2,
78 std::vector<int> order2, const Eigen::MatrixBase<Derived>& derivatives2,
79 value_type length) const {
80 // Compute the decomposition
81 // typedef Eigen::Matrix<value_type, SplineOrder+1, SplineOrder+1>
82 // ConstraintMatrix_t;
83 typedef Eigen::Matrix<value_type, Eigen::Dynamic, SplineOrder + 1,
84 Eigen::RowMajor>
85 ConstraintMatrix_t;
86 typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
87 Eigen::RowMajor>
88 RhsMatrix_t;
89
90
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 54 times.
 132 if (length <= 0) {
91 24 DistancePtr_t d = problem()->distance();
92
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 24 length = (*d)(q1, q2);
93 }
94
1/2
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
 262 SplinePathPtr_t p =
95 128 SplinePath::create(device_.lock(), interval_t(0, length), constraints());
96
97 128 const size_type nbConstraints = 2 + derivatives1.cols() + derivatives2.cols();
98
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
 126 ConstraintMatrix_t coeffs(nbConstraints, SplineOrder + 1);
99
2/4
✓ Branch 3 taken 66 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
 132 RhsMatrix_t rhs(nbConstraints, device_.lock()->numberDof());
100
101
2/4
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
 132 p->base(q1); // TODO use the center ?
102 // p->base(device_.lock()->neutralConfiguration()); // TODO use the center ?
103 // Configuration_t qmiddle (q1.size());
104 // pinocchio::interpolate<pinocchio::RnxSOnLieGroupMap>(device_.lock(), q1,
105 // q2, 0.5, qmiddle); p->base(qmiddle);
106
107 // Compute the matrices
108 // TODO calls to basisFunctionDerivative could be cached as they do not
109 // depend on the inputs.
110
4/8
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 63 times.
✗ Branch 12 not taken.
 132 p->basisFunctionDerivative(0, 0, coeffs.row(0).transpose());
111
3/6
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 66 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 60 times.
✗ Branch 9 not taken.
 254 pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(device_.lock(), q1,
112
3/6
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
 254 p->base(), rhs.row(0));
113
2/2
✓ Branch 1 taken 93 times.
✓ Branch 2 taken 66 times.
 312 for (std::size_t i = 0; i < order1.size(); ++i)
114
4/8
✓ Branch 2 taken 90 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 91 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 91 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 92 times.
✗ Branch 13 not taken.
 186 p->basisFunctionDerivative(order1[i], 0, coeffs.row(i + 1).transpose());
115
3/6
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63 times.
✗ Branch 9 not taken.
 132 rhs.middleRows(1, order1.size()).transpose() = derivatives1;
116
117 126 size_type row = 1 + order1.size();
118
4/8
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
 126 p->basisFunctionDerivative(0, 1, coeffs.row(row).transpose());
119
3/6
✓ Branch 1 taken 65 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
 258 pinocchio::difference<pinocchio::RnxSOnLieGroupMap>(device_.lock(), q2,
120
3/6
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
 260 p->base(), rhs.row(row));
121 132 ++row;
122
2/2
✓ Branch 1 taken 93 times.
✓ Branch 2 taken 66 times.
 318 for (std::size_t i = 0; i < order2.size(); ++i)
123
4/8
✓ Branch 2 taken 93 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 93 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 93 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 93 times.
✗ Branch 13 not taken.
 186 p->basisFunctionDerivative(order2[i], 1, coeffs.row(i + row).transpose());
124
3/6
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
 132 rhs.middleRows(row, order2.size()).transpose() = derivatives2;
125
126 // Solve the problem
127 // coeffs * P = rhs
128 typedef Eigen::JacobiSVD<ConstraintMatrix_t> SVD_t;
129
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
 130 SVD_t svd(coeffs, Eigen::ComputeFullU | Eigen::ComputeFullV);
130
3/6
✓ Branch 2 taken 63 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 62 times.
✗ Branch 9 not taken.
 132 p->parameters(svd.solve(rhs));
131
132 262 return p;
133 }
134
135 template <int _PB, int _SO>
136 42 Spline<_PB, _SO>::Spline(const ProblemConstPtr_t& problem)
137 42 : SteeringMethod(problem), device_(problem->robot()) {}
138
139 /// Copy constructor
140 template <int _PB, int _SO>
141 ✗ Spline<_PB, _SO>::Spline(const Spline& other)
142 ✗ : SteeringMethod(other), device_(other.device_) {}
143
144 // template class Spline<path::CanonicalPolynomeBasis, 1>; // equivalent to
145 // StraightPath template class Spline<path::CanonicalPolynomeBasis, 2>; template
146 // class Spline<path::CanonicalPolynomeBasis, 3>;
147 template class Spline<path::BernsteinBasis, 1>; // equivalent to StraightPath
148 // template class Spline<path::BernsteinBasis, 2>;
149 template class Spline<path::BernsteinBasis, 3>;
150 template class Spline<path::BernsteinBasis, 5>;
151 template class Spline<path::BernsteinBasis, 7>;
152 } // namespace steeringMethod
153 } // namespace core
154 } // namespace hpp
155