Directory: | ./ |
---|---|
File: | include/hpp/core/kinodynamic-oriented-path.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 12 | 37 | 32.4% |
Branches: | 4 | 36 | 11.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2016, LAAS-CNRS | ||
2 | // Authors: Pierre Fernbach (pierre.fernbach@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_CORE_KINODYNAMIC_ORIENTED_PATH_HH | ||
30 | #define HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH | ||
31 | |||
32 | #include <hpp/core/kinodynamic-path.hh> | ||
33 | |||
34 | namespace hpp { | ||
35 | namespace core { | ||
36 | /// This class is similar to \link hpp::core::KinodynamicPath KinodynamicPath | ||
37 | /// \endlink exept that the orientation of the robot always follow the direction | ||
38 | /// of the velocity. If the problem parameter "Kinodynamic/forceYawOrientation" | ||
39 | /// have been set to True, only the orientation around the z axis is set to | ||
40 | /// follow the direction of the velocity. | ||
41 | |||
42 | class HPP_CORE_DLLAPI KinodynamicOrientedPath : public KinodynamicPath { | ||
43 | public: | ||
44 | typedef KinodynamicPath parent_t; | ||
45 | 44000 | virtual ~KinodynamicOrientedPath() {} | |
46 | |||
47 | /// Create instance and return shared pointer | ||
48 | /// \param device Robot corresponding to configurations | ||
49 | /// \param init, end Start and end configurations of the path | ||
50 | /// \param length Distance between the configurations. | ||
51 | static KinodynamicOrientedPathPtr_t create( | ||
52 | const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end, | ||
53 | value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
54 | ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, | ||
55 | ConfigurationIn_t vLim, bool ignoreZValue = false) { | ||
56 | KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath( | ||
57 | device, init, end, length, a1, t0, t1, tv, t2, vLim, ignoreZValue); | ||
58 | KinodynamicOrientedPathPtr_t shPtr(ptr); | ||
59 | ptr->init(shPtr); | ||
60 | ptr->checkPath(); | ||
61 | return shPtr; | ||
62 | } | ||
63 | |||
64 | /// Create instance and return shared pointer | ||
65 | /// \param device Robot corresponding to configurations | ||
66 | /// \param init, end Start and end configurations of the path | ||
67 | /// \param length Distance between the configurations. | ||
68 | /// \param constraints the path is subject to | ||
69 | static KinodynamicOrientedPathPtr_t create( | ||
70 | const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end, | ||
71 | value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
72 | ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, | ||
73 | ConfigurationIn_t vLim, ConstraintSetPtr_t constraints, | ||
74 | bool ignoreZValue = false) { | ||
75 | KinodynamicOrientedPath* ptr = | ||
76 | new KinodynamicOrientedPath(device, init, end, length, a1, t0, t1, tv, | ||
77 | t2, vLim, constraints, ignoreZValue); | ||
78 | KinodynamicOrientedPathPtr_t shPtr(ptr); | ||
79 | ptr->init(shPtr); | ||
80 | ptr->checkPath(); | ||
81 | return shPtr; | ||
82 | } | ||
83 | |||
84 | /// Create copy and return shared pointer | ||
85 | /// \param path path to copy | ||
86 | ✗ | static KinodynamicOrientedPathPtr_t createCopy( | |
87 | const KinodynamicOrientedPathPtr_t& path) { | ||
88 | ✗ | KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath(*path); | |
89 | ✗ | KinodynamicOrientedPathPtr_t shPtr(ptr); | |
90 | ✗ | ptr->init(shPtr); | |
91 | ✗ | ptr->checkPath(); | |
92 | ✗ | return shPtr; | |
93 | } | ||
94 | |||
95 | 11000 | static KinodynamicOrientedPathPtr_t create(const KinodynamicPathPtr_t& path, | |
96 | bool ignoreZValue = false) { | ||
97 | KinodynamicOrientedPath* ptr = | ||
98 |
1/2✓ Branch 3 taken 11000 times.
✗ Branch 4 not taken.
|
11000 | new KinodynamicOrientedPath(*path, ignoreZValue); |
99 | 11000 | KinodynamicOrientedPathPtr_t shPtr(ptr); | |
100 |
1/2✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
|
11000 | ptr->init(shPtr); |
101 |
1/2✓ Branch 1 taken 11000 times.
✗ Branch 2 not taken.
|
11000 | ptr->checkPath(); |
102 | 11000 | return shPtr; | |
103 | } | ||
104 | |||
105 | /// Create copy and return shared pointer | ||
106 | /// \param path path to copy | ||
107 | /// \param constraints the path is subject to | ||
108 | ✗ | static KinodynamicOrientedPathPtr_t createCopy( | |
109 | const KinodynamicOrientedPathPtr_t& path, | ||
110 | const ConstraintSetPtr_t& constraints) { | ||
111 | KinodynamicOrientedPath* ptr = | ||
112 | ✗ | new KinodynamicOrientedPath(*path, constraints); | |
113 | ✗ | KinodynamicOrientedPathPtr_t shPtr(ptr); | |
114 | ✗ | ptr->init(shPtr); | |
115 | ✗ | ptr->checkPath(); | |
116 | ✗ | return shPtr; | |
117 | } | ||
118 | |||
119 | /// Return a shared pointer to this | ||
120 | /// | ||
121 | /// As StaightPath are immutable, and refered to by shared pointers, | ||
122 | /// they do not need to be copied. | ||
123 | ✗ | virtual PathPtr_t copy() const { return createCopy(weak_.lock()); } | |
124 | |||
125 | /// Return a shared pointer to a copy of this and set constraints | ||
126 | /// | ||
127 | /// \param constraints constraints to apply to the copy | ||
128 | /// \pre *this should not have constraints. | ||
129 | ✗ | virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const { | |
130 | ✗ | return createCopy(weak_.lock(), constraints); | |
131 | } | ||
132 | |||
133 | ✗ | bool ignoreZValue() const { return ignoreZValue_; } | |
134 | void ignoreZValue(bool ignoreZValue) { ignoreZValue_ = ignoreZValue; } | ||
135 | |||
136 | protected: | ||
137 | /// Print path in a stream | ||
138 | ✗ | virtual std::ostream& print(std::ostream& os) const { | |
139 | ✗ | os << "KinodynamicOrientedPath:" << std::endl; | |
140 | ✗ | os << "interval: [ " << timeRange().first << ", " << timeRange().second | |
141 | ✗ | << " ]" << std::endl; | |
142 | ✗ | os << "initial configuration: " << pinocchio::displayConfig(initial_) | |
143 | ✗ | << std::endl; | |
144 | ✗ | os << "final configuration: " << pinocchio::displayConfig(end_) | |
145 | ✗ | << std::endl; | |
146 | ✗ | return os; | |
147 | } | ||
148 | |||
149 | void orienteInitAndGoal(const DevicePtr_t& device); | ||
150 | |||
151 | /// Constructor | ||
152 | KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
153 | ConfigurationIn_t end, value_type length, | ||
154 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
155 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
156 | ConfigurationIn_t t2, ConfigurationIn_t vLim, | ||
157 | bool ignoreZValue); | ||
158 | |||
159 | /// Constructor with constraints | ||
160 | KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
161 | ConfigurationIn_t end, value_type length, | ||
162 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
163 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
164 | ConfigurationIn_t t2, ConfigurationIn_t vLim, | ||
165 | ConstraintSetPtr_t constraints, bool ignoreZValue); | ||
166 | |||
167 | /// Copy constructor | ||
168 | KinodynamicOrientedPath(const KinodynamicOrientedPath& path); | ||
169 | |||
170 | /// constructor from KinodynamicPath | ||
171 | KinodynamicOrientedPath(const KinodynamicPath& path, bool ignoreZValue); | ||
172 | |||
173 | /// Copy constructor with constraints | ||
174 | KinodynamicOrientedPath(const KinodynamicOrientedPath& path, | ||
175 | const ConstraintSetPtr_t& constraints); | ||
176 | |||
177 | 11000 | void init(KinodynamicOrientedPathPtr_t self) { | |
178 |
1/2✓ Branch 2 taken 11000 times.
✗ Branch 3 not taken.
|
11000 | parent_t::init(self); |
179 | 11000 | weak_ = self; | |
180 | 11000 | checkPath(); | |
181 | 11000 | } | |
182 | |||
183 | virtual bool impl_compute(ConfigurationOut_t result, value_type t) const; | ||
184 | |||
185 | virtual PathPtr_t impl_extract(const interval_t& subInterval) const; | ||
186 | |||
187 | private: | ||
188 | KinodynamicOrientedPathWkPtr_t weak_; | ||
189 | bool ignoreZValue_; | ||
190 | }; // class kinodynamic oriented path | ||
191 | } // namespace core | ||
192 | } // namespace hpp | ||
193 | |||
194 | #endif // HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH | ||
195 |