Workaround for GetCollisionCheckPts bug in VectorFieldPlanner

[mkoval]

Each step of VectorFieldPlanner currently validates the entire trajectory, from the start, instead of only the part added since the last check. @rachelholladay noticed that this significantly slows the planner down when using a slow status callback.

This is a workaround. The correct fix is to modify GetCollisionCheckPts to correctly handle a non-zero starting time.

[psigen]

What is wrong with GetCollisionCheckPts() implementation of start_time? https://github.com/personalrobotics/prpy/blob/master/src/prpy/util.py#L1532

[mkoval]

There is a comment near our call to GetCollisionCheckPts that explains this:

This should start at t_check. Unfortunately, a bug in GetCollisionCheckPts causes this to enter an infinite loop.

I spent a significant amount of time debugging this, but was never able to completely stamp it out. I don't think it's worth doing so at this point - we're better off switching to the excellent collision checking code that @DavidB-CMU added in #228. I created issue #289 to track this.

[psigen]

What is the status of this PR?

[mkoval]

I am happy to merge this once Travis is happy. Did you have any additional comments?

[mkoval]

PlanToEndEffectorOffset is failing a constraint deviation check now. I wonder if we have an off-by-one error in our logic. 😦

======================================================================
FAIL: test_PlanToEndEffectorOffset_SatisfiesConstraint (tests.planning.test_VectorFieldPlanner.VectorFieldPlannerTest)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/travis/workspace/src/prpy/tests/planning/methods/PlanToEndEffectorOffset.py", line 66, in test_PlanToEndEffectorOffset_SatisfiesConstraint
    self.assertAlmostEqual(distance, 0.1, delta=0.005)
AssertionError: 0.10909466094016954 != 0.1 within 0.005 delta
-------------------- >> begin captured logging << --------------------

[DavidB-CMU]

It might be nice to keep this function around because potentially it's a second method we can compare in our collision-checking benchmarks.

Here's a possible fix to GetCollisionCheckPts(), to correctly handle a non-zero start_time. It works for my test cases, but there is perhaps a different situation that we're not checking for: if the step size gets very small but one of the joints is still beyond DOF resolution.

def GetCollisionCheckPts(robot, traj, include_start=True, start_time=0.0,
                         first_step=None, epsilon=1e-6):
    """
    Generate a list of (time, configuration) pairs to collision check.

    This function works by taking two joint configurations, if the delta for
    any joint is greater than DOF resolution then the straight-line joint
    path is bisected to find a new end configuration to check.

    If every generated configuration is collision free, then the trajectory is
    guaranteed to be collision free up to DOF resolution. This function only
    operates on timed trajectories. If you want to use this function on a path,
    then consider using the util.ComputeUnitTiming function to compute its
    arclength parameterization.

    @param openravepy.Trajectory traj:  The timed trajectory for which we need
                                        to generate sample points.
    @param bool include_start: True = collision-check the first point, which
                               should be the current configuration.
    @param float start_time: Generate points from start_time to duration.
    @param float first_step: The first dt step size to take.
                             None = default to trajectory duration.
    @param float epsilon: Used to determine if a sample time is close enough
                          to being the duration time.

    @returns generator of (time, configuration) pairs 
    """

    if not IsTimedTrajectory(traj):
        raise ValueError('Trajectory must be timed. If you want to use this '
                         'function on a path, then consider using '
                         'util.ComputeUnitTiming() to compute its arclength '
                         'parameterization.')

    cspec = traj.GetConfigurationSpecification()
    dof_indices, _ = cspec.ExtractUsedIndices(robot)
    q_resolutions = robot.GetDOFResolutions()[dof_indices]
    duration = traj.GetDuration()

    if not (0. <= start_time < duration + epsilon):
        raise ValueError('Start time {:.6f} is out of range [0, {:.6f}].' \
                                                  .format(start_time, duration))

    start_time = min(start_time, duration)

    if first_step is None:
        first_step = duration - start_time

    if not (0. < first_step <= (duration - start_time)):
        raise ValueError('First step {:.6f} is out of range (0, {:.6f}]' \
                                     .format(first_step, duration - start_time))

    # Bisection method. Start at the begining of the trajectory and initialize
    # the stepsize to the end of the trajectory.
    t_prev = start_time

    # Error, this line causes infinite loop:
    # t_prev is a time, not a waypoint number
    #q_prev = cspec.ExtractJointValues(traj.GetWaypoint(t_prev), robot, dof_indices)
    q_prev = cspec.ExtractJointValues(traj.Sample(t_prev), robot, dof_indices)

    dt = first_step

    if include_start:
        # Collision check the first point
        yield t_prev, q_prev
    while t_prev < (duration - epsilon):
        t_curr = t_prev + dt
        q_curr = cspec.ExtractJointValues(traj.Sample(t_curr), robot, dof_indices)

        # Step violated dof resolution. Halve the step size and continue.
        if (numpy.abs(q_curr - q_prev) > q_resolutions).any():
            dt = dt / 2.0
        else:
            # Yield this configuration.
            yield t_curr, q_curr
            # Double the step size and continue.
            q_prev = q_curr
            t_prev = min(t_curr, duration)
            dt = 2.0 * dt

[mkoval]

Replaced by #359.