Changes for VectorFieldPlanner

Author: DavidB-CMU

src/prpy/planning/vectorfield.py

@@ -96,7 +96,8 @@ def PlanToEndEffectorPose(self, robot, goal_pose, timelimit=5.0,
 
         def vf_geodesic():
             """
-            Function defining a joint-space vector field.
+            Define a joint-space vector field, that moves along the
+            geodesic (shortest path) from the start pose to the goal pose.
             """
             twist = util.GeodesicTwist(manip.GetEndEffectorTransform(),
                                        goal_pose)
@@ -109,9 +110,10 @@ def vf_geodesic():
 
         def CloseEnough():
             """
-            Function defining the termination condition.
-
-            Succeed if end-effector pose is close to the goal pose.
+            The termination condition.
+            At each integration step, the geodesic error between the
+            start and goal poses is compared. If within threshold,
+            the integration will terminate.
             """
             pose_error = util.GetGeodesicDistanceBetweenTransforms(
                                                manip.GetEndEffectorTransform(),
@@ -120,9 +122,9 @@ def CloseEnough():
                 return Status.TERMINATE
             return Status.CONTINUE
 
-        integration_timelimit = 10.0
+        integration_interval = 10.0
         traj = self.FollowVectorField(robot, vf_geodesic, CloseEnough,
-                                      integration_timelimit,
+                                      integration_interval,
                                       timelimit,
                                       **kw_args)
 
@@ -163,7 +165,7 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
 
         # Normalize the direction vector.
         direction = numpy.array(direction, dtype='float')
-        direction /= numpy.linalg.norm(direction)
+        direction = util.NormalizeVector(direction)
 
         manip = robot.GetActiveManipulator()
         Tstart = manip.GetEndEffectorTransform()
@@ -217,9 +219,9 @@ def TerminateMove():
 
             return Status.CONTINUE
 
-        integration_timelimit = 10.0
+        integration_interval = 10.0
         return self.FollowVectorField(robot, vf_straightline, TerminateMove,
-                                      integration_timelimit,
+                                      integration_interval,
                                       timelimit, 
                                       **kw_args)
 
@@ -230,6 +232,8 @@ def PlanWorkspacePath(self, robot, traj,
                           position_tolerance=0.01,
                           angular_tolerance=0.15,
                           goal_pose_tol=0.01,
+                          Kp_ff=None,
+                          Kp_e=None,
                           **kw_args):
         """
         Plan a configuration space path given a workspace path.
@@ -243,13 +247,47 @@ def PlanWorkspacePath(self, robot, traj,
         @param float position_tolerance: Constraint tolerance (meters).
         @param float angular_tolerance:  Constraint tolerance (radians).
         @param float goal_pose_tol: Goal tolerance (meters).
+        @param numpy.array Kp_ff: Feed-forward gain.
+                                  A 1x6 vector, where first 3 elements
+                                  affect the translational velocity,
+                                  the last 3 elements affect the
+                                  rotational velocity.
+        @param numpy.array Kp_e: Error gain.
+                                 A 1x6 vector, where first 3 elements
+                                 affect the translational velocity,
+                                 the last 3 elements affect the
+                                 rotational velocity.
 
         @return openravepy.Trajectory qtraj: Configuration space path.
         """
+
+        if not util.IsTrajectoryTypeIkParameterization(traj):
+            raise ValueError("Trajectory is not a workspace trajectory, it "
+                             "must have configuration specification of "
+                             "openravepy.IkParameterizationType.Transform6D")
+
+        #if IsTimedTrajectory(traj):
+        #    raise ValueError("PlanWorkspacePath expected an un-timed "
+        #                     "trajectory.")
+
         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.')
+        elif goal_pose_tol < 0.0:
+            raise ValueError('Goal-pose tolerance must be non-negative.')
+
+        #
+        #
+        # ADD ComputeGeodesicUnitTiming()
+        #
+        #
+
+        # Set the default gains
+        if Kp_ff == None:
+            Kp_ff = numpy.array([1.0,1.0,1.0,1.0,1.0,1.0])
+        if Kp_e == None:
+            Kp_e = numpy.array([1.0,1.0,1.0,1.0,1.0,1.0])
 
         manip = robot.GetActiveManipulator()
         Tstart = manip.GetEndEffectorTransform()
@@ -266,7 +304,7 @@ def vf_path():
 
             # Find where we are on the goal trajectory by finding
             # the the closest point
-            (_,t) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
+            (_,t,_) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
                                                                    T_ee_actual,
                                                                    traj)
             # Get the desired end-effector transform from
@@ -278,27 +316,23 @@ def vf_path():
 
             # The twist between the actual end-effector position and
             # where it should be on the goal trajectory
+            # (the error term)
             twist_perpendicular = util.GeodesicTwist(T_ee_actual,
                                                      desired_T_ee)
-            direction_perpendicular = desired_T_ee[0:3,3] - T_ee_actual[0:3,3]
 
             # The twist tangent to where the end-effector should be
             # on the goal trajectory
+            # (the feed-forward term)
             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)
+            # Normalize the translational and angular velocity of
+            # the feed-forward twist
+            twist_parallel[0:3] = util.NormalizeVector(twist_parallel[0:3])
+            twist_parallel[3:6] = util.NormalizeVector(twist_parallel[3:6])
 
