GCC Code Coverage Report


Directory: ./
File: src/path-optimization/spline-gradient-based/joint-bounds.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 33 33 100.0%
Branches: 28 52 53.8%

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_JOINT_BOUNDS_HH
30 #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_JOINT_BOUNDS_HH
31
32 #include <hpp/core/path/spline.hh>
33 #include <hpp/pinocchio/joint-collection.hh>
34 #include <hpp/pinocchio/liegroup.hh>
35 #include <pinocchio/algorithm/joint-configuration.hpp>
36
37 namespace hpp {
38 namespace core {
39 namespace pathOptimization {
40 using hpp::pinocchio::RnxSOnLieGroupMap;
41 using hpp::pinocchio::liegroup::CartesianProductOperation;
42 using hpp::pinocchio::liegroup::SpecialOrthogonalOperation;
43 using hpp::pinocchio::liegroup::VectorSpaceOperation;
44
45 template <typename LieGroup>
46 struct JointBoundConstraintAlgo {};
47
48 template <int Size, bool rot>
49 struct JointBoundConstraintAlgo<VectorSpaceOperation<Size, rot> > {
50 132 static void run(const size_type& idxV, vectorIn_t low, vectorIn_t up,
51 vectorIn_t neutral, matrix_t& A, vector_t& b,
52 size_type& row) {
53 // size_type row;
54
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 66 times.
324 for (std::size_t i = 0; i < Size; ++i) {
55
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
192 if (!std::isinf(low(i))) {
56 // row = A.rows();
57 // A.conservativeResize(A.rows() + 1, A.cols());
58 // b.conservativeResize(b.rows() + 1);
59
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
192 A.row(row).setZero();
60 192 A(row, idxV + i) = -1;
61 192 b(row) = -(low(i) - neutral(i));
62 192 row++;
63 }
64
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
192 if (!std::isinf(up(i))) {
65 // row = A.rows();
66 // A.conservativeResize(A.rows() + 1, A.cols());
67 // b.conservativeResize(b.rows() + 1);
68
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
192 A.row(row).setZero();
69 192 A(row, idxV + i) = 1;
70 192 b(row) = up(i) - neutral(i);
71 192 row++;
72 }
73 }
74 }
75 };
76
77 template <int N>
78 struct JointBoundConstraintAlgo<SpecialOrthogonalOperation<N> > {
79 60 static void run(const size_type&, vectorIn_t, vectorIn_t, vectorIn_t,
80 60 matrix_t&, vector_t&, size_type&) {}
81 };
82
83 template <typename LieGroup1, typename LieGroup2>
84 struct JointBoundConstraintAlgo<
85 CartesianProductOperation<LieGroup1, LieGroup2> > {
86 60 static void run(const size_type& idxV, vectorIn_t low, vectorIn_t up,
87 vectorIn_t neutral, matrix_t& A, vector_t& b,
88 size_type& row) {
89
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
180 JointBoundConstraintAlgo<LieGroup1>::run(
90
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
180 idxV, low.head<LieGroup1::NQ>(), up.head<LieGroup1::NQ>(),
91 60 neutral.head<LieGroup1::NQ>(), A, b, row);
92
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
180 JointBoundConstraintAlgo<LieGroup2>::run(
93
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
120 idxV + LieGroup1::NV, low.tail<LieGroup2::NQ>(),
94
1/2
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
180 up.tail<LieGroup2::NQ>(), neutral.tail<LieGroup1::NQ>(), A, b, row);
95 }
96 };
97
98 struct JointBoundConstraintStep
99 : public ::pinocchio::fusion::JointUnaryVisitorBase<
100 JointBoundConstraintStep> {
101 typedef boost::fusion::vector<vectorIn_t, vectorIn_t, vectorIn_t, matrix_t&,
102 vector_t&, size_type&>
103 ArgsType;
104
105 template <typename JointModel>
106 132 static void algo(const ::pinocchio::JointModelBase<JointModel>& jmodel,
107 vectorIn_t low, vectorIn_t up, vectorIn_t neutral,
108 matrix_t& A, vector_t& b, size_type& row) {
109 typedef typename RnxSOnLieGroupMap::operation<JointModel>::type LG_t;
110
3/6
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 66 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 66 times.
✗ Branch 8 not taken.
396 JointBoundConstraintAlgo<LG_t>::run(
111
3/6
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 66 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 66 times.
✗ Branch 8 not taken.
264 jmodel.idx_v(), jmodel.jointConfigSelector(low),
112
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
396 jmodel.jointConfigSelector(up), jmodel.jointConfigSelector(neutral), A,
113 b, row);
114 }
115 };
116
117 // The bounds are satisfied iif A * v <= b, where v is the velocity from
118 // the neutral configuration.
119 // We consider only vector-space.
120 // \retval A, b must be of the correct size. If you do not know the number
121 // of rows, set it to 2 * robot->numberDof()
122 // \return the number of rows that were set in A and b. The other rows are
123 // left untouched.
124 66 size_type jointBoundMatrices(const DevicePtr_t& d, ConfigurationIn_t neutral,
125 matrix_t& A, vector_t& b) {
126 66 size_type row = 0;
127 66 const pinocchio::Model& model = d->model();
128
2/2
✓ Branch 1 taken 66 times.
✓ Branch 2 taken 66 times.
132 for (std::size_t i = 1; i < model.joints.size(); ++i) {
129
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
66 JointBoundConstraintStep::run(
130 132 model.joints[i], JointBoundConstraintStep::ArgsType(
131
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
66 model.lowerPositionLimit, model.upperPositionLimit,
132 neutral, A, b, row));
133 }
134 66 return row;
135 // TODO handle extra dof
136 }
137 } // namespace pathOptimization
138 } // namespace core
139 } // namespace hpp
140
141 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_COST_HH
142