Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (c) 2008-2014, Andrew Walker |
2 |
|
|
// 2017 Florent Lamiraux |
3 |
|
|
// |
4 |
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy |
5 |
|
|
// of this software and associated documentation files (the "Software"), to deal |
6 |
|
|
// in the Software without restriction, including without limitation the rights |
7 |
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
8 |
|
|
// copies of the Software, and to permit persons to whom the Software is |
9 |
|
|
// furnished to do so, subject to the following conditions: |
10 |
|
|
// |
11 |
|
|
// The above copyright notice and this permission notice shall be included in |
12 |
|
|
// all copies or substantial portions of the Software. |
13 |
|
|
// |
14 |
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
15 |
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
16 |
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
17 |
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
18 |
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
19 |
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
20 |
|
|
// THE SOFTWARE. |
21 |
|
|
|
22 |
|
|
#include <math.h> |
23 |
|
|
|
24 |
|
|
#include <boost/serialization/vector.hpp> |
25 |
|
|
#include <boost/serialization/weak_ptr.hpp> |
26 |
|
|
#include <hpp/core/dubins-path.hh> |
27 |
|
|
#include <hpp/core/steering-method/constant-curvature.hh> |
28 |
|
|
#include <hpp/pinocchio/configuration.hh> |
29 |
|
|
#include <hpp/pinocchio/device.hh> |
30 |
|
|
#include <hpp/pinocchio/joint.hh> |
31 |
|
|
#include <hpp/pinocchio/serialization.hh> |
32 |
|
|
#include <hpp/util/debug.hh> |
33 |
|
|
#include <hpp/util/serialization.hh> |
34 |
|
|
#include <pinocchio/serialization/eigen.hpp> |
35 |
|
|
#include <pinocchio/spatial/se3.hpp> |
36 |
|
|
|
37 |
|
|
#include "dubins.hh" |
38 |
|
|
|
39 |
|
|
namespace hpp { |
40 |
|
|
namespace core { |
41 |
|
|
using steeringMethod::ConstantCurvature; |
42 |
|
|
using steeringMethod::ConstantCurvaturePtr_t; |
43 |
|
|
|
44 |
|
✗ |
DubinsPathPtr_t DubinsPath::create(const DevicePtr_t& device, |
45 |
|
|
ConfigurationIn_t init, |
46 |
|
|
ConfigurationIn_t end, |
47 |
|
|
value_type extraLength, value_type rho, |
48 |
|
|
size_type xyId, size_type rzId, |
49 |
|
|
const std::vector<JointPtr_t> wheels) { |
50 |
|
|
DubinsPath* ptr = |
51 |
|
✗ |
new DubinsPath(device, init, end, extraLength, rho, xyId, rzId, wheels); |
52 |
|
✗ |
DubinsPathPtr_t shPtr(ptr); |
53 |
|
|
try { |
54 |
|
✗ |
ptr->init(shPtr); |
55 |
|
✗ |
} catch (const std::exception& exc) { |
56 |
|
✗ |
shPtr.reset(); |
57 |
|
|
} |
58 |
|
✗ |
return shPtr; |
59 |
|
|
} |
60 |
|
|
|
61 |
|
✗ |
DubinsPathPtr_t DubinsPath::create( |
62 |
|
|
const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end, |
63 |
|
|
value_type extraLength, value_type rho, size_type xyId, size_type rzId, |
64 |
|
|
const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints) { |
65 |
|
|
DubinsPath* ptr = new DubinsPath(device, init, end, extraLength, rho, xyId, |
66 |
|
✗ |
rzId, wheels, constraints); |
67 |
|
✗ |
DubinsPathPtr_t shPtr(ptr); |
68 |
|
✗ |
ptr->init(shPtr); |
69 |
|
✗ |
return shPtr; |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
inline value_type meanBounds(const JointPtr_t& j, const size_type& i) { |
73 |
|
|
return (j->upperBound(i) + j->lowerBound(i)) / 2; |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
inline value_type saturate(const value_type& v, const JointPtr_t& j, |
77 |
|
|
const size_type& i) { |
78 |
|
|
return std::min(j->upperBound(i), std::max(j->lowerBound(i), v)); |
79 |
|
|
} |
80 |
|
|
|
81 |
|
✗ |
DubinsPath::DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init, |
82 |
|
|
ConfigurationIn_t end, value_type extraLength, |
83 |
|
|
value_type rho, size_type xyId, size_type rzId, |
84 |
|
✗ |
const std::vector<JointPtr_t> wheels) |
85 |
|
✗ |
: parent_t(robot->configSize(), robot->numberDof()), |
86 |
|
✗ |
device_(robot), |
87 |
|
✗ |
initial_(init), |
88 |
|
✗ |
end_(end), |
89 |
|
✗ |
xyId_(xyId), |
90 |
|
✗ |
rzId_(rzId), |
91 |
|
✗ |
wheels_(wheels), |
92 |
|
✗ |
typeId_(0), |
93 |
|
✗ |
extraLength_(extraLength), |
94 |
|
✗ |
rho_(rho) { |
95 |
|
✗ |
assert(robot); |
96 |
|
✗ |
assert(rho_ > 0); |
97 |
|
✗ |
vector3_t q0, q1; |
98 |
|
✗ |
q0[0] = initial_[xyId_ + 0]; |
99 |
|
✗ |
q1[0] = end_[xyId_ + 0]; |
100 |
|
✗ |
q0[1] = initial_[xyId_ + 1]; |
101 |
|
✗ |
q1[1] = end_[xyId_ + 1]; |
102 |
|
✗ |
q0[2] = atan2(initial_[rzId_ + 1], initial_[rzId_ + 0]); |
103 |
|
✗ |
q1[2] = atan2(end_[rzId_ + 1], end_[rzId_ + 0]); |
104 |
|
✗ |
dubins_init(q0, q1); |
105 |
|
|
} |
106 |
|
|
|
107 |
|
✗ |
DubinsPath::DubinsPath(const DevicePtr_t& robot, ConfigurationIn_t init, |
108 |
|
|
ConfigurationIn_t end, value_type extraLength, |
109 |
|
|
value_type rho, size_type xyId, size_type rzId, |
110 |
|
|
const std::vector<JointPtr_t> wheels, |
111 |
|
✗ |
ConstraintSetPtr_t constraints) |
112 |
|
✗ |
: parent_t(robot->configSize(), robot->numberDof()), |
113 |
|
✗ |
device_(robot), |
114 |
|
✗ |
initial_(init), |
115 |
|
✗ |
end_(end), |
116 |
|
✗ |
xyId_(xyId), |
117 |
|
✗ |
rzId_(rzId), |
118 |
|
✗ |
wheels_(wheels), |
119 |
|
✗ |
typeId_(0), |
120 |
|
✗ |
extraLength_(extraLength), |
121 |
|
✗ |
rho_(rho) { |
122 |
|
✗ |
this->constraints(constraints); |
123 |
|
✗ |
assert(robot); |
124 |
|
✗ |
assert(rho_ > 0); |
125 |
|
✗ |
vector3_t q0, q1; |
126 |
|
✗ |
q0[0] = initial_[xyId_ + 0]; |
127 |
|
✗ |
q1[0] = end_[xyId_ + 0]; |
128 |
|
✗ |
q0[1] = initial_[xyId_ + 1]; |
129 |
|
✗ |
q1[1] = end_[xyId_ + 1]; |
130 |
|
✗ |
q0[2] = atan2(initial_[rzId_ + 1], initial_[rzId_ + 0]); |
131 |
|
✗ |
q1[2] = atan2(end_[rzId_ + 1], end_[rzId_ + 0]); |
132 |
|
✗ |
dubins_init(q0, q1); |
133 |
|
|
} |
134 |
|
|
|
135 |
|
✗ |
DubinsPath::DubinsPath(const DubinsPath& path) |
136 |
|
|
: parent_t(path), |
137 |
|
✗ |
device_(path.device_), |
138 |
|
✗ |
initial_(path.initial_), |
139 |
|
✗ |
end_(path.end_), |
140 |
|
✗ |
xyId_(path.xyId_), |
141 |
|
✗ |
rzId_(path.rzId_), |
142 |
|
✗ |
dxyId_(path.dxyId_), |
143 |
|
✗ |
drzId_(path.drzId_), |
144 |
|
✗ |
wheels_(path.wheels_), |
145 |
|
✗ |
typeId_(path.typeId_), |
146 |
|
✗ |
lengths_(path.lengths_), |
147 |
|
✗ |
extraLength_(path.extraLength_), |
148 |
|
✗ |
rho_(path.rho_), |
149 |
|
✗ |
qi_(path.qi_) {} |
150 |
|
|
|
151 |
|
✗ |
DubinsPath::DubinsPath(const DubinsPath& path, |
152 |
|
✗ |
const ConstraintSetPtr_t& constraints) |
153 |
|
|
: parent_t(path, constraints), |
154 |
|
✗ |
device_(path.device_), |
155 |
|
✗ |
initial_(path.initial_), |
156 |
|
✗ |
end_(path.end_), |
157 |
|
✗ |
xyId_(path.xyId_), |
158 |
|
✗ |
rzId_(path.rzId_), |
159 |
|
✗ |
wheels_(path.wheels_), |
160 |
|
✗ |
typeId_(path.typeId_), |
161 |
|
✗ |
lengths_(path.lengths_), |
162 |
|
✗ |
extraLength_(path.extraLength_), |
163 |
|
✗ |
rho_(path.rho_), |
164 |
|
✗ |
qi_(path.qi_) {} |
165 |
|
|
|
166 |
|
✗ |
void DubinsPath::init(DubinsPathPtr_t self) { |
167 |
|
✗ |
parent_t::init(self); |
168 |
|
✗ |
weak_ = self; |
169 |
|
|
} |
170 |
|
|
|
171 |
|
✗ |
void DubinsPath::dubins_init_normalised(double alpha, double beta, double d) { |
172 |
|
✗ |
double best_cost = INFINITY; |
173 |
|
|
int best_word; |
174 |
|
|
int i; |
175 |
|
|
|
176 |
|
✗ |
best_word = -1; |
177 |
|
✗ |
for (i = 0; i < 6; i++) { |
178 |
|
|
double params[3]; |
179 |
|
✗ |
int err = dubins_words[i](alpha, beta, d, params); |
180 |
|
✗ |
if (err == EDUBOK) { |
181 |
|
✗ |
double cost = params[0] + params[1] + params[2]; |
182 |
|
✗ |
if (cost < best_cost) { |
183 |
|
✗ |
best_word = i; |
184 |
|
✗ |
best_cost = cost; |
185 |
|
✗ |
lengths_[0] = params[0]; |
186 |
|
✗ |
lengths_[1] = params[1]; |
187 |
|
✗ |
lengths_[2] = params[2]; |
188 |
|
✗ |
typeId_ = i; |
189 |
|
|
} |
190 |
|
|
} |
191 |
|
|
} |
192 |
|
|
|
193 |
|
✗ |
if (best_word == -1) { |
194 |
|
|
hppDout(error, "Failed to build Dubins path between " |
195 |
|
|
<< initial_.transpose() << " and " << end_.transpose() |
196 |
|
|
<< "."); |
197 |
|
✗ |
throw std::logic_error("Failed to build Dubins path"); |
198 |
|
|
} |
199 |
|
✗ |
typeId_ = best_word; |
200 |
|
|
} |
201 |
|
|
|
202 |
|
✗ |
double fmodr(double x, double y) { return x - y * floor(x / y); } |
203 |
|
|
|
204 |
|
✗ |
double mod2pi(double theta) { return fmodr(theta, 2 * M_PI); } |
205 |
|
|
|
206 |
|
✗ |
void DubinsPath::dubins_init(vector3_t q0, vector3_t q1) { |
207 |
|
|
int i; |
208 |
|
✗ |
double dx = q1[0] - q0[0]; |
209 |
|
✗ |
double dy = q1[1] - q0[1]; |
210 |
|
✗ |
double D = sqrt(dx * dx + dy * dy); |
211 |
|
✗ |
double d = D / rho_; |
212 |
|
✗ |
double theta = mod2pi(atan2(dy, dx)); |
213 |
|
✗ |
double alpha = mod2pi(q0[2] - theta); |
214 |
|
✗ |
double beta = mod2pi(q1[2] - theta); |
215 |
|
✗ |
for (i = 0; i < 3; i++) { |
216 |
|
✗ |
qi_[i] = q0[i]; |
217 |
|
|
} |
218 |
|
✗ |
dubins_init_normalised(alpha, beta, d); |
219 |
|
|
// Find rank of translation and rotation in velocity vectors |
220 |
|
|
// Hypothesis: degrees of freedom all belong to a planar joint or |
221 |
|
|
// xyId_ belong to a tranlation joint, rzId_ belongs to a SO2 joint. |
222 |
|
✗ |
JointPtr_t joint(device_->getJointAtConfigRank(xyId_)); |
223 |
|
✗ |
size_type offset(xyId_ - joint->rankInConfiguration()); |
224 |
|
✗ |
dxyId_ = joint->rankInVelocity() + offset; |
225 |
|
✗ |
joint = device_->getJointAtConfigRank(rzId_); |
226 |
|
✗ |
offset = rzId_ - joint->rankInConfiguration(); |
227 |
|
✗ |
drzId_ = joint->rankInVelocity() + offset; |
228 |
|
|
// Create constant curvature segments |
229 |
|
✗ |
std::vector<Configuration_t> q(4); |
230 |
|
✗ |
q[0] = initial_; |
231 |
|
✗ |
q[1].resize(device_->configSize()); |
232 |
|
✗ |
q[2].resize(device_->configSize()); |
233 |
|
✗ |
q[3] = end_; |
234 |
|
✗ |
std::vector<value_type> extraL(3); |
235 |
|
✗ |
value_type L(lengths_[0] + lengths_[1] + lengths_[2]); |
236 |
|
✗ |
for (std::size_t i = 0; i < 3; ++i) { |
237 |
|
✗ |
extraL[i] = lengths_[i] / L * extraLength_; |
238 |
|
|
} |
239 |
|
✗ |
value_type l(lengths_[0]); |
240 |
|
✗ |
pinocchio::interpolate(device_, initial_.head(device_->configSize()), |
241 |
|
✗ |
end_.head(device_->configSize()), l / L, q[1]); |
242 |
|
✗ |
l += lengths_[1]; |
243 |
|
✗ |
pinocchio::interpolate(device_, initial_.head(device_->configSize()), |
244 |
|
✗ |
end_.head(device_->configSize()), l / L, q[2]); |
245 |
|
✗ |
const int* types = DIRDATA[typeId_]; |
246 |
|
✗ |
for (std::size_t i = 0; i < 3; ++i) { |
247 |
|
✗ |
value_type curvature = 0; |
248 |
|
✗ |
switch (types[i]) { |
249 |
|
✗ |
case L_SEG: |
250 |
|
✗ |
curvature = 1. / rho_; |
251 |
|
✗ |
break; |
252 |
|
✗ |
case S_SEG: |
253 |
|
✗ |
curvature = 0; |
254 |
|
✗ |
break; |
255 |
|
✗ |
case R_SEG: |
256 |
|
✗ |
curvature = -1. / rho_; |
257 |
|
✗ |
break; |
258 |
|
✗ |
default: |
259 |
|
✗ |
assert(false && "Invalid Dubins segment type."); |
260 |
|
|
} |
261 |
|
|
ConstantCurvaturePtr_t path(ConstantCurvature::create( |
262 |
|
✗ |
device_, q[i], q[i + 1], rho_ * lengths_[i], |
263 |
|
✗ |
rho_ * lengths_[i] + extraL[i], curvature, xyId_, rzId_, |
264 |
|
✗ |
device_->getJointAtConfigRank(rzId_), wheels_, ConstraintSetPtr_t())); |
265 |
|
✗ |
appendPath(path); |
266 |
|
✗ |
q[i + 1] = path->end(); |
267 |
|
|
} |
268 |
|
|
} |
269 |
|
|
|
270 |
|
✗ |
void dubins_segment(double t, vector3_t qi, vector3_t& qt, int type) { |
271 |
|
✗ |
assert(type == L_SEG || type == S_SEG || type == R_SEG); |
272 |
|
|
|
273 |
|
✗ |
if (type == L_SEG) { |
274 |
|
✗ |
qt[0] = qi[0] + sin(qi[2] + t) - sin(qi[2]); |
275 |
|
✗ |
qt[1] = qi[1] - cos(qi[2] + t) + cos(qi[2]); |
276 |
|
✗ |
qt[2] = qi[2] + t; |
277 |
|
✗ |
} else if (type == R_SEG) { |
278 |
|
✗ |
qt[0] = qi[0] - sin(qi[2] - t) + sin(qi[2]); |
279 |
|
✗ |
qt[1] = qi[1] + cos(qi[2] - t) - cos(qi[2]); |
280 |
|
✗ |
qt[2] = qi[2] - t; |
281 |
|
✗ |
} else if (type == S_SEG) { |
282 |
|
✗ |
qt[0] = qi[0] + cos(qi[2]) * t; |
283 |
|
✗ |
qt[1] = qi[1] + sin(qi[2]) * t; |
284 |
|
✗ |
qt[2] = qi[2]; |
285 |
|
|
} |
286 |
|
|
} |
287 |
|
|
|
288 |
|
✗ |
void dubins_segment_velocity(double t, vector3_t qi, vector3_t& v, int type) { |
289 |
|
✗ |
assert(type == L_SEG || type == S_SEG || type == R_SEG); |
290 |
|
|
|
291 |
|
✗ |
if (type == L_SEG) { |
292 |
|
✗ |
v[0] = cos(qi[2] + t); |
293 |
|
✗ |
v[1] = -sin(qi[2] + t); |
294 |
|
✗ |
v[2] = 1; |
295 |
|
✗ |
} else if (type == R_SEG) { |
296 |
|
✗ |
v[0] = cos(t - qi[2]); |
297 |
|
✗ |
v[1] = sin(qi[2] - t); |
298 |
|
✗ |
v[2] = -1; |
299 |
|
✗ |
} else if (type == S_SEG) { |
300 |
|
✗ |
v[0] = cos(qi[2]); |
301 |
|
✗ |
v[1] = sin(qi[2]); |
302 |
|
✗ |
v[2] = 0; |
303 |
|
|
} |
304 |
|
|
} |
305 |
|
|
|
306 |
|
|
template <class Archive> |
307 |
|
✗ |
void DubinsPath::serialize(Archive& ar, const unsigned int version) { |
308 |
|
|
using namespace boost::serialization; |
309 |
|
|
(void)version; |
310 |
|
✗ |
ar& make_nvp("base", base_object<PathVector>(*this)); |
311 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(device_); |
312 |
|
✗ |
serialization::remove_duplicate::serialize_vector(ar, "initial", initial_, |
313 |
|
|
version); |
314 |
|
✗ |
serialization::remove_duplicate::serialize_vector(ar, "end", end_, version); |
315 |
|
✗ |
ar& make_nvp("xyId_", const_cast<size_type&>(xyId_)); |
316 |
|
✗ |
ar& make_nvp("rzId_", const_cast<size_type&>(rzId_)); |
317 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(dxyId_); |
318 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(drzId_); |
319 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(wheels_); |
320 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(typeId_); |
321 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(lengths_); |
322 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(extraLength_); |
323 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(rho_); |
324 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(qi_); |
325 |
|
✗ |
ar& BOOST_SERIALIZATION_NVP(weak_); |
326 |
|
|
} |
327 |
|
|
|
328 |
|
|
HPP_SERIALIZATION_IMPLEMENT(DubinsPath); |
329 |
|
|
} // namespace core |
330 |
|
|
} // namespace hpp |
331 |
|
|
|
332 |
|
|
BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::DubinsPath) |
333 |
|
|
|