Invalid times in GetLinearCheckCollisionPts.

[psigen]

This happened with a tiny herbpy right-arm trajectory from relaxed_home:

>> path.GetWaypoint(0)
array([ 5.65, -1.76, -0.26,  1.96, -1.15,  0.87, -1.43,  0.  ])
>> path.GetWaypoint(1)
array([ 5.53026211, -1.72742278, -0.26695606,  2.22895681, -1.26622183, 0.72664121, -1.10269112,  0.1 ])
>> path.GetNumWaypoints()
2
>> path.GetDuration()
0.1

>> vdc = util.VanDerCorputSampleGenerator
>> c = util.GetLinearCollisionCheckPts(robot, path, norm_order=2, sampling_func=vdc)
>> c.next()
(0.0, array([ 5.65, -1.76, -0.26,  1.96, -1.15,  0.87, -1.43]))
>> c.next()
(23.925663580229685,  array([ 5.53026211, -1.72742278, -0.26695606,  2.22895681, -1.26622183, 0.72664121, -1.10269112])

Problem: 23.92 > 0.1

[DavidB-CMU]

Please post a minimal test-case.

[psigen]

Ok, here is the example from above with the necessary OpenRAVE boilerplate.

rosrun herbpy console.py -s

robot.right_arm.SetActive()
cspec = robot.GetActiveConfigurationSpecification('linear')
cspec.AddDeltaTimeGroup()

traj = openravepy.RaveCreateTrajectory(env, '')
traj.Init(cspec)
traj.Insert(0, [ 5.65, -1.76, -0.26,  1.96, -1.15,  0.87, -1.43,  0.])
traj.Insert(1, [ 5.53026211, -1.72742278, -0.26695606,  2.22895681, -1.26622183, 0.72664121, -1.10269112,  0.1 ])

import prpy.util
pts = prpy.util.GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=prpy.util.VanDerCorputSampleGenerator)
pts.next()
t, q = pts.next()

print "Problem: {:f} > {:f}".format(t, traj.GetDuration())

[DavidB-CMU]

@psigen based on your provided code, the output 23.92 > 0.1 you are seeing is correct. However you were expecting some different output. So this means we should improve the documentation or ... see below.

You are seeing this output:

t =  34.0
q =  [ 5.53251722 -1.72803633 -0.26682505  2.22389135 -1.26403294  0.72934119
 -1.10885556]

The complete sequence of checks looks like this:

check  1  t = 0.0  q = [ 5.65 -1.76 -0.26  1.96 -1.15  0.87 -1.43]
check  2  t = 34.0  q = [ 5.53251722 -1.72803633 -0.26682505  2.22389135 -1.26403294  0.72934119
 -1.10885556]
check  3  t = 16.0  q = [ 5.59471399 -1.74495827 -0.26321179  2.08418417 -1.20366256  0.80380762
 -1.27887321]
check  4  t = 8.0  q = [ 5.62235699 -1.75247914 -0.26160589  2.02209208 -1.17683128  0.83690381
 -1.3544366 ]
check  5  t = 26.0  q = [ 5.56016023 -1.73555719 -0.26521916  2.16179927 -1.23720166  0.76243738
 -1.18441896]
check  6  t = 4.0  q = [ 5.6361785  -1.75623957 -0.26080295  1.99104604 -1.16341564  0.8534519
 -1.3922183 ]
check  7  t = 22.0  q = [ 5.57398173 -1.73931763 -0.26441621  2.13075323 -1.22378602  0.77898547
 -1.22220066]
check  8  t = 12.0  q = [ 5.60853549 -1.7487187  -0.26240884  2.05313812 -1.19024692  0.82035571
 -1.3166549 ]
check  9  t = 30.0  q = [ 5.54633872 -1.73179676 -0.2660221   2.19284531 -1.2506173   0.74588928
 -1.14663726]
check  10  t = 2.0  q = [ 5.64308925 -1.75811978 -0.26040147  1.97552302 -1.15670782  0.86172595
 -1.41110915]
