Directory: | ./ |
---|---|
File: | include/hpp/core/kinodynamic-path.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 24 | 48 | 50.0% |
Branches: | 23 | 74 | 31.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_PATH_HH | ||
30 | #define HPP_CORE_KINODYNAMIC_PATH_HH | ||
31 | |||
32 | #include <hpp/core/config.hh> | ||
33 | #include <hpp/core/fwd.hh> | ||
34 | #include <hpp/core/straight-path.hh> | ||
35 | #include <hpp/pinocchio/configuration.hh> | ||
36 | |||
37 | namespace hpp { | ||
38 | namespace core { | ||
39 | /// \addtogroup path | ||
40 | /// \{ | ||
41 | |||
42 | /// Kino-dynamic straight path | ||
43 | /// | ||
44 | /// This Path has the same behavior as the \link hpp::core::StraightPath | ||
45 | /// StraightPath \endlink class except for the translation part of the | ||
46 | /// free-flyer. For the translation part of the free-flyer KinodynamicPath store | ||
47 | /// a "bang-bang" trajectory dependent on time with either 2 segment of constant | ||
48 | /// acceleration or 3 segments with a constant velocity segment | ||
49 | /// | ||
50 | /// In current implementation, only the translation part of the freeflyer joint | ||
51 | /// is considered by this class. The value of all other joint are interpolated | ||
52 | /// between the initial and end value using the interpolate() method. | ||
53 | /// | ||
54 | /// The current implementation assume that : | ||
55 | /// * The first joint of the robot is a freeflyer | ||
56 | /// * The robot have an extra Config Space of dimension >= 6. | ||
57 | /// The first 3 values of the extraConfig are the velocity of the root and the 3 | ||
58 | /// other values are the aceleration. | ||
59 | class HPP_CORE_DLLAPI KinodynamicPath : public StraightPath { | ||
60 | public: | ||
61 | typedef StraightPath parent_t; | ||
62 | /// Destructor | ||
63 | 110044 | virtual ~KinodynamicPath() {} | |
64 | |||
65 | /// Create instance and return shared pointer | ||
66 | /// \param device Robot corresponding to configurations | ||
67 | /// \param init, end Start and end configurations of the path | ||
68 | /// \param length Distance between the configurations. | ||
69 | 2007 | static KinodynamicPathPtr_t create(const DevicePtr_t& device, | |
70 | ConfigurationIn_t init, | ||
71 | ConfigurationIn_t end, value_type length, | ||
72 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
73 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
74 | ConfigurationIn_t t2, | ||
75 | ConfigurationIn_t vLim) { | ||
76 | KinodynamicPath* ptr = new KinodynamicPath(device, init, end, length, a1, | ||
77 |
9/18✓ Branch 2 taken 2007 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2007 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2007 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2007 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2007 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2007 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2007 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2007 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 2007 times.
✗ Branch 27 not taken.
|
2007 | t0, t1, tv, t2, vLim); |
78 | 2007 | KinodynamicPathPtr_t shPtr(ptr); | |
79 |
1/2✓ Branch 2 taken 2007 times.
✗ Branch 3 not taken.
|
2007 | ptr->init(shPtr); |
80 |
1/2✓ Branch 1 taken 2007 times.
✗ Branch 2 not taken.
|
2007 | ptr->checkPath(); |
81 | 2007 | return shPtr; | |
82 | } | ||
83 | |||
84 | /// Create instance and return shared pointer | ||
85 | /// \param device Robot corresponding to configurations | ||
86 | /// \param init, end Start and end configurations of the path | ||
87 | /// \param length Distance between the configurations. | ||
88 | /// \param constraints the path is subject to | ||
89 | 20004 | static KinodynamicPathPtr_t create( | |
90 | const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end, | ||
91 | value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
92 | ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, | ||
93 | ConfigurationIn_t vLim, ConstraintSetPtr_t constraints) { | ||
94 | KinodynamicPath* ptr = new KinodynamicPath( | ||
95 |
9/18✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20004 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20004 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20004 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20004 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20004 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20004 times.
✗ Branch 21 not taken.
✓ Branch 24 taken 20004 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 20004 times.
✗ Branch 28 not taken.
|
20004 | device, init, end, length, a1, t0, t1, tv, t2, vLim, constraints); |
96 | 20004 | KinodynamicPathPtr_t shPtr(ptr); | |
97 |
1/2✓ Branch 2 taken 20004 times.
✗ Branch 3 not taken.
|
20004 | ptr->init(shPtr); |
98 |
1/2✓ Branch 1 taken 20004 times.
✗ Branch 2 not taken.
|
20004 | ptr->checkPath(); |
99 | 20004 | return shPtr; | |
100 | } | ||
101 | |||
102 | /// Create copy and return shared pointer | ||
103 | /// \param path path to copy | ||
104 | ✗ | static KinodynamicPathPtr_t createCopy(const KinodynamicPathPtr_t& path) { | |
105 | ✗ | KinodynamicPath* ptr = new KinodynamicPath(*path); | |
106 | ✗ | KinodynamicPathPtr_t shPtr(ptr); | |
107 | ✗ | ptr->init(shPtr); | |
108 | ✗ | ptr->checkPath(); | |
109 | ✗ | return shPtr; | |
110 | } | ||
111 | |||
112 | /// Create copy and return shared pointer | ||
113 | /// \param path path to copy | ||
114 | /// \param constraints the path is subject to | ||
115 | ✗ | static KinodynamicPathPtr_t createCopy( | |
116 | const KinodynamicPathPtr_t& path, const ConstraintSetPtr_t& constraints) { | ||
117 | ✗ | KinodynamicPath* ptr = new KinodynamicPath(*path, constraints); | |
118 | ✗ | KinodynamicPathPtr_t shPtr(ptr); | |
119 | ✗ | ptr->init(shPtr); | |
120 | ✗ | ptr->checkPath(); | |
121 | ✗ | return shPtr; | |
122 | } | ||
123 | |||
124 | /// Return a shared pointer to this | ||
125 | /// | ||
126 | /// As StaightPath are immutable, and refered to by shared pointers, | ||
127 | /// they do not need to be copied. | ||
128 | ✗ | virtual PathPtr_t copy() const { return createCopy(weak_.lock()); } | |
129 | |||
130 | /// Return a shared pointer to a copy of this and set constraints | ||
131 | /// | ||
132 | /// \param constraints constraints to apply to the copy | ||
133 | /// \pre *this should not have constraints. | ||
134 | ✗ | virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const { | |
135 | ✗ | return createCopy(weak_.lock(), constraints); | |
136 | } | ||
137 | |||
138 | /// Extraction/Reversion of a sub-path | ||
139 | /// \param subInterval interval of definition of the extract path | ||
140 | /// If upper bound of subInterval is smaller than lower bound, return an empty | ||
141 | /// path | ||
142 | virtual PathPtr_t impl_extract(const interval_t& paramInterval) const; | ||
143 | |||
144 | 63042 | vector_t getT0() { return t0_; } | |
145 | |||
146 | 123052 | vector_t getT1() { return t1_; } | |
147 | |||
148 | 123046 | vector_t getT2() { return t2_; } | |
149 | |||
150 | 123038 | vector_t getTv() { return tv_; } | |
151 | |||
152 | 43 | vector_t getA1() { return a1_; } | |
153 | |||
154 | protected: | ||
155 | /// Print path in a stream | ||
156 | ✗ | virtual std::ostream& print(std::ostream& os) const { | |
157 | ✗ | os << "KinodynamicPath:" << std::endl; | |
158 | ✗ | os << "interval: [ " << timeRange().first << ", " << timeRange().second | |
159 | ✗ | << " ]" << std::endl; | |
160 | ✗ | os << "initial configuration: " << pinocchio::displayConfig(initial_) | |
161 | ✗ | << std::endl; | |
162 | ✗ | os << "final configuration: " << pinocchio::displayConfig(end_) | |
163 | ✗ | << std::endl; | |
164 | ✗ | return os; | |
165 | } | ||
166 | /// Constructor | ||
167 | KinodynamicPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
168 | ConfigurationIn_t end, value_type length, | ||
169 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
170 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
171 | ConfigurationIn_t t2, ConfigurationIn_t vLim); | ||
172 | |||
173 | /// Constructor with constraints | ||
174 | KinodynamicPath(const DevicePtr_t& robot, ConfigurationIn_t init, | ||
175 | ConfigurationIn_t end, value_type length, | ||
176 | ConfigurationIn_t a1, ConfigurationIn_t t0, | ||
177 | ConfigurationIn_t t1, ConfigurationIn_t tv, | ||
178 | ConfigurationIn_t t2, ConfigurationIn_t vLim, | ||
179 | ConstraintSetPtr_t constraints); | ||
180 | |||
181 | /// Copy constructor | ||
182 | KinodynamicPath(const KinodynamicPath& path); | ||
183 | |||
184 | /// Copy constructor with constraints | ||
185 | KinodynamicPath(const KinodynamicPath& path, | ||
186 | const ConstraintSetPtr_t& constraints); | ||
187 | |||
188 | 33011 | void init(KinodynamicPathPtr_t self) { | |
189 |
1/2✓ Branch 2 taken 33011 times.
✗ Branch 3 not taken.
|
33011 | parent_t::init(self); |
190 | 33011 | weak_ = self; | |
191 | 33011 | checkPath(); | |
192 | 33011 | } | |
193 | |||
194 | virtual bool impl_compute(ConfigurationOut_t result, value_type t) const; | ||
195 | |||
196 | inline double sgnenum(double val) const { return ((0. < val) - (val < 0.)); } | ||
197 | |||
198 | inline int sgn(double d) const { return d >= 0.0 ? 1 : -1; } | ||
199 | |||
200 | inline double sgnf(double d) const { return d >= 0.0 ? 1.0 : -1.0; } | ||
201 | |||
202 | 1175139 | const DevicePtr_t& device() const { return device_; } | |
203 | |||
204 | private: | ||
205 | KinodynamicPathWkPtr_t weak_; | ||
206 | DevicePtr_t device_; | ||
207 | vector_t a1_; | ||
208 | vector_t t0_, t1_, tv_, t2_, vLim_; | ||
209 | }; // class KinodynamicPath | ||
210 | /// \} | ||
211 | } // namespace core | ||
212 | } // namespace hpp | ||
213 | #endif // HPP_CORE_KINODYNAMIC_PATH_HH | ||
214 |