-            # Modify the twist
-            twist[0:3] = direction
+            # Apply gains
+            twist = Kp_e*twist_perpendicular + Kp_ff*twist_parallel
 
             # Calculate joint velocities using an optimized jacobian
             dqout, _ = util.ComputeJointVelocityFromTwist(robot, twist,
@@ -319,7 +353,7 @@ def TerminateMove():
 
             # Find where we are on the goal trajectory by finding
             # the the closest point
-            (_, t) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
+            (_,t,_) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
                                                                      T_ee_curr,
                                                                      traj)
             # Get the desired end-effector transform from
@@ -349,15 +383,15 @@ def TerminateMove():
 
             return Status.CONTINUE
 
-        integration_timelimit = 10.0
+        integration_interval = 10.0
         return self.FollowVectorField(robot, vf_path, TerminateMove,
-                                      integration_timelimit,
+                                      integration_interval,
                                       timelimit, **kw_args)
 
 
     @PlanningMethod
     def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
-                          integration_timelimit=10.,
+                          integration_time_interval=10.0,
                           timelimit=5.0,
                           **kw_args):
         """
@@ -366,8 +400,8 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
         @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.
+        @param integration_time_interval The time interval to integrate
+                                         over.
         @param timelimit time limit before giving up
         @param kw_args keyword arguments to be passed to fn_vectorfield
         @return traj
@@ -420,7 +454,12 @@ def fn_wrapper(t, q):
 
         def fn_status_callback(t, q):
             """
-            This is called for each collision check.
+            Check joint-limits and collisions for a specific joint
+            configuration. This is called multiple times at DOF
+            resolution in order to check along the entire length of the
+            trajectory.
+            Note: Currently this is called after each integration time
+            step, which means we are doing more checks than required.
             """
             if time.time() - time_start >= timelimit:
                 raise TimeLimitError()
@@ -496,7 +535,7 @@ def fn_callback(t, q):
         integrator.set_solout(fn_callback)
         # Initial conditions
         integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)
-        integrator.integrate(t=integration_timelimit)
+        integrator.integrate(t=integration_time_interval)
 
         t_cache = nonlocals['t_cache']
         exception = nonlocals['exception'] 

src/prpy/util.py

@@ -196,6 +196,19 @@ def ComputeAinv(N,dof):
                 invA[i*dof+k,j*dof+k] = invA_small[i-1,j-1]
     return invA
 
+def NormalizeVector(vec):
+    """
+    Normalize a vector.
+    This is faster than doing: vec/numpy.linalg.norm(vec)
+
+    @param numpy.array vec: A 1-dimensional vector.
+    @returns numpy.array result: A vector of the same size, where the
+                                 L2 norm of the elements equals 1.
+    """
+    magnitude = numpy.sqrt(vec.dot(vec))
+    vec2 = (vec / magnitude)
+    return numpy.nan_to_num(vec2) # convert NaN to zero
+
 def MatrixToTraj(traj_matrix,cs,dof,robot):
     env = robot.GetEnv()
     traj = openravepy.RaveCreateTrajectory(env,'')
@@ -730,12 +743,14 @@ 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.
+    Calculate the Euclidean distance between the translational
+    component of two 4x4 transforms.
     (also called L2 or Pythagorean distance)
     """
     p0 = T0[0:3,3] # Get the x,y,z translation from the 4x4 matrix
@@ -752,29 +767,44 @@ def GetMinDistanceBetweenTransformAndWorkspaceTraj(T, traj, dt=0.01):
     @param openravepy.Trajectory traj: A timed workspace trajectory.
     @param float dt: Resolution at which to sample along the trajectory.
 
-    @return (float,float) (min_dist, t_loc) The minimum distance and the
-                                            time value along the timed
-                                            trajectory.
+    @return (float,float) (min_dist, t_loc, T_loc) The minimum distance,
+                                         the time value along the timed
+                                         trajectory, and the transform.
     """
     if not IsTimedTrajectory(traj):
         raise ValueError("Trajectory must have timing information.")
 
-    if not IsWorkspaceTrajectory(traj):
+    if not IsTrajectoryTypeIkParameterization(traj):
         raise ValueError("Trajectory is not a workspace trajectory, it "
                          "must have configuration specification of "
                          "openravepy.IkParameterizationType.Transform6D")
 
+    def _GetError(t):
+        T_curr = openravepy.matrixFromPose(traj.Sample(t)[0:7])
+        error = GetEuclideanDistanceBetweenTransforms(T, T_curr)
+
     min_dist = numpy.inf
     t_loc = 0.0
+    T_loc = None
+
+    # Iterate over the trajectory
     t = 0.0
-    while t < traj.GetDuration():
-        T_curr = openravepy.matrixFromPose(traj.Sample(t)[0:7])
-        error = GetEuclideanDistanceBetweenTransforms(T, T_curr)
+    duration = traj.GetDuration()
+    while t < duration:
+        error = _GetError(t)
         if error < min_dist:
             min_dist = error
             t_loc = t
         t = t + dt
-    return (min_dist, t_loc)
+    # Also check the end-point
+    error = _GetError(duration)
+    if error < min_dist:
+        min_dist = error
+        t_loc = t
+
+    T_loc = openravepy.matrixFromPose(traj.Sample(t_loc)[0:7])
+
+    return (min_dist, t_loc, T_loc)
 
 
 def FindCatkinResource(package, relative_path):
@@ -965,7 +995,7 @@ def IsJointSpaceTrajectory(traj):
     return False
 
 
-def IsWorkspaceTrajectory(traj):
+def IsTrajectoryTypeIkParameterization(traj):
     """
     Check if trajectory is a workspace trajectory.