Failures on dense trajectory that oddly succeed when made more dense

[pbeeson]

Hung,

First off, please don't take the repeated issues I'm filing in a negative way. I value this repository very much as I have found it very useful, I dumbly didn't realize that TOPP was an active research issue, and I was trying to solve myself for a week without progress until I stumbled upon Kunz and Stilman then TOPP-RA.

However, as I mentioned previously, your implementation depends heavily on getting that initial "Alignment" right when forming the cublic spline from the trajectory (Y) values to the 0..1 spaced (X) values. Otherwise the cubic spline will "oscillate" outside of the limits set by the knot points (which might be infeasible given joint limits or obstacles). For a single-DOF system, this is easy because you just figure out the percentage of overall motion and that lets you set the spline X values properly. For multi-DOF this isn't possible because joints move differently across the trajectory, especially for straight line motion trajectories.

So, in order to ensure that the spline doesn't move outside the limits set by the trajectory waypoints for each segment, I was densifying the trajectory before calculating the initial spline. For instance, if the max motion of any joint from waypoint A to B is 30 degrees, then I would add 29 waypoints between A and B (one every degree). This reduces the possibility of the spline moving out of the segment limits, by adding more knots.

I've found real world examples where running without densifying finds a solution, though again that the solution is outside the limits. Then running with densifying, causes toppra to fail to return an answer. But, I've found that densifying even more (e.g., 299 waypoints in the example above) can then cause toppra to find a solution.

I thought you'd be interested in this. I've found that by just increasing the densification factor on failures ( doing this a most N times so that it doesn't run forever), that I do eventually find a solution 100% of the time for a sense trajectory. The question is why splines created from dense, but not "really dense" trajectories fail.

[pbeeson]

#!/usr/bin/python

import toppra 
import numpy
import time
import math
import matplotlib.pyplot as plt

def optimize(trajectory, velocities, accelerations, counter=1):
    global tdata, new_way_pts
    try:
        t0=time.time()

        way_pts = numpy.array(trajectory)
        max_vel = numpy.array(velocities)
        max_acc = numpy.array(accelerations)

        if counter == 0:
            new_way_pts=way_pts
        else:
            #Densify the trajectory with unknown alignment (times unknown and
            #multi-DOF) to try to ensure spline stays within the knot points
            new_way_pts = [list(way_pts[0])]
            for i in range(1, len(way_pts)):
                max_delta = max(2,math.ceil(max(abs(way_pts[i]-way_pts[i-1])))+1)
                new_way_pts.extend(map(list, zip(*[list(numpy.linspace(start,end,max_delta*pow(10,counter-1))) for start,end in zip(way_pts[i-1],way_pts[i])]))[1:])

            print("Original trajectory of size {:d} is now {:d}".format(len(trajectory),len(new_way_pts)))

        # Toppra stuff
        path = toppra.SplineInterpolator(numpy.linspace(0, 1, len(new_way_pts)), new_way_pts)

        vlim = numpy.vstack((-max_vel, max_vel)).T
        alim = numpy.vstack((-max_acc, max_acc)).T

        pc_vel = toppra.constraint.JointVelocityConstraint(vlim)
        pc_acc = toppra.constraint.JointAccelerationConstraint(alim, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation)

        # Setup a parametrization instance
        instance = toppra.algorithm.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel', gridpoints=numpy.linspace(0, 1.0, 1001))
        jnt_traj, aux_traj, tdata = instance.compute_trajectory(0, 0, bc_type='clamped', return_data=True)

        if jnt_traj is None:
            throw

        print("Trajectory solve took {:f} secs".format(time.time() - t0))
        # Convert to JointTrajectory message    

        frequency = 100

        last_index = 0
        ts = numpy.linspace(0, jnt_traj.duration, jnt_traj.duration*frequency+1)

        pos = jnt_traj.eval(ts)
        vel = jnt_traj.evald(ts)
        acc = jnt_traj.evaldd(ts)

        ## Samples Uniformly at frequency
    except:
        if counter == 0 or counter== 5:
            return None
        print("*Failed at densification factor {:d}".format(counter))
        return optimize(trajectory,velocities,accelerations,counter+1)

    fig, axs = plt.subplots(3, 1, sharex=True)
    axs[0].plot(ts, pos)
    axs[0].plot(tdata['t_waypts'], new_way_pts,'x')
    axs[1].plot(ts, vel)
    axs[2].plot(ts, acc)
    plt.show()

    return (pos, vel, ts)

traj = ([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ],
         [-27.50279282, -29.62305746, -46.55384316, 25.12237873, -22.93857853,
          55.42717779, 64.24461556, -21.50526157, -28.92329844, -11.85143927,
          25.15542639, -50.2142205, -59.86068025, 47.99742286],
         [-27.70480439, -29.12111491, -48.81081132, 26.33850747, -24.08028205,
          58.11998058, 67.31735613, -15.70974189, -27.72159688, -12.45287854,
          26.40460278, -52.61594099, -62.7682355 , 50.29172071],
         [-28.6071851, -19.9689193, -63.67399055, 34.33305974, -31.82975642,
          75.89734467, 87.22634902, 67.35331492, -8.77277506, -16.62302836,
          34.86182224, -68.17752794, -81.95838399, 65.14626572]],
        [42.97183463, 42.97183463, 42.97183463, 42.97183463, 114.59155903,
         114.59155903, 114.59155903, 42.97183463, 42.97183463, 42.97183463,
        42.97183463, 114.59155903, 114.59155903, 114.59155903],
        [85.94366927, 85.94366927, 85.94366927, 85.94366927, 229.18311805,
         229.18311805, 229.18311805, 85.94366927, 85.94366927, 85.94366927,
         85.94366927, 229.18311805, 229.18311805, 229.18311805])

