Add PlanWorkspacePath() method to VectorFieldPlanner

[DavidB-CMU]

This PR is to add a PlanWorkspacePath() method to VectorFieldPlanner.

[DavidB-CMU]

David wrote:

General questions about VectorFieldPlanner:

• If you call PlanToEndEffectorOffset() for some distance, the planner never reaches that distance. Can we fix this?
• Should we rename the "max_distance" argument? it seems confusing to me.
• Potentially PlanToEndEffectorOffset() could be refactored to call PlanWorkspacePath, however I think PlanToEndEffectorOffset() has a more efficient implementation.
• Do all the PlanWorkspacePath() methods in each planner need to have the same prototype?
• To implement PlanWorkspacePath(), I call GetMinDistance... in both of the callbacks, so maybe this is not optimal.

Mike replied:

This is a very nice addition. I left a bunch of comments on the diff and have answered your questions below:

If you call PlanToEndEffectorOffset() for some distance, the planner never reaches that distance. Can we fix this?

Can you be more specific?

Should we rename the "max_distance" argument? it seems confusing to me.

What do you suggest as a new name? I am open to renaming this as long as:

1. we add a stub that maintains backwards compatibility; i.e. forward the max_distance parameter and raise a deprecation warning
2. update any other PlanToEndEffectorOffset methods that take this argument in the same way

Potentially PlanToEndEffectorOffset() could be refactored to call PlanWorkspacePath, however I think PlanToEndEffectorOffset() has a more efficient implementation.

To implement PlanWorkspacePath(), I call GetMinDistance... in both of the callbacks, so maybe this is not optimal.

That would be a nice change as long as, like you said, it does not have major performance implications. Can you time the two implementations and see if there's a noticeable performance difference?

Do all the PlanWorkspacePath() methods in each planner need to have the same prototype?

The same arguments get passed to all planners in a Sequence. At a minimum, all of the planners should have identical required arguments. It's fine to have different optional arguments, but we do try to make them overlap as much as possible (e.g. we consistently use timelimit for all planners that have a planning time limit).

David wrote:

I will focus on finishing this PR (see below), then we can come back to the above comments.

[DavidB-CMU]

Mike's diff comments:

No. 1:

def vf_geodesic():
"""
Function defining a joint-space vector field.
"""

This docstring is misleading. An arbitrary vector field function implements a joint-space vector field. This function specifically implements a vector field that moves along the geodesic towards goal_pose.

I also don't think this function strictly needs a doscstring - although I'm certainly not going to argue with adding one - since it is only accessible inside this function.

No. 2:

def vf_geodesic():
return min(abs(vlimits[i] / dqout[i]) if dqout[i] != 0. else 1. for i in xrange(vlimits.shape[0])) * dqout

         def CloseEnough():
-            pose_error = util.GeodesicDistance(
-                        manip.GetEndEffectorTransform(),
-                        goal_pose)
+            """
+            Function defining the termination condition.
+
+            Succeed if end-effector pose is close to the goal pose.
+            """

See my comment above on vf_geodesic. The same applies here.

No. 3:

return Status.CONTINUE

+        integration_timelimit = 10.0

I believe this should use the timelimit argument.

No. 4:

+        # Get the final end-effector pose
+        duration = traj.GetDuration()
+        T_ee_goal = openravepy.matrixFromPose(traj.Sample(duration)[0:7])
+
+        def vf_path():
+            """
+            Function defining a joint-space vector field.
+            """
+            T_ee_actual = manip.GetEndEffectorTransform()
+
+            # Find where we are on the goal trajectory by finding
+            # the the closest point
+            (_,t) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
+                                                                   T_ee_actual,
+                                                                   traj)

I believe this will fail if the trajectory has a loop (or near-loop) in it. You may want to mention this in the docstring.

No. 5:

+        @param float angular_tolerance:  Constraint tolerance (radians).
+        @param float goal_pose_tol: Goal tolerance (meters).
+
+        @return openravepy.Trajectory qtraj: Configuration space path.
+        """
+        if position_tolerance < 0.0:
+            raise ValueError('Position tolerance must be non-negative.')
+        elif angular_tolerance < 0.0:
+            raise ValueError('Angular tolerance must be non-negative.')
+
+        manip = robot.GetActiveManipulator()
+        Tstart = manip.GetEndEffectorTransform()
+
+        # Get the final end-effector pose
+        duration = traj.GetDuration()
+        T_ee_goal = openravepy.matrixFromPose(traj.Sample(duration)[0:7])

This assumes that the trajectory has a single AffineDOFs group that is parameterized by a translation and a quaternion. If you are making this assumption, then you should check for this when you validate the input arguments (and add a few unit tests).

Also, if this is a path (i.e. untimed), how are you Sampleing with a time?

No. 6:

+        def vf_path():
+            """
+            Function defining a joint-space vector field.
+            """
+            T_ee_actual = manip.GetEndEffectorTransform()
+
+            # Find where we are on the goal trajectory by finding
+            # the the closest point
+            (_,t) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
+                                                                   T_ee_actual,
+                                                                   traj)
+            # Get the desired end-effector transform from
+            # the goal trajectory
+            t_step = 0.2
+            desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])
+            pose_ee_next = traj.Sample(t+t_step)[0:7]

It looks like you are using t_step to approximate a finite-difference velocity. This is problematic because:

• the trajectory is un-timed (see above)
• you have no guarantee on the scale of the parameterization - 0.2 may be very large or very small

Ideally, we would directly extract the derivative from the trajectory and use this as the feed-forward term in the controller. If that is not possible, then we should at least convert the trajectory to a known parametrization (e.g. unit velocity) so we know the scale of t_step

No. 7:

+            # on the goal trajectory
+            twist_parallel = util.GeodesicTwist(desired_T_ee,
+                                                desired_T_ee_next)
+            direction_parallel = desired_T_ee_next[0:3,3] - desired_T_ee[0:3,3]
+
+            twist = 0.0*twist_perpendicular + 1.0*twist_parallel
+
+            K_p = 0.5
+            direction = (1.0-K_p)*direction_perpendicular + \
+                         K_p*direction_parallel
+            # Normalize the direction vector
+            direction = numpy.array(direction, dtype='float')
+            direction /= numpy.linalg.norm(direction)
+
+            # Modify the twist
+            twist[0:3] = direction

I'm not sure whats going on here. I think the current implementation will allow arbitrary drift of orientation, since the (1 - Kp) * direction_parallel feedback term is only applied to the translational component.

No. 8:

+            twist = 0.0*twist_perpendicular + 1.0*twist_parallel
+
+            K_p = 0.5
+            direction = (1.0-K_p)*direction_perpendicular + \
+                         K_p*direction_parallel
+            # Normalize the direction vector
+            direction = numpy.array(direction, dtype='float')
+            direction /= numpy.linalg.norm(direction)
+
+            # Modify the twist
+            twist[0:3] = direction
+
+            # Calculate joint velocities using an optimized jacobian
+            dqout, _ = util.ComputeJointVelocityFromTwist(robot, twist,
+                                              joint_velocity_limits=numpy.PINF)

Why are you passing infinite velocity limits here? This is concerning for a few reasons:

• Isn't joint_velocity_limits typically a vector of length len(robot.GetActiveDOFs()), not a scalar?
• Does this function behave reasonably when the value is set to infinity?
• Won't this result in very large steps?

No. 9:

+                    'Deviated from orientation constraint.')
+
+            position_deviation = numpy.linalg.norm(position_error)
+            if position_deviation > position_tolerance:
+                raise ConstraintViolationPlanningError(
+                    'Deviated from straight line constraint.')
+
+            # Check if we have reached the end of the goal trajectory
+            dist_to_goal = util.GetGeodesicDistanceBetweenTransforms(T_ee_curr,
+                                                                     T_ee_goal)
+            if numpy.fabs(dist_to_goal) <= goal_pose_tol:
+                return Status.CACHE_AND_TERMINATE
+
+            return Status.CONTINUE
+
+        integration_timelimit = 10.0

This should be an argument.

No. 10:

return fn_vectorfield()

         def fn_status_callback(t, q):
+            """
+            This is called for each collision check.
+            """

This is overly specific. More than just collision checking happens in here - e.g. constraint violation checks.

No. 11:

"""
         Follow a joint space vectorfield to termination.

         @param robot
         @param fn_vectorfield a vectorfield of joint velocities
         @param fn_terminate custom termination condition
+        @param integration_timelimit The initial time step to be taken
+                                     by the integrator.

This docstring is misleading. The initial step size is passed as the first_step parameter to the integrator and is currently hard-coded to 0.1. This argument appears to be the value of t for which we're trying to compute the value of the integral.

No. 12:

+
+def GetGeodesicDistanceBetweenTransforms(T0, T1):
+    """
+    Wrapper, to match GetGeodesicDistanceBetweenQuaternions()
+    """
+    return GeodesicDistance(T0, T1, r=1.0)
+
+
+def GetEuclideanDistanceBetweenTransforms(T0, T1):
+    """
+    Calculate the Euclidean distance between two 4x4 transforms.
+    (also called L2 or Pythagorean distance)

This appears to only consider translational distance. This is not clear from the name of the function of the docstring.

No. 13:

+    if not IsWorkspaceTrajectory(traj):
+        raise ValueError("Trajectory is not a workspace trajectory, it "
+                         "must have configuration specification of "
+                         "openravepy.IkParameterizationType.Transform6D")
+
+    min_dist = numpy.inf
+    t_loc = 0.0
+    t = 0.0
+    while t < traj.GetDuration():
+        T_curr = openravepy.matrixFromPose(traj.Sample(t)[0:7])
+        error = GetEuclideanDistanceBetweenTransforms(T, T_curr)
+        if error < min_dist:
+            min_dist = error
+            t_loc = t
+        t = t + dt
+    return (min_dist, t_loc)

You may want to add a special case that explicitly checks traj.GetDuration(). Otherwise, I could imagine this messing up the termination condition above.

It would also be useful to return T_curr in the tuple so the user doesn't have to evaluate the trajectory again.

[DavidB-CMU]

Most of the above items have been addressed.
Still outstanding:

• Implement new function: util.ComputeGeodesicUnitTiming()
• Another new function: util.GetAngularVelocity() for trajectory of type IkParametrization
• add unit tests: pass in a non-workspace trajectory, etc.

[mkoval]

I think this is very close to being ready to merge. I added a few comments about minor quibbles above, but the only major issue remaining is to add unit tests and the ComputeGeodesicUnitTiming function.

I think it's okay to keep the finite-differenced velocities for now, as long as you can guarantee that the trajectory has unit timing. Adding support for derivatives is likely more trouble than it's worth - we don't even have a good way to generating a timing for an IkParameterization trajectory in the first place.

[DavidB-CMU]

All of the above have been addressed. I'm working on the Unit Tests. @mkoval Can you have a look at this please?

[DavidB-CMU]

Added Unit Tests for new functions in prpy.util

[mkoval]

:+1: Thanks for the great unit tests as always!