示例#1
0
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;
    }
}
示例#3
0
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;
    }
}
示例#5
0
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);
}
示例#6
0
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;
}
示例#10
0
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);
        }
    }
}