#Non-dense version
print("**TOPP-RA on original trajectory")
data = optimize(traj[0],traj[1],traj[2],0)

if data is None:
    print "Failed"
else:
    print "Time: ",data[2][-1]

#Dense version
print("\n**TOPP-RA on dense trajectory")
data = optimize(traj[0],traj[1],traj[2])

if data is None:
    print "Failed"
else:
    print "Time: ",data[2][-1]

[pbeeson]

Above is a python program that you can run that illustrates this with real data where Topp-RA failed on the initial densification of the trajectory. The values are in degrees, not radians, so that densification (and then sparsifcation of the solution) can be easily defined in terms of degree intervals. This is 2 7-DOF arms making a 14-DOF trajectory (actually a simulated Baxter that I was using for testing TOPP-RA). This is a coordinated trajectory to move between each waypoint in a synchronized fashion without colliding the arms, thus the arms need to be in the same trajectory. MY planner works very hard to get these collision free trajectories for coordinated multi-arm tasks, so I need the splines to be within the trajectory waypoints, thus the need for densification.

Output from above:

**TOPP-RA on original trajectory
Trajectory solve took 0.066681 secs
Time:  4.725033196901585

**TOPP-RA on dense trajectory
Original trajectory of size 4 is now 156
*Failed at densification factor 1
Original trajectory of size 4 is now 1578
Trajectory solve took 0.072888 secs
Time:  4.229806425198373

[pbeeson]

Additionally, the final durations change dramatically with different levels of densification. You can call the above program with counter 4 and 5 see the differences in duration (from 4.3 to 3.6 over different values of counter = 2 to 5). I can understand this will change a little bit due to grid discretation, but wanted to point it out any way in case it was interesting to you.

[hungpham2511]

Hi Patrick,

I really appreciate and welcome such detailed reports on TOPP-RA performance. Addressing these and make TOPP-RA more robust and useful to others are also my goals. So again, thank you for the reports.

I will take a look at the issue and see if there is anything we can do about it.

[hungpham2511]

compute_trajectory with seidel solver works fine now with the lastest commit on master, I tested your script with counter equals 1 to 5 and all work well. Guess what is left is the interpolation of waypoints.

Can I know in the geometric planner what interpolation method is used when the trajectory is checked for collision? That scheme should be used with toppra in order to ensure consistency. Densification does not look like a good option, since it create very high-curvature segments at the original waypoints.

[pbeeson]

I find that it does not make high curvature segments if you clamp the endpoints instead of using not-a-knot.

The interpolation used in planning is just linear as there is no way to use curves without knowing the times and velocities. So multi-limb, synchronized, collision free motion is computed by knowing that synchronized waypoints are not in collision and that the linear interpolation between these are not in collision. On Thu, Oct 18, 2018 at 12:20 AM Hung Pham (Phạm Tiến Hùng) < notifications@github.com> wrote:

compute_trajectory with seidel solver works fine now with the lastest commit on master, I tested your script with counter equals 1 to 5 and all work well. Guess what is left is the interpolation of waypoints.

Can I know in the geometric planner what interpolation method is used when the trajectory is checked for collision? That scheme should be used with toppra in order to ensure consistency. Densification does not look like a good option, since it create very high-curvature segments at the original waypoints.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/hungpham2511/toppra/issues/13#issuecomment-430878538, or mute the thread https://github.com/notifications/unsubscribe-auth/ADrfxjglzMYnL81nBx28DaURrhd269yQks5umA-tgaJpZM4XkttI .

[pbeeson]

I see what you are saying, yes because by densifying, I’m essentially approximating a linear fit between the original waypoints, it does mean that there can be high curvature, but I haven’t noticed any issues. I improved upon the original iterative algorithm from my example and it generally solves the first time now (densified to 0.5 degrees) and only rarely takes 2 iterations (0.125 degrees). My guess is that your fix for Seidel may improve this even further. On Thu, Oct 18, 2018 at 6:16 AM Patrick Beeson beeson.p@gmail.com wrote:

I find that it does not make high curvature segments if you clamp the endpoints instead of using not-a-knot.

The interpolation used in planning is just linear as there is no way to use curves without knowing the times and velocities. So multi-limb, synchronized, collision free motion is computed by knowing that synchronized waypoints are not in collision and that the linear interpolation between these are not in collision. On Thu, Oct 18, 2018 at 12:20 AM Hung Pham (Phạm Tiến Hùng) < notifications@github.com> wrote:

compute_trajectory with seidel solver works fine now with the lastest commit on master, I tested your script with counter equals 1 to 5 and all work well. Guess what is left is the interpolation of waypoints.

Can I know in the geometric planner what interpolation method is used when the trajectory is checked for collision? That scheme should be used with toppra in order to ensure consistency. Densification does not look like a good option, since it create very high-curvature segments at the original waypoints.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/hungpham2511/toppra/issues/13#issuecomment-430878538, or mute the thread https://github.com/notifications/unsubscribe-auth/ADrfxjglzMYnL81nBx28DaURrhd269yQks5umA-tgaJpZM4XkttI .

[hungpham2511]

That sounds great. Also, please note that you need to recompile the Cython source file to have the new Seidel solver working. (by doing python setup.py develop again).

I will close this issue. Feel free to re-open, or create a new one if you find any other problems.