GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/graph/edge.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 1 12 8.3%
Branches: 0 2 0.0%
Line Branch Exec Source
1 // Copyright (c) 2014, 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_EDGE_HH
30 #define HPP_MANIPULATION_GRAPH_EDGE_HH
31
32 #include <hpp/core/constraint-set.hh>
33 #include <hpp/core/path.hh>
34 #include <hpp/core/relative-motion.hh>
35 #include <hpp/core/steering-method.hh>
36
37 #include "hpp/manipulation/config.hh"
38 #include "hpp/manipulation/fwd.hh"
39 #include "hpp/manipulation/graph/graph.hh"
40
41 namespace hpp {
42 namespace manipulation {
43 namespace graph {
44 /// \addtogroup constraint_graph
45 /// \{
46
47 /// Transition between two states of a constraint graph
48 ///
49 /// An edge stores two types of constraints.
50
51 /// \li <b> Path constraints </b> should be safisfied by paths belonging
52 /// to the edge. Along any path, the right hand side of the
53 /// constraint is constant, but can differ between paths. For
54 /// instance if an edge represents a transit path of a robot
55 /// that can grasp an object, the right hand side of the
56 /// constraint represents the position of the object. Along any
57 /// transit path, the object does not move, but for different paths
58 /// the object can be at different positions.
59 /// \sa method pathConstraint.
60 /// \li <b> Configuration constraints </b> are constraints that
61 /// configurations in the destination state should satisfy and
62 /// the constraints that paths should satisfy. For instance, if
63 /// the edge links a state where the robot does not hold the
64 /// object to a state where the robot holds the object, the
65 /// configuration constraints represent a fixed relative
66 /// position of the object with respect to the gripper and a
67 /// stable position of the object. Configuration constraints are
68 /// necessary to generate a configuration in the destination
69 /// state of the edge that is reachable from a given
70 /// configuration in the start state by an admissible path.
71 class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
72 friend class WaypointEdge;
73
74 public:
75 typedef core::RelativeMotion RelativeMotion;
76
77 /// Destructor
78 virtual ~Edge();
79
80 /// Create a new empty Edge.
81 static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
82 const StateWkPtr_t& from, const StateWkPtr_t& to);
83
84 /// Generate a reachable configuration in the target state
85 ///
86 /// \param nStart node containing the configuration defining the right
87 /// hand side of the edge path constraint,
88 /// \param[in,out] q input configuration used to initialize the
89 /// numerical solver and output configuration lying
90 /// in the target state and reachable along the edge
91 /// from nStart.
92 virtual bool generateTargetConfig(core::NodePtr_t nStart,
93 ConfigurationOut_t q) const;
94
95 /// Generate a reachable configuration in the target state
96 ///
97 /// \param qStart node containing the configuration defining the right
98 /// hand side of the edge path constraint,
99 /// \param[in,out] q input configuration used to initialize the
100 /// numerical solver and output configuration lying
101 /// in the target state and reachable along the edge
102 /// from nnear.
103 virtual bool generateTargetConfig(ConfigurationIn_t qStart,
104 ConfigurationOut_t q) const;
105
106 virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
107
108 virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
109 ConfigurationIn_t q2) const;
110
111 /// Get the destination
112 StatePtr_t stateTo() const;
113
114 /// Get the origin
115 StatePtr_t stateFrom() const;
116
117 /// Get the state in which path is.
118 16 StatePtr_t state() const { return state_.lock(); }
119
120 ✗ void state(StatePtr_t state) { state_ = state; }
121
122 /// Get steering method associated to the edge.
123 ✗ const core::SteeringMethodPtr_t& steeringMethod() const {
124 ✗ return steeringMethod_;
125 }
126
127 /// Get path validation associated to the edge.
128 ✗ const core::PathValidationPtr_t& pathValidation() const {
129 ✗ return pathValidation_;
130 }
131
132 ✗ const RelativeMotion::matrix_type& relativeMotion() const {
133 ✗ return relMotion_;
134 }
135
136 /// Update the relative motion matrix
137 void relativeMotion(const RelativeMotion::matrix_type& m);
138
139 /// Set Security margin for a pair of joints
140 ///
141 /// \param row index of joint1 + 1 in robot,
142 /// \param col index of joint2 + 1 in robot,
143 /// \param margin security margin for collision checking between
144 /// those joints.
145 ///
146 /// \note set value to matrix [row, col] and matrix [col, row].
147 void securityMarginForPair(const size_type& row, const size_type& col,
148 const value_type& margin);
149
150 /// Accessor to the security margin.
151 const matrix_t& securityMargins() const { return securityMargins_; }
152
153 /// Get direction of the path compare to the edge
154 /// \return true is reverse
155 virtual bool direction(const core::PathPtr_t& path) const;
156
157 /// Populate a ConfigProjector with constraints required to generate
158 /// a path at the intersection of two edges.
159 virtual bool intersectionConstraint(const EdgePtr_t& other,
160 ConfigProjectorPtr_t projector) const;
161
162 /// Print the object in a stream.
163 virtual std::ostream& dotPrint(
164 std::ostream& os,
165 dot::DrawingAttributes da = dot::DrawingAttributes()) const;
166
167 /// Constraint of the destination state and of the path
168 ConstraintSetPtr_t targetConstraint() const;
169
170 ✗ void setShort(bool isShort) { isShort_ = isShort; }
171
172 ✗ bool isShort() const { return isShort_; }
173 /// Constraint to project a path.
174 /// \return The initialized constraint.
175 ConstraintSetPtr_t pathConstraint() const;
176
177 protected:
178 /// Initialization of the object.
179 void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
180 const StateWkPtr_t& from, const StateWkPtr_t& to);
181
182 /// Constructor
183 Edge(const std::string& name);
184
185 /// Build path and target state constraint set.
186 virtual ConstraintSetPtr_t buildTargetConstraint();
187
188 /// Build path constraints
189 virtual ConstraintSetPtr_t buildPathConstraint();
190
191 virtual void initialize();
192
193 /// Print the object in a stream.
194 virtual std::ostream& print(std::ostream& os) const;
195
196 bool isShort_;
197
198 private:
199 /// See pathConstraint member function.
200 ConstraintSetPtr_t pathConstraints_;
201
202 /// Constraint ensuring that a q_proj will be in to_ and in the
203 /// same leaf of to_ as the configuration used for initialization.
204 ConstraintSetPtr_t targetConstraints_;
205
206 /// The two ends of the transition.
207 StateWkPtr_t from_, to_;
208
209 /// True if this path is in state from, False if in state to
210 StateWkPtr_t state_;
211
212 /// Steering method used to create paths associated to the edge
213 core::SteeringMethodPtr_t steeringMethod_;
214
215 /// Path validation associated to the edge
216 mutable RelativeMotion::matrix_type relMotion_;
217 /// matrix of security margins for collision checking between joints
218 matrix_t securityMargins_;
219
220 core::PathValidationPtr_t pathValidation_;
221
222 /// Weak pointer to itself.
223 EdgeWkPtr_t wkPtr_;
224
225 friend class Graph;
226 }; // class Edge
227
228 /// Edge with intermediate waypoint states.
229 ///
230 /// This class implements a transition from one state to another state
231 /// with intermediate states in-between. This feature is particularly
232 /// interesting when manipulating objects. Between a state where a gripper
233 /// does not grasp an object and a state where the same gripper grasps
234 /// the object, it is useful to add an intermediate state where the
235 /// gripper is close to the object.
236 ///
237 /// Waypoints are handled recursively, i.e.\ class WaypointEdge
238 /// contains only a State and an Edge, the second Edge being
239 /// itself. In this package, the State in a WaypointEdge is
240 /// semantically different from other State because it does not
241 /// correspond to a state with different manipulation rules. It
242 /// has the same rules as another State (either Edge::stateFrom() or
243 /// Edge::stateTo()).
244 ///
245 /// Semantically, a waypoint State is fully part of the WaypointEdge.
246 /// When a corresponding path reaches it, no planning is required to know
247 /// what to do next. To the contrary, when a path reaches
248 /// Edge::stateFrom() or Edge::stateTo(), there may be several
249 /// possibilities.
250 ///
251 /// \note
252 /// Implementation details: let's say, between the two states \f$N_f\f$
253 /// and \f$N_t\f$, two waypoints are required:
254 /// \f$ N_f \xrightarrow{e_0} n_0 \xrightarrow{e_1} n_1
255 /// \xrightarrow{e_2} N_t\f$.
256 /// The WaypointEdge contains:
257 /// \li from: \f$N_f\f$,
258 /// \li to: \f$N_t\f$,
259 /// \li states: \f$(n_0, n_1)\f$
260 /// \li transitions: \f$(e_0, e_1, e_2)\f$
261 /// \li constraints: any calls to the constraints throw,
262 class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge {
263 public:
264 /// Create a new WaypointEdge.
265 static WaypointEdgePtr_t create(const std::string& name,
266 const GraphWkPtr_t& graph,
267 const StateWkPtr_t& from,
268 const StateWkPtr_t& to);
269
270 virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
271
272 virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
273 ConfigurationIn_t q2) const;
274
275 /// Generate a reachable configuration in the target state
276 ///
277 /// \param qStart node containing the configuration defining the right
278 /// hand side of the edge path constraint,
279 /// \param[in,out] q input configuration used to initialize the
280 /// numerical solver and output configuration lying
281 /// in the target state and reachable along the edge
282 /// from nnear.
283 virtual bool generateTargetConfig(ConfigurationIn_t qStart,
284 ConfigurationOut_t q) const;
285
286 /// Return the index-th edge.
287 const EdgePtr_t& waypoint(const std::size_t index) const;
288
289 /// Print the object in a stream.
290 virtual std::ostream& dotPrint(
291 std::ostream& os,
292 dot::DrawingAttributes da = dot::DrawingAttributes()) const;
293
294 /// Set the number of waypoints
295 void nbWaypoints(const size_type number);
296
297 ✗ std::size_t nbWaypoints() const { return edges_.size() - 1; }
298
299 /// Set waypoint index with wEdge and wTo.
300 /// \param wTo is the destination state of wEdge
301 void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
302 const StatePtr_t wTo);
303
304 protected:
305 ✗ WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
306 /// Initialization of the object.
307 void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
308 const StateWkPtr_t& from, const StateWkPtr_t& to);
309
310 /// Initialize each of the internal edges
311 virtual void initialize();
312 /// Print the object in a stream.
313 virtual std::ostream& print(std::ostream& os) const;
314
315 private:
316 Edges_t edges_;
317 States_t states_;
318
319 mutable matrix_t configs_;
320 mutable bool lastSucceeded_;
321
322 WaypointEdgeWkPtr_t wkPtr_;
323 }; // class WaypointEdge
324
325 /// Edge that handles crossed foliations
326 ///
327 /// Let us consider the following simple constraint graph
328 /// corresponding to a robot grasping an object with one gripper.
329 ///
330 /// \image html constraint-graph.png "Simple constraint graph corresponding to a
331 /// robot grasping an object."
332 ///
333 /// In order to disambiguate, we assume here that
334 /// \li transition <b>Grasp object</b> is in \b Placement state,
335 /// \li transition <b>Release object</b> is in \b Grasp state.
336 ///
337 /// If state \b Placement is defined by the object lying on a planar
338 /// polygonal surface, then
339 /// \li state \b Placement,
340 /// \li transition \b Transit, and
341 /// \li transition <b>Grasp object</b>
342 ///
343 /// are all constrained in a foliated manifold parameterized by the
344 /// position of the box on the surface.
345 ///
346 /// Likewise, if the object is cylindrical the grasp may have a degree
347 /// of freedom corresponding to the angle around z-axis of the gripper
348 /// with respect to the object. See classes
349 /// \link hpp::manipulation::Handle Handle\endlink and
350 /// \link hpp::pinocchio::Gripper Gripper\endlink for details.
351 /// In this latter case,
352 /// \li state \b Grasp,
353 /// \li transition \b Transfer, and
354 /// \li transition <b>Release object</b>
355 ///
356 /// are all constrained in a foliated manifold parameterized by the
357 /// angle around z-axis of the gripper with respect to the object.
358 ///
359 /// Let us denote
360 /// \li \c grasp the numerical constraint defining state \b Grasp,
361 /// \li \c placement the numerical constraint defining state \b Placement,
362 /// \li \c grasp_comp the parameterized constraint defining a leaf
363 /// of \c Transfer (the angle between the gripper and the
364 /// object),
365 /// \li \c placement_comp the parameterized constraint defining a leaf
366 /// of \b Placement (the position of the object on the contact
367 /// surface).
368 ///
369 /// As explained in <a
370 /// href="https://hal.archives-ouvertes.fr/hal-01358767">this
371 /// paper </a>, we are in the crossed foliation case and manipulation RRT
372 /// will never be able to connect trees expanding in different leaves of
373 /// the foliation.
374 ///
375 /// This class solves this issue in the following way by creating an
376 /// instance of LevelSetEdge between \b Placement and \b Grasp.
377 ///
378 /// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
379 /// \b Placement, this transition will produce a target configuration
380 /// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
381 /// \endlink as follows.
382 ///
383 /// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
384 /// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
385 /// \li compute right hand side of \c grasp_comp with
386 /// \f$\mathbf{q}_{rand}\f$,
387 /// \li compute right hand side of \c placement_comp with
388 /// \f$\mathbf{q}_{start}\f$,
389 /// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
390 /// using input configuration \f$\mathbf{q}\f$. Note that the
391 /// parent method Edge::generateTargetConfig does the same without
392 /// adding \c grasp_comp.
393 ///
394 /// The constraints parameterizing the target state foliation
395 /// (\c graps_comp in our example) are passed to class instances
396 /// using method \link LevelSetEdge::insertParamConstraint
397 /// insertParamConstraint\endlink.
398 class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
399 public:
400 virtual ~LevelSetEdge();
401
402 /// Create a new LevelSetEdge.
403 static LevelSetEdgePtr_t create(const std::string& name,
404 const GraphWkPtr_t& graph,
405 const StateWkPtr_t& from,
406 const StateWkPtr_t& to);
407
408 /// Generate a reachable configuration in the target state
409 ///
410 /// \param nStart node containing the configuration defining the right
411 /// hand side of the edge path constraint,
412 /// \param[in,out] q input configuration used to initialize the
413 /// numerical solver and output configuration lying
414 /// in the target state and reachable along the edge
415 /// from nStart.
416 virtual bool generateTargetConfig(core::NodePtr_t nStart,
417 ConfigurationOut_t q) const;
418
419 /// Generate a reachable configuration in the target state
420 ///
421 /// \param qStart configuration defining the right hand side of the
422 /// edge path constraint,
423 /// \param[in,out] q input configuration used to initialize the
424 /// numerical solver and output configuration lying
425 /// in the target state and reachable along the edge
426 /// from nnear.
427 virtual bool generateTargetConfig(ConfigurationIn_t qStart,
428 ConfigurationOut_t q) const;
429
430 /// Generate a reachable configuration in leaf of target state
431 /// \param qStart configuration defining the right hand side of the
432 /// edge path constraint,
433 /// \param qLeaf configuration used to set the right hand side of
434 /// the target state foliation. See method
435 /// \link LevelSetEdge::insertParamConstraint
436 /// insertParamConstraint\endlink.
437 bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
438 ConfigurationIn_t qLeaf,
439 ConfigurationOut_t q) const;
440
441 /// Build path and target state constraints
442 virtual ConstraintSetPtr_t buildTargetConstraint();
443
444 /// Build the histogram
445 /// \sa LevelSetEdge::histogram.
446 void buildHistogram();
447
448 /// Return pointer on histogram of the edge
449 ///
450 /// The edge histogram is a container of configurations defined by
451 /// a set of constraints called the <b>condition constraints</b>
452 /// that a configuration should satisfy to be inserted in the
453 /// histogram.
454 ///
455 /// The histogram is passed to the Roadmap via the graph (method
456 /// Graph::insertHistogram). The roadmap then populates the histogram
457 /// with all new configurations satisfying the condition constraints.
458 ///
459 /// The condition constraints should therefore be the constraints of
460 /// the target state of the level set edge.
461 ///
462 /// \sa LevelSetEdge::conditionConstraints
463 /// LevelSetEdge::insertConditionConstraint
464 LeafHistogramPtr_t histogram() const;
465
466 /// \name Foliation definition
467 /// \{
468
469 /// Insert a constraints parameterizing the target state foliation
470 /// \param nm the numerical constraint,
471 void insertParamConstraint(const ImplicitPtr_t& nm);
472
473 /// Get constraints parameterizing the target state foliation
474 const NumericalConstraints_t& paramConstraints() const;
475
476 /// Insert a condition constraint
477 /// \sa LevelSetEdge::histogram
478 void insertConditionConstraint(const ImplicitPtr_t& nm);
479
480 /// Get constraints parameterizing the target state foliation
481 /// \sa LevelSetEdge::histogram
482 const NumericalConstraints_t& conditionConstraints() const;
483 /// \}
484
485 /// Print the object in a stream.
486 virtual std::ostream& dotPrint(
487 std::ostream& os,
488 dot::DrawingAttributes da = dot::DrawingAttributes()) const;
489
490 protected:
491 /// Initialization of the object.
492 void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
493 const StateWkPtr_t& from, const StateWkPtr_t& to);
494
495 LevelSetEdge(const std::string& name);
496
497 /// Print the object in a stream.
498 virtual std::ostream& print(std::ostream& os) const;
499
500 /// Populate DrawingAttributes tooltip
501 virtual void populateTooltip(dot::Tooltip& tp) const;
502
503 virtual void initialize();
504
505 private:
506 // Parametrizer
507 // NumericalConstraints_t
508 NumericalConstraints_t paramNumericalConstraints_;
509
510 // Condition
511 // NumericalConstraints_t
512 NumericalConstraints_t condNumericalConstraints_;
513
514 /// This histogram will be used to find a good level set.
515 LeafHistogramPtr_t hist_;
516
517 LevelSetEdgeWkPtr_t wkPtr_;
518 }; // class LevelSetEdge
519
520 /// \}
521 } // namespace graph
522 } // namespace manipulation
523 } // namespace hpp
524
525 #endif // HPP_MANIPULATION_GRAPH_EDGE_HH
526