vector<CubicBezierControlPoints> RRTPlanner::generateCubicBezierPath(
const vector<Geometry2d::Point>& points,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, const boost::optional<vector<float>>& times) {
size_t length = points.size();
size_t curvesNum = length - 1;
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = points[i].x;
pointsY[i] = points[i].y;
}
const float startSpeed = vi.mag();
const float endSpeed = vf.mag();
if (times) {
assert(times->size() == points.size());
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (times->at(i + 1) - times->at(i));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
} else {
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 / (getTime(points, i + 1, motionConstraints, startSpeed,
endSpeed) -
getTime(points, i, motionConstraints, startSpeed,
endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
debugThrow(
"Something went wrong. Points are too close to each other "
"probably");
return vector<CubicBezierControlPoints>();
}
}
}
VectorXd solutionX =
RRTPlanner::cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY =
RRTPlanner::cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
vector<CubicBezierControlPoints> path;
for (int i = 0; i < curvesNum; i++) {
Point p0 = points[i];
Point p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
Point p2 =
Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
Point p3 = points[i + 1];
path.emplace_back(p0, p1, p2, p3);
}
return path;
}
// TODO: Use targeted end velocity
InterpolatedPath* RRTPlanner::cubicBezier(
InterpolatedPath& path, const Geometry2d::ShapeSet* obstacles,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf) {
int length = path.waypoints.size();
int curvesNum = length - 1;
if (curvesNum <= 0) {
delete &path;
return nullptr;
}
// TODO: Get the actual values
vector<double> pointsX(length);
vector<double> pointsY(length);
vector<double> ks(length - 1);
vector<double> ks2(length - 1);
for (int i = 0; i < length; i++) {
pointsX[i] = path.waypoints[i].pos().x;
pointsY[i] = path.waypoints[i].pos().y;
}
float startSpeed = vi.mag();
// This is pretty hacky;
Geometry2d::Point startDirection =
(path.waypoints[1].pos() - path.waypoints[0].pos()).normalized();
if (startSpeed < 0.3) {
startSpeed = 0.3;
vi = startDirection * startSpeed;
} else {
vi = vi.mag() * (startDirection + vi.normalized()) / 2.0 * 0.8;
}
const float endSpeed = vf.mag();
for (int i = 0; i < curvesNum; i++) {
ks[i] = 1.0 /
(getTime(path, i + 1, motionConstraints, startSpeed, endSpeed) -
getTime(path, i, motionConstraints, startSpeed, endSpeed));
ks2[i] = ks[i] * ks[i];
if (std::isnan(ks[i])) {
delete &path;
return nullptr;
}
}
VectorXd solutionX = cubicBezierCalc(vi.x, vf.x, pointsX, ks, ks2);
VectorXd solutionY = cubicBezierCalc(vi.y, vf.y, pointsY, ks, ks2);
Geometry2d::Point p0, p1, p2, p3;
vector<InterpolatedPath::Entry> pts;
const int interpolations = 10;
double time = 0;
for (int i = 0; i < curvesNum; i++) {
p0 = path.waypoints[i].pos();
p3 = path.waypoints[i + 1].pos();
p1 = Geometry2d::Point(solutionX(i * 2), solutionY(i * 2));
p2 = Geometry2d::Point(solutionX(i * 2 + 1), solutionY(i * 2 + 1));
for (int j = 0; j < interpolations; j++) {
double k = ks[i];
float t = (((float)j / (float)(interpolations)));
Geometry2d::Point pos =
pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 +
3 * (1.0 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
t = ((float)j / (float)(interpolations)) / k;
// 3 k (-(A (-1 + k t)^2) + k t (2 C - 3 C k t + D k t) + B (1 - 4 k
// t + 3 k^2 t^2))
Geometry2d::Point vel =
3 * k * (-(p0 * pow(-1 + k * t, 2)) +
k * t * (2 * p2 - 3 * p2 * k * t + p3 * k * t) +
p1 * (1 - 4 * k * t + 3 * pow(k, 2) * pow(t, 2)));
pts.emplace_back(MotionInstant(pos, vel), time + t);
}
time += 1.0 / ks[i];
}
pts.emplace_back(MotionInstant(path.waypoints[length - 1].pos(), vf), time);
path.waypoints = pts;
return &path;
}
