Another corner case

Hung,

I ran into a case today where The following two waypoints for a 6-DOF arm were sent to toppra, and it failed because the controllable sets saw a nan in K.

Reproduce by updating scalar_example.py:

    waypts = [[-9.089468271438139e-07, -0.46400441351211447, -0.5760014655483718, -3.9375206752326924e-07, -1.6999970211081608, 5.429519493702008e-06], [0.0, -0.464, -0.576, 0.0, -1.7, 0.0]]
    path = ta.SplineInterpolator(np.linspace(0, 1, len(waypts)), waypts)
    vlim = np.array([[-3, 3],[-3, 3],[-3, 3],[-3, 3],[-3, 3],[-3, 3]])
    alim = np.array([[-4, 4],[-4, 4],[-4, 4],[-4, 4],[-4, 4],[-4, 4] ])

However, I noticed that if I comment out the np.isnan(K[i]).any() if in both compute_controllable_sets() and compute_parameterization, that the rest of computer_paramterization() seems to work just fine and produce the correct answer.

So might you be able to either ignore nan for K or at least set nans in K to constants.MAXX?

Both of those “solutions” work for me in testing.

[hungpham2511]

Clearly this should not happen. Seem like there is a bug hiding somewhere. I will investigate this issue.

[github-actions[bot]]

Stale issue message