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