check  11  t = 20.0  q = [ 5.58089248 -1.74119784 -0.26401474  2.11523021 -1.2170782   0.78725952
 -1.24109151]
check  12  t = 10.0  q = [ 5.61544624 -1.75059892 -0.26200737  2.0376151  -1.1835391   0.82862976
 -1.33554575]
check  13  t = 28.0  q = [ 5.55324947 -1.73367698 -0.26562063  2.17732229 -1.24390948  0.75416333
 -1.16552811]
check  14  t = 6.0  q = [ 5.62926774 -1.75435935 -0.26120442  2.00656906 -1.17012346  0.84517786
 -1.37332745]
check  15  t = 24.0  q = [ 5.56707098 -1.73743741 -0.26481768  2.14627625 -1.23049384  0.77071143
 -1.20330981]
check  16  t = 14.0  q = [ 5.60162474 -1.74683849 -0.26281032  2.06866115 -1.19695474  0.81208167
 -1.29776406]
check  17  t = 32.0  q = [ 5.53942797 -1.72991655 -0.26642358  2.20836833 -1.25732512  0.73761524
 -1.12774641]
check  18  t = 18.0  q = [ 5.58780323 -1.74307806 -0.26361326  2.09970719 -1.21037038  0.79553357
 -1.25998236]

Initially you created a trajectory of duration 0.1, but the first thing GetLinearCollisionCheckPts() does is this:

If trajectory is already timed, strip the deltatime values

if IsTimedTrajectory(traj): traj = UntimeTrajectory(traj)

The t values returned are not related to the timing that you gave the trajectory. The decision to return them was agreed-upon with @mkoval as this function can be used in many ways.

Perhaps we could print a warning "Warning: delta_time values were stripped"? or improve the docstring?

[psigen]

Ok, if that's the case, then I think we might need to improve the docstring, which currently states:

@returns generator: A tuple (t,q) of float values, being the sample
                        time and joint configuration.

So what is the "sample time"? It is clearly not a time at all, but some sort of proportional alpha value between waypoints. How do I use this value to figure out where a collision occurred along an initially timed trajectory?

I was expecting that the "sample time" t would be a time which, when Sample(t) was called on the trajectory, would yield q.

[mkoval]

I agree with @psigen that this is a documentation issue. The value t is the path parameter of the trajectory after we re-parameterize it so we can perform collision checks at steps of unit size. It's not possible to interpret this value without duplicating the internal timing logic.

I am fine with either: (1) changing the comment to make it clear that t does not correspond to "sample time" or (2) mapping t back to a time in the input trajectory.

[DavidB-CMU]

Here's a proposed fix that does both of the above things:

def GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=None):
    """
    For a piece-wise linear trajectory, generate a list
    of configuration pairs that need to be collision checked.

    This will step along the trajectory from start to end
    at a resolution that satisifies the specified error metric.

    @param openravepy.Robot      robot: The robot.
    @param openravepy.Trajectory traj:  The trajectory for which we need
                                        to generate sample points.
    @param int      norm_order: 1  ==>  The L1 norm
                                2  ==>  The L2 norm
                                inf  ==>  The L_infinity norm
    @param generator sampling_func A function that returns a sequence of
                                   sample times.
                                   e.g. SampleTimeGenerator() 
                                         or
                                        VanDerCorputSampleGenerator()
    @returns generator: A tuple (t,q) of float values:
                              t = (for an un-timed input traj)
                                     A proportional value used internally.
                                  (for timed input traj)
                                     Sample time along the duration of the
                                     input traj.
                              q = Joint configuration.
    """

    # If the user passes in a timed trajectory, the returned sample
    # time 't' for each configuration 'q' will be based on the
    # duration of the input trajectory.
    return_t_based_on_input_traj = False
    input_traj_duration = traj.GetDuration()

    # If trajectory is already timed, strip the deltatime values
    if IsTimedTrajectory(traj):
        traj = UntimeTrajectory(traj)
        return_t_based_on_input_traj = True

    traj_cspec = traj.GetConfigurationSpecification()

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

    dof_indices, _ = traj_cspec.ExtractUsedIndices(robot)

    # If trajectory only has 1 waypoint then we only need to
    # do 1 collision check.
    num_waypoints = traj.GetNumWaypoints()
    if num_waypoints == 1:
        t = 0.0
        waypoint = traj.GetWaypoint(0)
        q = traj_cspec.ExtractJointValues(waypoint, robot, dof_indices)
        yield t, q
        return

    env = robot.GetEnv()

    # Create a temporary trajectory that we will use
    # for sampling the points to collision check,
    # because there is no method to modify the 'deltatime'
    # values of waypoints in an OpenRAVE trajectory.
    temp_traj_cspec = traj.GetConfigurationSpecification()
    temp_traj_cspec.AddDeltaTimeGroup()
    temp_traj = openravepy.RaveCreateTrajectory(env, '')
    temp_traj.Init(temp_traj_cspec)

    # Set timing of first waypoint in temporary trajectory to t=0
    waypoint = traj.GetWaypoint(0, temp_traj_cspec)
    q0 = traj_cspec.ExtractJointValues(waypoint, robot, dof_indices)
    delta_time = 0.0
    temp_traj_cspec.InsertDeltaTime(waypoint, delta_time)
    temp_traj.Insert(0, waypoint)

    # Get the resolution (in radians) for each joint
    q_resolutions = robot.GetDOFResolutions()[dof_indices]

    # Iterate over each segment in the trajectory and set
    # the timing of each waypoint in the temporary trajectory
    # so that taking steps of t=1 will be within a required error norm.
    for i in range(1, num_waypoints):
        # We already have the first waypoint (q0) of this segment,
        # so get the joint values for the second waypoint.
        waypoint = traj.GetWaypoint(i)
        q1 = traj_cspec.ExtractJointValues(waypoint, robot, dof_indices)
        dq = numpy.abs(q1 - q0)
        max_diff_float = numpy.max( numpy.abs(q1 - q0) / q_resolutions)

        # Get the number of steps (as a float) required for
        # each joint at DOF resolution
        num_steps = dq / q_resolutions

        # Calculate the norm:
        #
        # norm_order = 1  ==>  The L1 norm
        # Which is like a diamond shape in configuration space
        # and equivalent to: L1_norm=sum(num_steps)
        #
        # norm_order = 2  ==>  The L2 norm
        # Which is like an ellipse in configuration space
        # and equivalent to: L2_norm=numpy.linalg.norm(num_steps)
        #
        # norm_order = inf  ==>  The L_infinity norm
        # Which is like a box shape in configuration space
        # and equivalent to: L_inf_norm=numpy.max(num_steps)
        norm = numpy.linalg.norm(num_steps, ord=norm_order)

        # Set timing of this waypoint
        waypoint = traj.GetWaypoint(i, temp_traj_cspec)
        delta_time = norm
        temp_traj_cspec.InsertDeltaTime(waypoint, delta_time)
        temp_traj.Insert(i, waypoint)

        # The last waypoint becomes the first in the next segment
        q0 = q1

    traj_duration = temp_traj.GetDuration()

    # Sample the trajectory using the specified sample generator
    step_size = 2
    seq = None
    if sampling_func == None:
        # (default) Linear sequence, from start to end
        seq = SampleTimeGenerator(0, traj_duration, step=step_size)
    else:
        seq = sampling_func(0, traj_duration, step=step_size)

    last_time_in_seq = 0.0
    if return_t_based_on_input_traj:
        # Get the largest time value 't' that will be in the above sequence
        last_time_in_seq = float(traj_duration - (traj_duration % step_size))

    # Sample the trajectory in time
    # and return time value and joint positions
    for t in seq:
        sample = temp_traj.Sample(t)
        q = temp_traj_cspec.ExtractJointValues(sample, robot, dof_indices)

        if return_t_based_on_input_traj:
            # Scale returned time 't' based on the duration
            # of the timed input trajectory
            fraction = input_traj_duration / last_time_in_seq
            t = t * fraction

        yield t, q

[mkoval]

Fixed by @jeking04 in #332.