Directory: | ./ |
---|---|
File: | include/hpp/core/problem.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 28 | 30 | 93.3% |
Branches: | 4 | 10 | 40.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | |||
2 | // | ||
3 | // Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 CNRS | ||
4 | // Authors: Florent Lamiraux | ||
5 | // | ||
6 | |||
7 | // Redistribution and use in source and binary forms, with or without | ||
8 | // modification, are permitted provided that the following conditions are | ||
9 | // met: | ||
10 | // | ||
11 | // 1. Redistributions of source code must retain the above copyright | ||
12 | // notice, this list of conditions and the following disclaimer. | ||
13 | // | ||
14 | // 2. Redistributions in binary form must reproduce the above copyright | ||
15 | // notice, this list of conditions and the following disclaimer in the | ||
16 | // documentation and/or other materials provided with the distribution. | ||
17 | // | ||
18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
22 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
29 | // DAMAGE. | ||
30 | |||
31 | #ifndef HPP_CORE_PROBLEM_HH | ||
32 | #define HPP_CORE_PROBLEM_HH | ||
33 | |||
34 | #include <hpp/core/config.hh> | ||
35 | #include <hpp/core/container.hh> | ||
36 | #include <hpp/core/parameter.hh> | ||
37 | #include <hpp/core/steering-method.hh> | ||
38 | #include <hpp/pinocchio/device.hh> | ||
39 | #include <hpp/util/pointer.hh> | ||
40 | #include <stdexcept> | ||
41 | |||
42 | namespace hpp { | ||
43 | namespace core { | ||
44 | /// \addtogroup path_planning | ||
45 | /// \{ | ||
46 | |||
47 | /// Defines a path planning problem for one robot. | ||
48 | /// A path planning problem is defined by | ||
49 | /// \li a robot: instance of class hpp::pinocchio::Device, | ||
50 | /// \li a set of obstacles: a list of hpp::pinocchio::CollisionObject, | ||
51 | /// \li initial and goal configurations, | ||
52 | /// \li a SteeringMethod to handle the robot dynamics, | ||
53 | /// Additional objects are stored in this object: | ||
54 | /// \li a method to validate paths, | ||
55 | /// \li a set of methods to validate configurations. Default methods are | ||
56 | /// collision checking and joint bound checking. | ||
57 | class HPP_CORE_DLLAPI Problem { | ||
58 | public: | ||
59 | /// Create a path planning problem. | ||
60 | /// \param robot robot associated to the path planning problem. | ||
61 | static ProblemPtr_t create(DevicePtr_t robot); | ||
62 | |||
63 | /// Copy a path planning problem and return a shared pointer. | ||
64 | /// \param other problem to copy | ||
65 | static ProblemPtr_t createCopy(const ProblemConstPtr_t& other); | ||
66 | |||
67 | /// Constructor without argument | ||
68 | /// \warning do not use this constructor. It is necessary to define | ||
69 | /// std::pairs <Problem, OtherType>. | ||
70 | Problem(); | ||
71 | |||
72 | virtual ~Problem(); | ||
73 | |||
74 | /// \name Problem definition | ||
75 | /// \{ | ||
76 | |||
77 | /// return shared pointer to robot. | ||
78 | 1872856 | const DevicePtr_t& robot() const { return robot_; } | |
79 | |||
80 | /// Get shared pointer to initial configuration. | ||
81 | 15 | const Configuration_t& initConfig() const { return initConf_; } | |
82 | /// Set initial configuration. | ||
83 | void initConfig(ConfigurationIn_t inConfig); | ||
84 | /// Set the target | ||
85 | 7 | void target(const ProblemTargetPtr_t& target) { target_ = target; } | |
86 | /// Get the target | ||
87 | 59 | const ProblemTargetPtr_t& target() const { return target_; } | |
88 | /// Get goal configurations. | ||
89 | /// if problem target is an instance of problemTarget::GoalConfigurations, | ||
90 | /// return the vector of goal configurations. Otherwise return an empty | ||
91 | /// vector. | ||
92 | const Configurations_t goalConfigs() const; | ||
93 | /// Add goal configuration. | ||
94 | /// if problem target is not an instance of | ||
95 | /// problemTarget::GoalConfigurations, set problem target as a new | ||
96 | /// instance of this class and add the given configuration as a goal. | ||
97 | void addGoalConfig(ConfigurationIn_t config); | ||
98 | /// Reset the set of goal configurations | ||
99 | /// if problem target is not an instance of | ||
100 | /// problemTarget::GoalConfigurations, set problem target as a new | ||
101 | /// empty instance of this class. | ||
102 | void resetGoalConfigs(); | ||
103 | |||
104 | /// \} | ||
105 | |||
106 | /// \name Steering method and distance function | ||
107 | /// \{ | ||
108 | |||
109 | /// Set steering method | ||
110 | /// \param sm steering method. | ||
111 | /// If problem contains constraints they are passed to the steering | ||
112 | /// method. | ||
113 | 20 | void steeringMethod(const SteeringMethodPtr_t& sm) { | |
114 | 20 | steeringMethod_ = sm; | |
115 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | if (constraints_) steeringMethod_->constraints(constraints_); |
116 | 20 | } | |
117 | |||
118 | /// Get steering method | ||
119 | 199 | SteeringMethodPtr_t steeringMethod() const { return steeringMethod_; } | |
120 | |||
121 | /// Set distance between configurations | ||
122 | ✗ | void distance(const DistancePtr_t& distance) { distance_ = distance; } | |
123 | /// Get distance between configuration | ||
124 | 4151 | const DistancePtr_t& distance() const { return distance_; } | |
125 | /// \} | ||
126 | |||
127 | /// \name Configuration validation | ||
128 | /// \{ | ||
129 | |||
130 | /// Set configuration validation methods | ||
131 | /// Before starting tos solve a path planning problem, the initial and | ||
132 | /// goal configurations are checked for validity. | ||
133 | void configValidation(const ConfigValidationsPtr_t& configValidations) { | ||
134 | configValidations_ = configValidations; | ||
135 | } | ||
136 | /// Get configuration validation methods | ||
137 | 3 | const ConfigValidationsPtr_t& configValidations() const { | |
138 | 3 | return configValidations_; | |
139 | } | ||
140 | |||
141 | /// Clear the ConfigValidations | ||
142 | /// by calling ConfigValidations::clear | ||
143 | void clearConfigValidations(); | ||
144 | |||
145 | /// Add a config validation method | ||
146 | void addConfigValidation(const ConfigValidationPtr_t& configValidation); | ||
147 | |||
148 | /// \name Path validation | ||
149 | /// \{ | ||
150 | /// Set path validation method | ||
151 | virtual void pathValidation(const PathValidationPtr_t& pathValidation); | ||
152 | |||
153 | /// Get path validation method | ||
154 | 82 | PathValidationPtr_t pathValidation() const { return pathValidation_; } | |
155 | /// \} | ||
156 | |||
157 | /// \name Configuration shooter | ||
158 | /// \{ | ||
159 | /// Set configuration shooter method | ||
160 | void configurationShooter( | ||
161 | const ConfigurationShooterPtr_t& configurationShooter); | ||
162 | |||
163 | /// Get path validation method | ||
164 | 7 | ConfigurationShooterPtr_t configurationShooter() const { | |
165 | 7 | return configurationShooter_; | |
166 | } | ||
167 | /// \} | ||
168 | |||
169 | /// \name Path projector | ||
170 | /// \{ | ||
171 | /// Set path projector method | ||
172 | 7 | void pathProjector(const PathProjectorPtr_t& pathProjector) { | |
173 | 7 | pathProjector_ = pathProjector; | |
174 | 7 | } | |
175 | |||
176 | /// Get path projector method | ||
177 | 81 | PathProjectorPtr_t pathProjector() const { return pathProjector_; } | |
178 | /// \} | ||
179 | |||
180 | /// \name Constraints applicable to the robot | ||
181 | /// \{ | ||
182 | |||
183 | /// Set constraint set | ||
184 | /// \param constraints a set of constraints | ||
185 | /// If problem contains a steering method, constraints are passed to | ||
186 | /// the steering method. | ||
187 | 54 | void constraints(const ConstraintSetPtr_t& constraints) { | |
188 | 54 | constraints_ = constraints; | |
189 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | if (steeringMethod_) steeringMethod_->constraints(constraints); |
190 | 54 | } | |
191 | |||
192 | /// Get constraint set | ||
193 | 1 | const ConstraintSetPtr_t& constraints() const { return constraints_; } | |
194 | /// \} | ||
195 | |||
196 | /// Check that problem is well formulated | ||
197 | virtual void checkProblem() const; | ||
198 | |||
199 | /// \name Obstacles | ||
200 | /// \{ | ||
201 | |||
202 | /// Add obstacle to the list. | ||
203 | /// \param object a new object. | ||
204 | void addObstacle(const CollisionObjectPtr_t& object); | ||
205 | /// Remove a collision pair between a joint and an obstacle | ||
206 | /// \param joint that holds the inner objects, | ||
207 | /// \param obstacle to remove. | ||
208 | void removeObstacleFromJoint(const JointPtr_t& joint, | ||
209 | const CollisionObjectConstPtr_t& obstacle); | ||
210 | |||
211 | /// Build matrix of relative motions between joints | ||
212 | /// | ||
213 | /// Loop over constraints in the current constraint set | ||
214 | /// (see Problem::constraints) and for each LockedJoint and each | ||
215 | /// constraints::RelativeTransformation, fill a matrix the column and | ||
216 | /// rows represent joints and the values are the following | ||
217 | /// \li RelativeMotionType::Constrained when the two joints are rigidly | ||
218 | /// fixed by the constraint, | ||
219 | /// \li RelativeMotionType::Parameterized when the two joints are rigidly | ||
220 | /// fixed by the constraint, the relative position is a parameter | ||
221 | /// constant along path, but that can change for different paths, | ||
222 | /// \li RelativeMotionType::Unconstrained when the two joints are not | ||
223 | /// rigidly fixed by the constraint. | ||
224 | /// | ||
225 | /// \note the matrix is passed to the current configuration validation | ||
226 | /// instance (Problem::configValidation) and to the current | ||
227 | /// path validation instance (Problem::pathValidation). | ||
228 | void filterCollisionPairs(); | ||
229 | |||
230 | /// \copydoc ObstacleUserInterface::setSecurityMargins | ||
231 | void setSecurityMargins(const matrix_t& securityMatrix); | ||
232 | |||
233 | /// Vector of objects considered for collision detection | ||
234 | const ObjectStdVector_t& collisionObstacles() const; | ||
235 | /// Set the vector of objects considered for collision detection | ||
236 | void collisionObstacles(const ObjectStdVector_t& collisionObstacles); | ||
237 | /// \} | ||
238 | |||
239 | /// Get a parameter named name. | ||
240 | /// | ||
241 | /// \param name of the parameter. | ||
242 | 328 | const Parameter& getParameter(const std::string& name) const { | |
243 |
2/2✓ Branch 1 taken 53 times.
✓ Branch 2 taken 275 times.
|
328 | if (parameters.has(name)) |
244 | 53 | return parameters.get(name); | |
245 | else | ||
246 | 275 | return parameterDescription(name).defaultValue(); | |
247 | } | ||
248 | |||
249 | /// Set a parameter named name. | ||
250 | /// | ||
251 | /// \param name of the parameter. | ||
252 | /// \param value value of the parameter | ||
253 | /// \throw std::invalid_argument if a parameter exists but has a different | ||
254 | /// type. | ||
255 | void setParameter(const std::string& name, const Parameter& value); | ||
256 | |||
257 | /// Declare a parameter | ||
258 | /// In shared library, use the following snippet in your cc file: | ||
259 | /// \code{.cpp} | ||
260 | /// HPP_START_PARAMETER_DECLARATION(name) | ||
261 | /// Problem::declareParameter (ParameterDescription ( | ||
262 | /// Parameter::FLOAT, | ||
263 | /// "name", | ||
264 | /// "doc", | ||
265 | /// Parameter(value))); | ||
266 | /// HPP_END_PARAMETER_DECLARATION(name) | ||
267 | /// \endcode | ||
268 | static void declareParameter(const ParameterDescription& desc); | ||
269 | |||
270 | /// Get all the parameter descriptions | ||
271 | static const Container<ParameterDescription>& parameterDescriptions(); | ||
272 | |||
273 | /// Access one parameter description | ||
274 | static const ParameterDescription& parameterDescription( | ||
275 | const std::string& name); | ||
276 | |||
277 | Container<Parameter> parameters; | ||
278 | |||
279 | protected: | ||
280 | /// \copydoc Problem::create(DevicePtr_t); | ||
281 | Problem(DevicePtr_t robot); | ||
282 | /// \copydoc Problem::create(DevicePtr_t); | ||
283 | ✗ | Problem(const Problem& other) = default; | |
284 | |||
285 | void init(ProblemWkPtr_t wkPtr); | ||
286 | |||
287 | private: | ||
288 | ProblemWkPtr_t wkPtr_; | ||
289 | /// The robot | ||
290 | DevicePtr_t robot_; | ||
291 | /// Distance between configurations of the robot | ||
292 | DistancePtr_t distance_; | ||
293 | /// Shared pointer to initial configuration. | ||
294 | Configuration_t initConf_; | ||
295 | /// Shared pointer to problem target | ||
296 | ProblemTargetPtr_t target_; | ||
297 | /// Steering method associated to the problem | ||
298 | SteeringMethodPtr_t steeringMethod_; | ||
299 | /// Configuration validation | ||
300 | ConfigValidationsPtr_t configValidations_; | ||
301 | /// Path validation | ||
302 | PathValidationPtr_t pathValidation_; | ||
303 | /// Path projector | ||
304 | PathProjectorPtr_t pathProjector_; | ||
305 | /// List of obstacles | ||
306 | ObjectStdVector_t collisionObstacles_; | ||
307 | /// Set of constraints applicable to the robot | ||
308 | ConstraintSetPtr_t constraints_; | ||
309 | /// Configuration shooter | ||
310 | ConfigurationShooterPtr_t configurationShooter_; | ||
311 | }; // class Problem | ||
312 | /// \} | ||
313 | } // namespace core | ||
314 | } // namespace hpp | ||
315 | |||
316 | #define HPP_START_PARAMETER_DECLARATION(name) \ | ||
317 | struct HPP_CORE_DLLAPI __InitializerClass_##name { \ | ||
318 | __InitializerClass_##name() { | ||
319 | #define HPP_END_PARAMETER_DECLARATION(name) \ | ||
320 | } \ | ||
321 | } \ | ||
322 | ; \ | ||
323 | extern "C" { \ | ||
324 | __InitializerClass_##name __instance_##name; \ | ||
325 | } | ||
326 | |||
327 | #endif // HPP_CORE_PROBLEM_HH | ||
328 |