Add functions: TrajToArray() & ArrayToTraj()

[DavidB-CMU]

1. Add two helper functions TrajToArray() & ArrayToTraj() that convert between an OpenRAVE trajectory and a 2D numpy array.
2. Add unit tests

These are similar to existing prpy.util functions MatrixToTraj() and TrajToMatrix().

Does anyone remember what the purpose of these is?
They seem to use a single dimension matrix, which seems strange to me.

[mkoval]

Would the array only contain positions? Otherwise, I think it needs to have dimensions of #waypoints x #dofs x #derivatives.

I'm not sure exactly what MatrixToTraj or TrajToMatrix do. I think they are helper functions that Anca added AdaptTrajectory in 8244fde. I suspect that something similar happens inside CHOMP - maybe @cdellin can enlighten us?

[DavidB-CMU]

Yes currently TrajToArray() gets the waypoints from an OpenRAVE trajectory and puts the joint angles into an N x 7 array.

[psigen]

Umm.. Won't this only be anything reasonable on a single WAM arm
position-only trajectory?

If so, this seems much more herbpy than prpy. Even then timed trajectories
and head, base, etc will break it, right?
On Jan 12, 2016 3:16 PM, "David Butterworth" [email protected]
wrote:

Yes currently TrajToArray() gets the waypoints from an OpenRAVE trajectory
and puts the joint angles into an N x 7 array.

—
Reply to this email directly or view it on GitHub
#252 (comment)
.

[DavidB-CMU]

ArrayToTraj() is used in a new, proposed planner. If people don't think it's general enough, I'll just leave it in with the planner's code.

def ArrayToTraj(robot, cspec, dof, traj_array):
    """
    Convert an array of joint positions into a timed trajectory.

    @param robot: The robot object.
    @param cspec: OpenRAVE Configuration Specification.
    @param dof:   (int) Num of joints in the array.
    @param traj_array: (numpy.array) Rows for each configuration,
                                     columns for each joint.

    @returns openravepy.Trajectory: A timed trajectory.
    """
    env = robot.GetEnv()
    traj = openravepy.RaveCreateTrajectory(env,'')
    traj.Init(cs)

    num_rows,num_cols = traj_array.shape

    if num_cols != dof:
        raise ValueError('num_cols != dof')

    for i in range(num_rows):

        traj.Insert(i, traj_array[i,:])

    openravepy.planningutils.RetimeActiveDOFTrajectory(
                   traj, robot, False, 0.2, 0.2, "LinearTrajectoryRetimer", "")
    return traj

# Convert array of joint configurations into OpenRAVE trajectory
cspec = robot.GetActiveConfigurationSpecification('linear')
jointspace_traj = ArrayToTraj(robot, cspec, 7, q)

[DavidB-CMU]

Ok yes TrajToArray() may be more accurately called PathToArray() or something.
This function is used inside another function for getting some statistics on paths that are returned by a planner.
Once again, if people don't think this function is generally applicable then I can put it elsewhere.

def PathToArray(path, num_dof):
    """
    Get the joint angles at each waypoint in a path
    and return them in an array.

    @param path: A path, being an un-timed OpenRAVE trajectory.
    @param num_dof: (int) The number of joints.

    @returns positions_array (numpy.array) Rows for each configuration,
                                           columns for each joint.
    """
    num_waypoints = path.GetNumWaypoints()

    # ToDo: if path is timed, strip the timing or throw exception??

    positions_array = numpy.zeros((num_waypoints, num_dof))
    for i in range(num_waypoints):
        # Get first num_dof values, the values that
        # come afterwards are velocities
        q = path.GetWaypoint(i)[range(num_dof)]
        positions_array[i,:] = q

    return positions_array

[mkoval]

I'm not opposed to adding a function that converts a piecewise linear joint trajectory to a NumPy array. All of our planners return a trajectory of this type, so it is a useful utility function to have.

However, you would need to add significantly better error checking to the PathToArray function. At a minimum, it should: (1) throw an exception if the trajectory is not piecewise linear and (2) take a Robot and list of DOFs as input.

[mkoval]

I also just noticed that there is a function JointStatesFromTraj that handles an arbitrary number of derivatives. Perhaps you could improve that function instead of adding another one?

[DavidB-CMU]

prpy.util.JointStatesFromTraj() is for a timed trajectory, and you need to specify the sample times as a parameter.

I am proposing to add a function PathToArray() that operates on un-timed trajectories, so I think we should add it.

[DavidB-CMU]

I don't understand where to add 'robot' and 'list of DOFs' to the PathToArray() function?

Here's some improved input checking:

def PathToArray(path, num_dof):
    """
    Get the joint angles at each waypoint in a path
    and return them in an array.

    @param path: A path, being an un-timed OpenRAVE trajectory.
    @param num_dof: (int) The number of joints.

    @returns positions_array (numpy.array) Rows for each configuration,
                                           columns for each joint.
    """
    # Make sure path does not have timing
    if IsTimedTrajectory(path):
        ValueError('PathToArray() requires an un-timed path') 

    path_cspec = path.GetConfigurationSpecification()

    # Make sure path is linear in joint space
    try:
        # OpenRAVE trajectory type can be 'linear', 'quadratic', or
        # other values including 'cubic', 'quadric' or 'quintic'
        interp_type = path_cspec.GetGroupFromName('joint_values').interpolation
    except openravepy.openrave_exception:
        raise ValueError('Path does not have a joint_values group')
    if interp_type != 'linear':
        raise ValueError('Path must be linear in joint space')

    num_waypoints = path.GetNumWaypoints()

    positions_array = numpy.zeros((num_waypoints, num_dof))
    for i in range(num_waypoints):
        # Get first num_dof values, the values that
        # come afterwards are velocities
        q = path.GetWaypoint(i)[range(num_dof)]
        positions_array[i,:] = q

    return positions_array