GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/steering-method/cross-state-optimization.hh
Date: 2025-05-07 11:07:45
Exec Total Coverage
Lines: 0 16 0.0%
Branches: 0 10 0.0%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
3 // Florent Lamiraux (florent.lamiraux@laas.fr)
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #ifndef HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
31 #define HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
32
33 #include <hpp/core/config-projector.hh>
34 #include <hpp/core/steering-method.hh>
35 #include <hpp/manipulation/config.hh>
36 #include <hpp/manipulation/fwd.hh>
37 #include <hpp/manipulation/problem.hh>
38 #include <hpp/manipulation/steering-method/fwd.hh>
39 #include <hpp/manipulation/steering-method/graph.hh>
40
41 namespace hpp {
42 namespace manipulation {
43 namespace steeringMethod {
44 /// \addtogroup steering_method
45 /// \{
46
47 /// Optimization-based steering method.
48 ///
49 /// #### Sketch of the method
50 ///
51 /// Given two configuration \f$ (q_1,q_2) \f$, this class formulates and
52 /// solves the problem as follows.
53 /// - Compute the corresponding states \f$ (s_1, s_2) \f$.
54 /// - For a each path \f$ (e_0, ... e_n) \f$ of length not more than
55 /// parameter "CrossStateOptimization/maxDepth" between
56 /// \f$ (s_1, s_2)\f$ in the constraint graph, do:
57 /// - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
58 /// - initialize the optimization problem, as explained below,
59 /// - solve the optimization problem, which gives \f$ p^*_i \f$,
60 /// - in case of failure, continue the loop.
61 /// - call the Edge::build of each \f$ e_j \f$ for each consecutive
62 /// \f$ (p^*_i, p^*_{i+1}) \f$.
63 ///
64 /// #### Problem formulation
65 /// Find \f$ (p_i) \f$ such that:
66 /// - \f$ p_0 = q_1 \f$,
67 /// - \f$ p_{n+1} = q_2 \f$,
68 /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref
69 /// StateFunction)
70 /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref
71 /// EdgeFunction).
72 ///
73 /// #### Problem resolution
74 ///
75 /// One solver (hpp::constraints::solver::BySubstitution) is created
76 /// for each waypoint \f$p_i\f$.
77 /// - method buildOptimizationProblem builds a matrix the rows of which
78 /// are the parameterizable numerical constraints present in the
79 /// graph, and the columns of which are the waypoints. Each value in the
80 /// matrix defines the status of each constraint right hand side for
81 /// this waypoint, among {absent from the solver,
82 /// equal to value for previous waypoint,
83 /// equal to value for start configuration,
84 /// equal to value for end configuration}.
85 /// - method CrossStateOptimization::solveOptimizationProblem loops over
86 /// the waypoint solvers, solves for each waypoint after
87 /// initializing the right hand sides with the proper values.
88 /// - eventually method buildPath build paths between waypoints with
89 /// the constraints of the transition in which the path lies.
90 ///
91 /// #### Current status
92 ///
93 /// The method has been successfully tested with romeo holding a placard
94 /// and the construction set benchmarks. The result is satisfactory
95 /// except between pregrasp and grasp waypoints that may be far
96 /// away from each other if the transition between those state does
97 /// not contain the grasp complement constraint. The same holds
98 /// between placement and pre-placement.
99 class HPP_MANIPULATION_DLLAPI CrossStateOptimization : public SteeringMethod {
100 public:
101 struct OptimizationData;
102
103 static CrossStateOptimizationPtr_t create(const ProblemConstPtr_t& problem);
104
105 /// \warning core::Problem will be casted to Problem
106 static CrossStateOptimizationPtr_t create(
107 const core::ProblemConstPtr_t& problem);
108
109 template <typename T>
110 static CrossStateOptimizationPtr_t create(
111 const core::ProblemConstPtr_t& problem);
112
113 core::SteeringMethodPtr_t copy() const;
114
115 protected:
116 ✗ CrossStateOptimization(const ProblemConstPtr_t& problem)
117 ✗ : SteeringMethod(problem), sameRightHandSide_() {
118 ✗ gatherGraphConstraints();
119 }
120
121 ✗ CrossStateOptimization(const CrossStateOptimization& other)
122 ✗ : SteeringMethod(other),
123 ✗ constraints_(other.constraints_),
124 ✗ index_(other.index_),
125 ✗ sameRightHandSide_(other.sameRightHandSide_),
126 ✗ weak_() {}
127
128 core::PathPtr_t impl_compute(ConfigurationIn_t q1,
129 ConfigurationIn_t q2) const;
130
131 ✗ void init(CrossStateOptimizationWkPtr_t weak) {
132 ✗ SteeringMethod::init(weak);
133 ✗ weak_ = weak;
134 }
135
136 private:
137 typedef constraints::solver::BySubstitution Solver_t;
138 struct GraphSearchData;
139
140 /// Gather constraints of all edges
141 void gatherGraphConstraints();
142
143 /// Step 1 of the algorithm
144 /// \return whether the max depth was reached.
145 bool findTransitions(GraphSearchData& data) const;
146
147 /// Step 2 of the algorithm
148 graph::Edges_t getTransitionList(GraphSearchData& data,
149 const std::size_t& i) const;
150
151 /// Step 3 of the algorithm
152 bool buildOptimizationProblem(OptimizationData& d,
153 const graph::Edges_t& transitions) const;
154
155 /// Step 4 of the algorithm
156 bool solveOptimizationProblem(OptimizationData& d) const;
157
158 bool checkConstantRightHandSide(OptimizationData& d, size_type index) const;
159
160 core::PathVectorPtr_t buildPath(OptimizationData& d,
161 const graph::Edges_t& edges) const;
162
163 bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
164
165 /// Vector of parameterizable edge numerical constraints
166 NumericalConstraints_t constraints_;
167 /// Map of indexes in constraints_
168 std::map<std::string, std::size_t> index_;
169
170 /// associative map that stores pairs of constraints of the form
171 /// (constraint, constraint/hold)
172 std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
173
174 /// Weak pointer to itself
175 CrossStateOptimizationWkPtr_t weak_;
176 }; // class CrossStateOptimization
177 /// \}
178
179 template <typename T>
180 ✗ CrossStateOptimizationPtr_t CrossStateOptimization::create(
181 const core::ProblemConstPtr_t& problem) {
182 ✗ CrossStateOptimizationPtr_t gsm = CrossStateOptimization::create(problem);
183 ✗ gsm->innerSteeringMethod(T::create(problem));
184 ✗ return gsm;
185 }
186 } // namespace steeringMethod
187 } // namespace manipulation
188 } // namespace hpp
189
190 #endif // HPP_MANIPULATION_STEERING_METHOD_CROSS_STATE_OPTIMIZATION_HH
191