Use Trajectory class instead of tuples

[sea-bass]

Right now, trajectories are represented as tuples of (t_pts, x_pts, y_pts, yaw_pts) which could be made nicer.

I propose doing something similar to what is in the pyrobosim.utils.motion.Path class, which contains a list of poses, but expanding this to include times as well.

[ibrahiminfinite]

This one is a bit trickier than it seems.

There are 2 ways to go about this
If we store as poses, there will have to be 2 additional conversions on each call of the interpolation function.

1. Pose to numpy arrays
2. Numpy arrays to poses

The other way involves re-writing the interpolation function to do the interpolation manually while iterating over poses instead of using numpy. This will have more performance, but not sure if it will be significant enough.

Option 1 is easier and we can use the trusty numpy methods, but at the added cost of the 2 conversions.

[sea-bass]

I think both conversions will have to be necessary, but I also think that's OK because these are only happening once at motion planning time.

Right now, the first conversion is happening anyways in the get_constant_speed_trajectory() function:

    # Package up the trajectory
    x_pts = np.array([p.x for p in path.poses])
    y_pts = np.array([p.y for p in path.poses])
    yaw_pts = np.array([p.get_yaw() for p in path.poses])
    traj = (t_pts, x_pts, y_pts, yaw_pts)
    return traj

So this means that during the interpolation, we'd need to use the same list comprehension stuff (without single letter variables ;)) to grab the poses from the trajectory, do the interpolation, and repackage up the poses.

Something like:

t_pts = np.array([point.time for point in traj.points])
x_pts = np.array([point.pose.x for point in traj.points])

t_interp = np.arange(0, t_final, dt)
x_interp = np.interp(t_interp, t_pts, x_pts)
# ... same for y and yaw

# Probably a more efficient way to do this w/ list comprehensions ...
traj_points = []
for (t_pt, x_pt, y_pt, yaw_pt) in zip(t_interp, x_interp, y_interp, yaw_interp):
    traj_points.append(
        TrajectoryPoint(pose=Pose(x=x_pt, y=y_pt, yaw=yaw_pt), time=t_pt)
    )

final_traj = Trajectory(traj_points)

... where Trajectory and TrajectoryPoint would be our hypothetical new classes, which I'm shamelessly borrowing from how ROS messages do it :)

[ibrahiminfinite]

I actually implemented this strategy, but it felt very inefficient to convert numpy to Pose and back multiple time.
But looks like we don't have much choice here.

[sea-bass]

I actually implemented this strategy, but it felt very inefficient to convert numpy to Pose and back multiple time. But looks like we don't have much choice here.

You were right -- converting back and forth would have been inefficient, so I made the class a little more lightweight than this by internally storing the necessary np.arrays