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