Directory: | ./ |
---|---|
File: | include/hpp/core/steering-method.hh |
Date: | 2024-12-13 16:14:03 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 19 | 23 | 82.6% |
Branches: | 7 | 36 | 19.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2014 CNRS | ||
3 | // Authors: Florent Lamiraux | ||
4 | // | ||
5 | |||
6 | // Redistribution and use in source and binary forms, with or without | ||
7 | // modification, are permitted provided that the following conditions are | ||
8 | // met: | ||
9 | // | ||
10 | // 1. Redistributions of source code must retain the above copyright | ||
11 | // notice, this list of conditions and the following disclaimer. | ||
12 | // | ||
13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
14 | // notice, this list of conditions and the following disclaimer in the | ||
15 | // documentation and/or other materials provided with the distribution. | ||
16 | // | ||
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
28 | // DAMAGE. | ||
29 | |||
30 | #ifndef HPP_CORE_STEERING_METHOD_HH | ||
31 | #define HPP_CORE_STEERING_METHOD_HH | ||
32 | |||
33 | #include <hpp/core/fwd.hh> | ||
34 | #include <hpp/core/path.hh> | ||
35 | #include <hpp/core/projection-error.hh> | ||
36 | #include <hpp/util/debug.hh> | ||
37 | |||
38 | namespace hpp { | ||
39 | namespace core { | ||
40 | /// \addtogroup steering_method | ||
41 | /// \{ | ||
42 | |||
43 | /// Steering method | ||
44 | /// | ||
45 | /// A steering method creates paths between pairs of | ||
46 | /// configurations for a robot. They are usually used to take | ||
47 | /// into account nonholonomic constraints of some robots | ||
48 | class HPP_CORE_DLLAPI SteeringMethod { | ||
49 | public: | ||
50 | /// create a path between two configurations | ||
51 | /// \return a Path from q1 to q2 if found. An empty | ||
52 | /// Path if Path could not be built. | ||
53 | /// \note if q1 == q2, the steering method should not return an empty | ||
54 | /// shared pointer. | ||
55 | 1858684 | PathPtr_t operator()(ConfigurationIn_t q1, ConfigurationIn_t q2) const { | |
56 | 1858684 | PathPtr_t path; | |
57 | try { | ||
58 |
3/6✓ Branch 1 taken 1858684 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1858684 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1858684 times.
✗ Branch 8 not taken.
|
1858684 | path = impl_compute(q1, q2); |
59 | ✗ | } catch (const projection_error& e) { | |
60 | ✗ | hppDout(info, "Could not build path: " << e.what()); | |
61 | } | ||
62 |
2/6✓ Branch 1 taken 1858684 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1858684 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
1858684 | assert(q1 != q2 || path); |
63 | 1858684 | return path; | |
64 | } | ||
65 | |||
66 | /// \copydoc SteeringMethod::operator()(ConfigurationIn_t,ConfigurationIn_t) | ||
67 | ✗ | PathPtr_t steer(ConfigurationIn_t q1, ConfigurationIn_t q2) const { | |
68 | ✗ | return this->operator()(q1, q2); | |
69 | } | ||
70 | |||
71 | 192 | virtual ~SteeringMethod() {}; | |
72 | |||
73 | /// Copy instance and return shared pointer | ||
74 | virtual SteeringMethodPtr_t copy() const = 0; | ||
75 | |||
76 | 1876860 | ProblemConstPtr_t problem() const { return problem_.lock(); } | |
77 | |||
78 | /// \name Constraints applicable to the robot. | ||
79 | /// These constraints are not automatically taken into | ||
80 | /// account. Child class can use it if they need. | ||
81 | /// \{ | ||
82 | |||
83 | /// Set constraint set | ||
84 | 156 | void constraints(const ConstraintSetPtr_t& constraints) { | |
85 | 156 | constraints_ = constraints; | |
86 | 156 | } | |
87 | |||
88 | /// Get constraint set | ||
89 | 1861050 | const ConstraintSetPtr_t& constraints() const { return constraints_; } | |
90 | /// \} | ||
91 | |||
92 | protected: | ||
93 | /// Constructor | ||
94 | 93 | SteeringMethod(const ProblemConstPtr_t& problem) | |
95 | 93 | : problem_(problem), constraints_(), weak_() {} | |
96 | /// Copy constructor | ||
97 | /// | ||
98 | /// Constraints are copied | ||
99 | 12 | SteeringMethod(const SteeringMethod& other) | |
100 | 12 | : problem_(other.problem_), constraints_(), weak_() { | |
101 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | if (other.constraints_) { |
102 | constraints_ = | ||
103 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | HPP_DYNAMIC_PTR_CAST(ConstraintSet, other.constraints_->copy()); |
104 | } | ||
105 | 12 | } | |
106 | /// create a path between two configurations | ||
107 | virtual PathPtr_t impl_compute(ConfigurationIn_t q1, | ||
108 | ConfigurationIn_t q2) const = 0; | ||
109 | /// Store weak pointer to itself. | ||
110 | 105 | void init(SteeringMethodWkPtr_t weak) { weak_ = weak; } | |
111 | |||
112 | private: | ||
113 | ProblemConstWkPtr_t problem_; | ||
114 | /// Set of constraints to apply on the paths produced | ||
115 | ConstraintSetPtr_t constraints_; | ||
116 | /// Weak pointer to itself | ||
117 | SteeringMethodWkPtr_t weak_; | ||
118 | }; // class SteeringMethod | ||
119 | /// \} | ||
120 | } // namespace core | ||
121 | } // namespace hpp | ||
122 | #endif // HPP_CORE_STEERING_METHOD_HH | ||
123 |