TEST(InterpolatedPath, evaluate) {
Point p0(1, 1), p1(1, 2), p2(2, 2);
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(p0, Point()), 0s);
path.waypoints.emplace_back(MotionInstant(p1, p1), 3s);
path.waypoints.emplace_back(MotionInstant(p2, Point()), 6s);
// path should be invalid and at end state when t > duration
auto out = path.evaluate(1000s);
ASSERT_FALSE(out);
}
std::unique_ptr<Path> RRTPlanner::run(
MotionInstant start, const MotionCommand* cmd,
const MotionConstraints& motionConstraints,
const Geometry2d::ShapeSet* obstacles, std::unique_ptr<Path> prevPath) {
// This planner only works with commands of type 'PathTarget'
assert(cmd->getCommandType() == Planning::MotionCommand::PathTarget);
Planning::PathTargetCommand target =
*static_cast<const Planning::PathTargetCommand*>(cmd);
MotionInstant goal = target.pathGoal;
// Simple case: no path
if (start.pos == goal.pos) {
InterpolatedPath* path = new InterpolatedPath();
path->setStartTime(RJ::timestamp());
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
return unique_ptr<Path>(path);
}
// Locate a goal point that is obstacle-free
boost::optional<Geometry2d::Point> prevGoal;
if (prevPath) prevGoal = prevPath->end().motion.pos;
goal.pos = EscapeObstaclesPathPlanner::findNonBlockedGoal(
goal.pos, prevGoal, *obstacles);
// Replan if needed, otherwise return the previous path unmodified
if (shouldReplan(start, goal, motionConstraints, obstacles,
prevPath.get())) {
// Run bi-directional RRT to generate a path.
auto points = runRRT(start, goal, motionConstraints, obstacles);
// Optimize out uneccesary waypoints
optimize(points, obstacles, motionConstraints, start.vel, goal.vel);
// Check if Planning or optimization failed
if (points.size() < 2) {
debugLog("PathPlanning Failed");
auto path = make_unique<InterpolatedPath>();
path->waypoints.emplace_back(MotionInstant(start.pos, Point()), 0);
path->waypoints.emplace_back(MotionInstant(start.pos, Point()), 0);
return std::move(path);
}
// Generate and return a cubic bezier path using the waypoints
return generateCubicBezier(points, *obstacles, motionConstraints,
start.vel, goal.vel);
} else {
return prevPath;
}
}
TEST(Path, nearestSegment) {
Point p0, p1(1.0, 0.0), p2(2.0, 0.0), p3(3.0, 0.0);
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(p0, Point()), 0s);
path.waypoints.emplace_back(MotionInstant(p1, Point()), 0s);
path.waypoints.emplace_back(MotionInstant(p2, Point()), 0s);
path.waypoints.emplace_back(MotionInstant(p3, Point()), 0s);
Segment actSeg = path.nearestSegment(Point(0.5, -0.5));
EXPECT_FLOAT_EQ(p0.x(), actSeg.pt[0].x());
EXPECT_FLOAT_EQ(p0.y(), actSeg.pt[0].y());
EXPECT_FLOAT_EQ(p1.x(), actSeg.pt[1].x());
EXPECT_FLOAT_EQ(p1.y(), actSeg.pt[1].y());
}
std::unique_ptr<Path> RRTPlanner::run(
MotionInstant start, const MotionCommand* cmd,
const MotionConstraints& motionConstraints,
const Geometry2d::ShapeSet* obstacles, std::unique_ptr<Path> prevPath) {
// This planner only works with commands of type 'PathTarget'
assert(cmd->getCommandType() == Planning::MotionCommand::PathTarget);
Planning::PathTargetCommand target =
*static_cast<const Planning::PathTargetCommand*>(cmd);
MotionInstant goal = target.pathGoal;
// Simple case: no path
if (start.pos == goal.pos) {
InterpolatedPath* path = new InterpolatedPath();
path->setStartTime(RJ::timestamp());
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
return unique_ptr<Path>(path);
}
// Locate a goal point that is obstacle-free
boost::optional<Geometry2d::Point> prevGoal;
if (prevPath) prevGoal = prevPath->end().pos;
goal.pos = EscapeObstaclesPathPlanner::findNonBlockedGoal(
goal.pos, prevGoal, *obstacles);
// Replan if needed, otherwise return the previous path unmodified
if (shouldReplan(start, goal, motionConstraints, obstacles,
prevPath.get())) {
// Run bi-directional RRT to generate a path.
InterpolatedPath* path =
runRRT(start, goal, motionConstraints, obstacles);
// If RRT failed, the path will be empty, so we need to add a single
// point to make it valid.
if (path && path->waypoints.empty()) {
path->waypoints.emplace_back(
MotionInstant(start.pos, Geometry2d::Point()), 0);
}
return unique_ptr<Path>(path);
} else {
return prevPath;
}
}
TEST(InterpolatedPath, subPath1) {
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(Point(1, 1), Point(0, 0)), 0s);
path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(1, 1)), 3s);
path.waypoints.emplace_back(MotionInstant(Point(1, 3), Point(0, 0)), 6s);
// test invalid parameters to subPath()
EXPECT_THROW(path.subPath(-1s, 5s), invalid_argument);
EXPECT_THROW(path.subPath(8s, 20s),
invalid_argument); // start time beyond bounds of path
EXPECT_THROW(path.subPath(5s, -2s), invalid_argument);
// make a subpath that cuts off one second from the start and end of the
// original
unique_ptr<Path> subPath = path.subPath(1s, 5s);
RJ::Seconds midTime((5 - 1) / 2.0);
auto mid = subPath->evaluate(midTime);
ASSERT_TRUE(mid);
EXPECT_FLOAT_EQ(Point(1, 1).x(), mid->motion.vel.x());
// mid velocity of subpath should be the same as velocity of original path
EXPECT_FLOAT_EQ(Point(1, 1).y(), mid->motion.vel.y());
EXPECT_FLOAT_EQ(1, mid->motion.pos.x());
EXPECT_FLOAT_EQ(2, mid->motion.pos.y());
// test the subpath at t = 0
auto start = subPath->evaluate(0s);
ASSERT_TRUE(start);
// the starting velocity of the subpath should be somewhere between the 0
// and the velocity at the middle
EXPECT_GT(start->motion.vel.mag(), 0);
EXPECT_LT(start->motion.vel.mag(), Point(1, 1).mag());
// the starting position of the subpath should be somewhere between the
// start pos of the original path and the middle point
EXPECT_GT(start->motion.pos.y(), 1);
EXPECT_LT(start->motion.pos.x(), 2);
}
TEST(InterpolatedPath, subpath2) {
// Create a test path
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(Point(1, 0), Point(0, 0)), 0s);
path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(-1, -1)), 1s);
path.waypoints.emplace_back(MotionInstant(Point(-2, 19), Point(1, 1)), 3s);
path.waypoints.emplace_back(MotionInstant(Point(1, 6), Point(0, 0)), 9s);
// Create 6 subPaths of length 1.5
vector<unique_ptr<Path>> subPaths;
RJ::Seconds diff = 1500ms;
for (RJ::Seconds i = 0ms; i < 9s; i += diff) {
subPaths.push_back(path.subPath(i, i + diff));
}
// Compare the subPaths to the origional path and check that the results of
// evaluating the paths are close enough
for (int i = 0; i < 6; i++) {
for (auto j = 0ms; j < 1500ms; j += 1ms) {
auto time = i * 1500ms + j;
auto org = path.evaluate(time);
auto sub = subPaths[i]->evaluate(j);
ASSERT_TRUE(org);
ASSERT_TRUE(sub);
EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001)
<< "i+j=" << to_string(time);
EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001)
<< "i+j=" << to_string(time);
EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001)
<< "i+j=" << to_string(time);
EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001)
<< "i+j=" << to_string(time);
}
}
}
boost::optional<RobotInstant> TrapezoidalPath::evaluate(float time) const {
float distance;
float speedOut;
bool valid = TrapezoidalMotion(_pathLength, // PathLength
_maxSpeed, // maxSpeed
_maxAcc, // maxAcc
time, // time
_startSpeed, // startSpeed
_endSpeed, // endSpeed
distance, // posOut
speedOut); // speedOut
if (!valid) return boost::none;
return RobotInstant(MotionInstant(_pathDirection * distance + _startPos,
_pathDirection * speedOut));
}
/**
* Generates a Cubic Bezier Path based on Albert's random Bezier Velocity Path
* Algorithm
*/
std::vector<InterpolatedPath::Entry> RRTPlanner::generateVelocityPath(
const std::vector<CubicBezierControlPoints>& controlPoints,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, int interpolations) {
// Interpolate Through Bezier Path
vector<Point> newPoints, newPoints1stDerivative, newPoints2ndDerivative;
vector<float> newPointsCurvature, newPointsDistance, newPointsSpeed;
float totalDistance = 0;
const float maxAceleration = motionConstraints.maxAcceleration;
for (const CubicBezierControlPoints& controlPoint : controlPoints) {
Point p0 = controlPoint.p0;
Point p1 = controlPoint.p1;
Point p2 = controlPoint.p2;
Point p3 = controlPoint.p3;
for (int j = 0; j < interpolations; j++) {
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;
// Derivitive 1
// 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 d1 = 3 * pow(1 - t, 2) * (p1 - p0) +
6 * (1 - t) * t * (p2 - p1) +
3 * pow(t, 2) * (p3 - p2);
// Derivitive 2
// https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B.C3.A9zier_curves
// B''(t) = 6(1-t)(P2 - 2*P1 + P0) + 6*t(P3 - 2 * P2 + P1)
Geometry2d::Point d2 =
6 * (1 - t) * (p2 - 2 * p1 + p0) + 6 * t * (p3 - 2 * p2 + p1);
// https://en.wikipedia.org/wiki/Curvature#Local_expressions
// K = |x'*y'' - y'*x''| / (x'^2 + y'^2)^(3/2)
float curvature =
std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// Handle 0 velocity case
if (isnan(curvature)) {
curvature = 0;
}
assert(curvature >= 0);
if (!newPoints.empty()) {
float distance = pos.distTo(newPoints.back());
totalDistance += distance;
}
newPointsDistance.push_back(totalDistance);
newPoints.push_back(pos);
newPoints1stDerivative.push_back(d1);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
// Isolated maxSpeed based on Curvature
// Curvature = 1/Radius of Curvature
// vmax = sqrt(acceleartion/abs(Curvature))
float constantMaxSpeed = std::sqrt(maxAceleration / curvature);
newPointsSpeed.push_back(constantMaxSpeed);
}
}
// Get last point in Path
CubicBezierControlPoints lastControlPoint = controlPoints.back();
Point p0 = lastControlPoint.p0;
Point p1 = lastControlPoint.p1;
Point p2 = lastControlPoint.p2;
Point p3 = lastControlPoint.p3;
Point pos = p3;
Point d1 = vf;
Geometry2d::Point d2 = 6 * (1) * (p3 - 2 * p2 + p1);
float curvature = std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// handle 0 velcoity case
if (isnan(curvature)) {
curvature = 0;
}
totalDistance += pos.distTo(newPoints.back());
newPoints.push_back(pos);
newPoints1stDerivative.push_back(vf);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
newPointsDistance.push_back(totalDistance);
newPointsSpeed[0] = vi.mag();
newPointsSpeed.push_back(vf.mag());
// Velocity Profile Generation
// Forward Smoothing
const float size = newPoints.size();
assert(size == newPoints.size());
assert(size == newPoints1stDerivative.size());
assert(size == newPoints2ndDerivative.size());
assert(size == newPointsDistance.size());
assert(size == newPointsSpeed.size());
assert(size == newPointsCurvature.size());
// Generate Velocity Profile from Interpolation of Bezier Path
// Forward Constraints
for (int i = 1; i < size; i++) {
int i1 = i - 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
// Backwards Constraints
for (int i = size - 2; i >= 0; i--) {
int i1 = i + 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
float totalTime = 0;
vector<InterpolatedPath::Entry> entries;
for (int i = 0; i < size; i++) {
float currentSpeed = newPointsSpeed[i];
float distance = newPointsDistance[i];
if (i != 0) {
distance -= newPointsDistance[i - 1];
float averageSpeed = (currentSpeed + newPointsSpeed[i - 1]) / 2.0;
float deltaT = distance / averageSpeed;
totalTime += deltaT;
}
Point point = newPoints[i];
Point vel = newPoints1stDerivative[i].normalized();
entries.emplace_back(MotionInstant(point, vel * currentSpeed),
totalTime);
}
return entries;
}
// 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;
}
TEST(CompositePath, CompositeSubPath) {
// Create a test path
InterpolatedPath path;
path.waypoints.emplace_back(MotionInstant(Point(1, 0), Point(0, 0)), 0s);
path.waypoints.emplace_back(MotionInstant(Point(1, 2), Point(-1, -1)), 1s);
path.waypoints.emplace_back(MotionInstant(Point(-2, 19), Point(1, 1)), 3s);
path.waypoints.emplace_back(MotionInstant(Point(1, 6), Point(0, 0)), 9s);
// Create 6 subPaths and rejoin them together into one compositePath
CompositePath compositePath;
auto diff = 1500ms;
for (auto i = 0ms; i < 7500ms; i += diff) {
compositePath.append(path.subPath(i, i + diff));
}
compositePath.append(path.subPath(7500ms));
// Compare that the compositePath and origonal path are mostly equal
for (RJ::Seconds i = 0ms; i <= 10s; i += 1ms) {
auto org = path.evaluate(i);
auto sub = compositePath.evaluate(i);
if (!org && !sub) break;
ASSERT_TRUE(org);
ASSERT_TRUE(sub);
EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001)
<< "i=" << to_string(i);
EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001)
<< "i=" << to_string(i);
}
// Create 9 subPaths from the compositePaths
vector<unique_ptr<Path>> subPaths;
diff = 1000ms;
for (auto i = 0ms; i < 8s; i += diff) {
subPaths.push_back(compositePath.subPath(i, i + diff));
}
subPaths.push_back(compositePath.subPath(8000ms));
// Compare the subPaths of the compositePaths to the origional path and
// check that the results of evaluating the paths are close enough
for (int i = 0; i < 9; i++) {
for (auto j = 0ms; j < 1s; j += 100ms) {
auto time = i * 1000ms + j;
auto org = path.evaluate(time);
auto sub = subPaths[i]->evaluate(j);
ASSERT_TRUE(org);
ASSERT_TRUE(sub);
EXPECT_NEAR(org->motion.vel.x(), sub->motion.vel.x(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.vel.y(), sub->motion.vel.y(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.pos.x(), sub->motion.pos.x(), 0.000001)
<< "newPathTime=" << to_string(time);
EXPECT_NEAR(org->motion.pos.y(), sub->motion.pos.y(), 0.00001)
<< "newPathTime=" << to_string(time);
}
}
}
