| 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 |