Trajectory Optimization crashes for simple trajectory

[mirkow]

Hi,

when testing some simple trajectory, the algorithm causes an abort in Eigen.
The trajectory is 1d: 0, 1, 0.7, 1.6, 0

It fails with:

TimeOptimalTrajectoryTest: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:378: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator[](Eigen::DenseCoeffsBase<Derived, 1>::Index) [with Derived = Eigen::Matrix<double, -1, 1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::DenseCoeffsBase<Derived, 1>::Index = long int]: Assertion `index >= 0 && index < size()' failed.

and backtrace:

Eigen::DenseCoeffsBase<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 1>::operator[]:378
TimeOptimalTrajectory::getVelocityMaxPathVelocityDeriv:438
TimeOptimalTrajectory::integrateForward:270
TimeOptimalTrajectory::TimeOptimalTrajectory

The full code example to test this is:

std::list<Eigen::VectorXd> waypoints;
    Eigen::VectorXd waypoint(1);
    waypoint << 0;
    waypoints.push_back(waypoint);
    waypoint << 1;
    waypoints.push_back(waypoint);
    waypoint << 0.7;
    waypoints.push_back(waypoint);
    waypoint << 1.6;
    waypoints.push_back(waypoint);
//    waypoint << 1;
//    waypoints.push_back(waypoint);
    waypoint << 0;
    waypoints.push_back(waypoint);
    Eigen::VectorXd maxAcceleration(1);
    maxAcceleration << 0.4;
    Eigen::VectorXd maxVelocity(1);
    maxVelocity << 0.3;

    double maxDeviation = 0.01;

    time_t  timev;
    time(&timev);

    std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();

    VirtualRobot::TimeOptimalTrajectory trajectory(VirtualRobot::Path(waypoints, maxDeviation), maxVelocity, maxAcceleration, 0.1);

    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
    int dtime = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();


    trajectory.outputPhasePlaneTrajectory();
    if(trajectory.isValid()) {
        double duration = trajectory.getDuration();
        std::cout << "Trajectory duration: " << duration << " s" << std::endl << std::endl;
        std::cout << "Time      Position                  Velocity" << std::endl;
        for(double t = 0.0; t < duration; t += 0.1) {
            printf("%6.4f   %7.4f  %7.4f \n", t, trajectory.getPosition(t)[0],
                trajectory.getVelocity(t)[0]);
        }
        printf("%6.4f   %7.4f   %7.4f\n", duration, trajectory.getPosition(duration)[0],
            trajectory.getVelocity(duration)[0]);
    }
    else {
        std::cout << "Trajectory generation (exampleFromSimulationEasy) failed." << std::endl;
    }

    std::cout << "Trajectory generation (Simulation example easy) took: " << dtime << " microseconds" << std::endl;

Any idea why this happens?

[Dale-Koenig]

This occurs because when three consecutive waypoints cause a 180 degree turn, construction of CircularPathSegment has a call to acos(start_vector.dot(end_vector)) where start_vector.dot(end_vector) = -1. Due to floating point errors, taking acos can result in nan which causes a crash.