GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/graph/helper.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 0 8 0.0%
Branches: 0 14 0.0%
Line Branch Exec Source
1 // Copyright (c) 2016, LAAS-CNRS
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_MANIPULATION_GRAPH_HELPER_HH
30 #define HPP_MANIPULATION_GRAPH_HELPER_HH
31
32 #include <algorithm>
33 #include <string>
34 #include <tuple>
35
36 #include "hpp/manipulation/config.hh"
37 #include "hpp/manipulation/fwd.hh"
38 #include "hpp/manipulation/graph/fwd.hh"
39
40 namespace hpp {
41 namespace manipulation {
42 typedef constraints::ImplicitPtr_t ImplicitPtr_t;
43 namespace graph {
44 namespace helper {
45 /// \defgroup helper Helpers to build the graph of constraints
46 /// \addtogroup helper
47 /// \{
48
49 ✗ NumericalConstraints_t merge_nc(const NumericalConstraints_t& a,
50 const NumericalConstraints_t& b) {
51 ✗ NumericalConstraints_t nc;
52 ✗ nc.reserve(a.size() + b.size());
53 ✗ std::copy(a.begin(), a.end(), nc.begin());
54 ✗ std::copy(b.begin(), b.end(), nc.begin());
55 ✗ return nc;
56 }
57
58 struct FoliatedManifold {
59 // Manifold definition
60 NumericalConstraints_t nc;
61 LockedJoints_t lj;
62 NumericalConstraints_t nc_path;
63 // Foliation definition
64 NumericalConstraints_t nc_fol;
65 LockedJoints_t lj_fol;
66
67 FoliatedManifold merge(const FoliatedManifold& other) {
68 FoliatedManifold both;
69 both.nc = merge_nc(nc, other.nc);
70 both.nc_path = merge_nc(nc_path, other.nc_path);
71
72 std::copy(lj.begin(), lj.end(), both.lj.end());
73 std::copy(other.lj.begin(), other.lj.end(), both.lj.end());
74 return both;
75 }
76
77 void addToState(StatePtr_t comp) const;
78 void addToEdge(EdgePtr_t comp) const;
79 void specifyFoliation(LevelSetEdgePtr_t lse) const;
80
81 ✗ bool foliated() const { return !lj_fol.empty() || !nc_fol.empty(); }
82 ✗ bool empty() const { return lj.empty() && nc.empty(); }
83 };
84
85 enum GraspingCase {
86 NoGrasp = 1 << 0,
87 GraspOnly = 1 << 1,
88 WithPreGrasp = 1 << 2
89 };
90 enum PlacementCase {
91 NoPlace = 1 << 3,
92 PlaceOnly = 1 << 4,
93 WithPrePlace = 1 << 5
94 };
95
96 struct Rule {
97 std::vector<std::string> grippers_;
98 std::vector<std::string> handles_;
99 bool link_;
100 Rule() : grippers_(), handles_(), link_(false) {}
101 };
102
103 typedef std::vector<Rule> Rules_t;
104
105 /// Create edges according to the case.
106 /// gCase is a logical OR combination of GraspingCase and PlacementCase
107 ///
108 /// When an argument is not relevant, use the default constructor
109 /// of FoliatedManifold
110 template <int gCase>
111 Edges_t createEdges(
112 const std::string& forwName, const std::string& backName,
113 const StatePtr_t& from, const StatePtr_t& to, const size_type& wForw,
114 const size_type& wBack, const FoliatedManifold& grasp,
115 const FoliatedManifold& pregrasp, const FoliatedManifold& place,
116 const FoliatedManifold& preplace, const bool levelSetGrasp,
117 const bool levelSetPlace,
118 const FoliatedManifold& submanifoldDef = FoliatedManifold());
119
120 EdgePtr_t createLoopEdge(
121 const std::string& loopName, const StatePtr_t& state, const size_type& w,
122 const bool levelSet,
123 const FoliatedManifold& submanifoldDef = FoliatedManifold());
124
125 /// Create a waypoint edge taking into account:
126 /// \li grasp
127 /// \li placement
128 /// \li preplacement
129
130 /// Create a waypoint edge taking into account
131 /// \li grasp
132 /// \li pregrasp
133 /// \li placement
134
135 void graspManifold(const GripperPtr_t& gripper, const HandlePtr_t& handle,
136 FoliatedManifold& grasp, FoliatedManifold& pregrasp);
137
138 /// The placement foliation constraint is built using
139 /// hpp::constraints::ConvexShapeMatcherComplement
140 void strictPlacementManifold(const ImplicitPtr_t placement,
141 const ImplicitPtr_t preplacement,
142 const ImplicitPtr_t placementComplement,
143 FoliatedManifold& place,
144 FoliatedManifold& preplace);
145
146 /// The placement foliation constraint is built locked joints
147 /// It is faster than strictPlacementManifold but the foliation
148 /// parametrisation is redundant.
149 void relaxedPlacementManifold(const ImplicitPtr_t placement,
150 const ImplicitPtr_t preplacement,
151 const LockedJoints_t objectLocks,
152 FoliatedManifold& place,
153 FoliatedManifold& preplace);
154
155 typedef std::tuple<ImplicitPtr_t, ImplicitPtr_t, LockedJoints_t>
156 PlacementConstraint_t;
157 typedef std::vector<HandlePtr_t> Handles_t;
158 typedef std::vector<GripperPtr_t> Grippers_t;
159 /// Tuple representing an object as follows:
160 /// \li PlacementConstraint_t constraint to place the object
161 /// \li Handles_t list of handles of the object
162 /// \li std::size_t the index of this tuple in Objects_t.
163 /// \note the index must be unique, as object equallity is checked using this
164 /// index.
165 typedef std::tuple<PlacementConstraint_t, Handles_t, std::size_t> Object_t;
166 typedef std::vector<Object_t> Objects_t;
167
168 /// Fill a Graph
169 ///
170 /// \note It is assumed that a gripper can grasp only one handle and each
171 /// handle cannot be grasped by several grippers at the same time.
172 ///
173 /// \param[in,out] graph must be an initialized empty Graph.
174 void graphBuilder(const ProblemSolverPtr_t& ps, const Objects_t& objects,
175 const Grippers_t& grippers, GraphPtr_t graph,
176 const Rules_t& rules = Rules_t());
177
178 struct ObjectDef_t {
179 std::string name;
180 Strings_t handles, shapes;
181 };
182
183 GraphPtr_t graphBuilder(const ProblemSolverPtr_t& ps,
184 const std::string& graphName, const Strings_t& griNames,
185 const std::vector<ObjectDef_t>& objs,
186 const Strings_t& envNames, const Rules_t& rules,
187 const value_type& prePlaceWidth = 0.05);
188 /// \}
189 } // namespace helper
190 } // namespace graph
191 } // namespace manipulation
192 } // namespace hpp
193
194 #endif // HPP_MANIPULATION_GRAPH_HELPER_HH
195