GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/graph-path-validation.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 0 20 0.0%
Branches: 0 20 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_GRAPHPATHVALIDATOR_HH
30 #define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
31
32 #include <hpp/core/obstacle-user.hh>
33 #include <hpp/core/path-validation.hh>
34 #include <hpp/manipulation/config.hh>
35 #include <hpp/manipulation/fwd.hh>
36 #include <hpp/manipulation/graph/fwd.hh>
37
38 namespace hpp {
39 namespace manipulation {
40 using hpp::core::Path;
41 using hpp::core::PathPtr_t;
42 using hpp::core::PathValidation;
43 using hpp::core::PathValidationPtr_t;
44 using hpp::core::PathVector;
45 using hpp::core::PathVectorPtr_t;
46
47 /// \addtogroup validation
48 /// \{
49
50 /// Path validation for a constraint graph
51 ///
52 /// This class encapsulates another path validation class.
53 /// The encapsulated path validation is responsible for collision
54 /// checking, whereas this class checks if a path is valid regarding
55 /// the constraint graph.
56 class HPP_MANIPULATION_DLLAPI GraphPathValidation
57 : public PathValidation,
58 public core::ObstacleUserInterface {
59 public:
60 /// Check that path is valid regarding the constraint graph.
61 ///
62 /// \param path the path to check for validity,
63 /// \param reverse if true check from the end,
64 /// \retval the extracted valid part of the path, pointer to path if
65 /// path is valid,
66 /// \retval report information about the validation process. unused in
67 /// this case,
68 /// \return whether the whole path is valid.
69 ///
70 /// \notice Call the encapsulated PathValidation::validate.
71 virtual bool validate(const PathPtr_t& path, bool reverse,
72 PathPtr_t& validPart,
73 PathValidationReportPtr_t& report);
74
75 /// Set the encapsulated path validator.
76 void innerValidation(const PathValidationPtr_t& pathValidation) {
77 pathValidation_ = pathValidation;
78 }
79
80 /// Get the encapsulated path validator.
81 ✗ const PathValidationPtr_t& innerValidation() { return pathValidation_; }
82
83 /// Set the graph of constraints
84 ✗ void constraintGraph(const graph::GraphPtr_t& graph) {
85 ✗ constraintGraph_ = graph;
86 }
87
88 /// Get the graph of constraints
89 graph::GraphPtr_t constraintGraph() const { return constraintGraph_; }
90
91 /// Create a new instance of this class.
92 /// \param pathValidation a PathValidation that is responsible for collision
93 // checking.
94 // \param graph A pointer to the constraint graph.
95 static GraphPathValidationPtr_t create(
96 const PathValidationPtr_t& pathValidation);
97
98 template <typename T>
99 static GraphPathValidationPtr_t create(const pinocchio::DevicePtr_t& robot,
100 const value_type& stepSize);
101
102 /// Add obstacle in the environment
103 ///
104 /// Dynamic cast inner path validation into
105 /// hpp::core::ObstacleUserInterface and calls
106 /// hpp::core::ObstacleUserInterface::addObstacle in case of success.
107 ✗ void addObstacle(const hpp::core::CollisionObjectConstPtr_t& object) {
108 shared_ptr<core::ObstacleUserInterface> oui =
109 ✗ HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
110 ✗ if (oui) oui->addObstacle(object);
111 }
112
113 /// Remove a collision pair between a joint and an obstacle
114 ///
115 /// Dynamic cast inner path validation into
116 /// hpp::core::ObstacleUserInterface and calls
117 /// hpp::core::ObstacleUserInterface::removeObstacleFromJoint in case
118 /// of success.
119 ✗ void removeObstacleFromJoint(
120 const JointPtr_t& joint,
121 const pinocchio::CollisionObjectConstPtr_t& obstacle) {
122 shared_ptr<core::ObstacleUserInterface> oui =
123 ✗ HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
124 ✗ if (oui) oui->removeObstacleFromJoint(joint, obstacle);
125 }
126
127 /// \brief Filter collision pairs.
128 ///
129 /// Dynamic cast inner path validation into
130 /// hpp::core::ObstacleUserInterface and calls
131 /// hpp::core::ObstacleUserInterface::filterCollisionPairs in case of
132 /// success.
133 ✗ void filterCollisionPairs(
134 const core::RelativeMotion::matrix_type& relMotion) {
135 shared_ptr<core::ObstacleUserInterface> oui =
136 ✗ HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
137 ✗ if (oui) oui->filterCollisionPairs(relMotion);
138 }
139
140 /// Set different security margins for collision pairs
141 ///
142 /// Dynamic cast inner path validation into
143 /// hpp::core::ObstacleUserInterface and calls
144 /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
145 /// success.
146 ✗ void setSecurityMargins(const matrix_t& securityMargins) {
147 shared_ptr<core::ObstacleUserInterface> oui =
148 ✗ HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
149 ✗ if (oui) oui->setSecurityMargins(securityMargins);
150 }
151
152 /// \copydoc hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
153 ///
154 /// Dynamic cast inner path validation into
155 /// hpp::core::ObstacleUserInterface and calls
156 /// hpp::core::ObstacleUserInterface::setSecurityMargins in case of
157 /// success.
158 ✗ void setSecurityMarginBetweenBodies(const std::string& body_a,
159 const std::string& body_b,
160 const value_type& margin) {
161 shared_ptr<core::ObstacleUserInterface> oui =
162 ✗ HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
163 ✗ if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin);
164 }
165
166 protected:
167 /// Constructor
168 GraphPathValidation(const PathValidationPtr_t& pathValidation);
169
170 private:
171 /// Do validation regarding the constraint graph for PathVector
172 bool impl_validate(const PathVectorPtr_t& path, bool reverse,
173 PathPtr_t& validPart, PathValidationReportPtr_t& report);
174 /// Do validation regarding the constraint graph for Path
175 bool impl_validate(const PathPtr_t& path, bool reverse, PathPtr_t& validPart,
176 PathValidationReportPtr_t& report);
177 /// The encapsulated PathValidation.
178 PathValidationPtr_t pathValidation_;
179 /// Pointer to the constraint graph.
180 graph::GraphPtr_t constraintGraph_;
181 };
182
183 template <typename T>
184 ✗ GraphPathValidationPtr_t GraphPathValidation::create(
185 const pinocchio::DevicePtr_t& robot, const value_type& stepSize) {
186 ✗ return GraphPathValidation::create(T::create(robot, stepSize));
187 }
188 /// \}
189 } // namespace manipulation
190 } // namespace hpp
191
192 #endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
193