Velocity constraint violated and no error thrown

[toancea]

Hey there,

I am working on a simple 1D trajectory generation with ruckig, where the goal is to reach a position sE with zero velocity and acceleration, while satisfying constraints on velocity, acceleration, jerk. One of the constraints is to always have positive velocity (i.e. forward motion). In some cases (i.e. for some values of sE, constraints and initial conditions), the trajectory given by ruckig does not satisfy the constraints, and not error is thrown, or negative exit code is returned.

You can find below a minimal repro:

#include <chrono>
#include <iostream>
#include <vector>

#include "ruckig/ruckig.hpp"

typedef double float64_t;

struct Profiles
{
    std::vector<float64_t> time;
    std::vector<float64_t> jerk;
    std::vector<float64_t> acc;
    std::vector<float64_t> vel;
    std::vector<float64_t> length;
};

using namespace ruckig;

int main()
{
    const float64_t dt = 0.005;
    const float64_t sE = 0.018480837230235015;
    const float64_t vMax = 1.0, aMax = 1.5, jMax = 0.5;
    const float64_t v0 = 0.092317526809040512, a0 = -0.30818708575763137;

    // Create input parameters
    InputParameter<1> input;

    input.current_position = {0.0};
    input.current_velocity = {v0};
    input.current_acceleration = {a0};

    input.target_position = {sE};
    input.target_velocity = {0.0};
    input.target_acceleration = {0.0};

    input.max_velocity = {vMax};
    input.min_velocity = {0.0};

    input.max_acceleration = {aMax};
    input.max_jerk = {jMax};

    // We don't need to pass the control rate (cycle time) when using only offline
    // features
    Ruckig<1, StandardVector, true> otg;
    Trajectory<1> traj;

    // Calculate the trajectory in an offline manner (outside of the control loop)
    Result result;
    try
    {
        result = otg.calculate(input, traj);
    }
    catch (RuckigError err)
    {
        std::cerr << "Thrown error: " << err.what() << std::endl;
        return 1;
    }
    if (result == Result::ErrorInvalidInput)
    {
        std::cerr << "Invalid input!" << std::endl;
        return -1;
    }
    else if (result < 0)
    {
        std::cerr << "Error code:" << result << std::endl;
        return result;
    }

    // Compute total trajectory time.
    float64_t t_total = traj.get_duration();

    // Reserve memory for profiles.
    const size_t nsamples = std::round(t_total / dt);
    Profiles profiles{};
    profiles.time.reserve(nsamples + 1);
    profiles.jerk.reserve(nsamples + 1);
    profiles.acc.reserve(nsamples + 1);
    profiles.vel.reserve(nsamples + 1);
    profiles.length.reserve(nsamples + 1);

    // Sample the Ruckig trajectory uniformly in time.
    float64_t new_length, new_vel, new_acc, new_jerk;
    size_t new_section{}; // what is that?
    for (size_t i = 0U; i < nsamples + 1U; ++i)
    {
        traj.at_time(i * dt, new_length, new_vel, new_acc, new_jerk, new_section);
        profiles.time.push_back(i * dt);
        profiles.jerk.push_back(new_jerk);
        profiles.acc.push_back(new_acc);
        profiles.vel.push_back(new_vel);
        profiles.length.push_back(new_length);
        if (profiles.vel.back() < 0.0)
        {
            std::cerr << "negative velocity: " << profiles.vel.back() << std::endl;
        }
    }
}

which gives the following output:

negative velocity: -9.25724e-05
negative velocity: -0.000339758
negative velocity: -0.000574443
negative velocity: -0.000796629
negative velocity: -0.00100631
negative velocity: -0.0012035
negative velocity: -0.00138818
negative velocity: -0.00156037
negative velocity: -0.00172006
negative velocity: -0.00186724
negative velocity: -0.00200193
negative velocity: -0.00212411
negative velocity: -0.0022338
negative velocity: -0.00233098
negative velocity: -0.00241567
negative velocity: -0.00248785
negative velocity: -0.00254754
negative velocity: -0.00259472
negative velocity: -0.00262941
negative velocity: -0.0026516
negative velocity: -0.00266128
negative velocity: -0.00265847
negative velocity: -0.00264315
negative velocity: -0.00261534
negative velocity: -0.00257502
negative velocity: -0.00252221
negative velocity: -0.00245689
negative velocity: -0.00237908
negative velocity: -0.00228876
negative velocity: -0.00218595
negative velocity: -0.00207064
negative velocity: -0.00194282
negative velocity: -0.00180251
negative velocity: -0.00164969
negative velocity: -0.00148438
negative velocity: -0.00130656
negative velocity: -0.00111625
negative velocity: -0.000913433
negative velocity: -0.000698119
negative velocity: -0.000470304
negative velocity: -0.00022999

At first I thought they might be numerical issues that would be within some tolerances but:

1. In the readme, there were mentions of tolerances with respect to the target state, but nothing about constraints, so I am not sure what tolerances to expect from ruckig.
2. The maximal violation of $\approx 2.6e-3$ seems a bit excessive for it to be simple tolerance.

Is there a ruckig limitation I am not aware of? Or did I mess something up in the problem setup?

Thank you in advance for your precious help.

Bets,

Tudor

[pantor]

Hi Tudor,

it seems to me that your initial state will always result in a negative velocity - even when the system is accelerating with full maximum jerk. You can see that the acceleration increases as much as it is allowed to, but it still reaches negative velocity.

This is the expected behavior of Ruckig. It will use the maximum allowed limits to brake without creating new limit violations where possible.

As a side note, I'm not sure if a minimum velocity of zero is fully supported. Let me know if you see any other issues.

[toancea]

Thank you for your prompt answer. I will add some post-processing to manually deal with such cases.