Improvements and new functions for prpy.util

Author: DavidB-CMU

src/prpy/util.py

@@ -205,6 +205,7 @@ def NormalizeVector(vec):
     @returns numpy.array result: A vector of the same size, where the
                                  L2 norm of the elements equals 1.
     """
+    numpy.seterr(divide='ignore', invalid='ignore')
     magnitude = numpy.sqrt(vec.dot(vec))
     vec2 = (vec / magnitude)
     return numpy.nan_to_num(vec2) # convert NaN to zero
@@ -726,6 +727,25 @@ def GeodesicError(t1, t2):
     return numpy.hstack((trans, angle))
 
 
+def AngleBetweenQuaternions(quat1, quat2):
+    """
+    Compute the angle between two quaternions.
+    From 0 to 2pi.
+    """
+    theta = numpy.arccos( 2.0*( quat1.dot(quat2) )**2 - 1.0)
+    return theta
+
+
+def AngleBetweenRotations(rot1, rot2):
+    """
+    Compute the angle between two 3x3 rotation matrices.
+    From 0 to 2pi.
+    """
+    quat1 = openravepy.quatFromRotationMatrix(rot1)
+    quat2 = openravepy.quatFromRotationMatrix(rot2)
+    return AngleBetweenQuaternions(quat1, quat2)
+
+
 def GeodesicDistance(t1, t2, r=1.0):
     '''
     Computes the geodesic distance between two transforms
@@ -739,12 +759,28 @@ def GeodesicDistance(t1, t2, r=1.0):
     return numpy.linalg.norm(error)
 
 
-def GetGeodesicDistanceBetweenTransforms(T0, T1):
+def GetGeodesicDistanceBetweenTransforms(T0, T1, r=1.0):
     """
     Wrapper, to match GetGeodesicDistanceBetweenQuaternions()
+
+    Calculate the geodesic distance between two transforms, being
+    gd = norm( relative translation + r * axis-angle error )
+
+    @param t1 current transform
+    @param t2 goal transform
+    @param r in units of meters/radians converts radians to meters
     """
+    return GeodesicDistance(T0, T1, r)
 
-    return GeodesicDistance(T0, T1, r=1.0)
+
+def GetEuclideanDistanceBetweenPoints(p0, p1):
+    """
+    Calculate the Euclidean distance (L2 norm) between two vectors.
+    """
+    sum = 0.0
+    for i in xrange(len(p0)):
+        sum = sum + (p0[i]-p1[i])*(p0[i]-p1[i])
+    return numpy.sqrt(sum)
 
 
 def GetEuclideanDistanceBetweenTransforms(T0, T1):
@@ -755,7 +791,7 @@ def GetEuclideanDistanceBetweenTransforms(T0, T1):
     """
     p0 = T0[0:3,3] # Get the x,y,z translation from the 4x4 matrix
     p1 = T1[0:3,3]
-    return numpy.sqrt(numpy.sum((p0-p1)**2))
+    return GetEuclideanDistanceBetweenPoints(p0, p1)
 
 
 def GetMinDistanceBetweenTransformAndWorkspaceTraj(T, traj, dt=0.01):
@@ -774,14 +810,15 @@ def GetMinDistanceBetweenTransformAndWorkspaceTraj(T, traj, dt=0.01):
     if not IsTimedTrajectory(traj):
         raise ValueError("Trajectory must have timing information.")
 
-    if not IsTrajectoryTypeIkParameterization(traj):
+    if not IsTrajectoryTypeIkParameterizationTransform6D(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)
+        return error
 
     min_dist = numpy.inf
     t_loc = 0.0
@@ -995,10 +1032,37 @@ def IsJointSpaceTrajectory(traj):
     return False
 
 
-def IsTrajectoryTypeIkParameterization(traj):
+def IsWorkspaceTrajectory(traj):
     """
     Check if trajectory is a workspace trajectory.
 
+    @param openravepy.Trajectory traj: A path or trajectory.
+    @return bool result: Returns True or False.
+    """
+    return IsTrajectoryTypeIkParameterizationTransform6D(traj)
+
+
+def IsTrajectoryTypeIkParameterization(traj):
+    """
+    Check if trajectory has a configuration specification
+    of type IkParameterization:
+      Transform6d
+      Rotation3D
+      Translation3D
+      Direction3D
+      Ray4D
+      Lookat3D
+      TranslationDirection5D
+      TranslationXY2D
+      TranslationXYOrientation3D
+      TranslationLocalGlobal6D
+      TranslationXAxisAngle4D
+      TranslationYAxisAngle4D
+      TranslationZAxisAngle4D
+      TranslationXAxisAngleZNorm4D
+      TranslationYAxisAngleXNorm4D
+      TranslationZAxisAngleYNorm4D
+
     @param openravepy.Trajectory traj: A path or trajectory.
     @return bool result: Returns True or False.
     """
@@ -1010,6 +1074,43 @@ def IsTrajectoryTypeIkParameterization(traj):
     return False
 
 
+def IsTrajectoryTypeIkParameterizationTransform6D(traj):
+    """
+    Check if trajectory has a configuration specification
+    of type IkParameterization.Transform6D
+
+    @param openravepy.Trajectory traj: A path or trajectory.
+    @return bool result: Returns True or False.
+    """
+    try:
+        IKP_type = openravepy.IkParameterizationType.Transform6D
+        # The IKP type must be passed as a number
+        group_name = "ikparam_values {0}".format(int(IKP_type))
+        if traj.GetConfigurationSpecification().GetGroupFromName(group_name):
+            return True
+    except openravepy.openrave_exception:
+        pass
+    return False
+
+
+def IsTrajectoryTypeIkParameterizationTranslationDirection5D(traj):
+    """
+    Check if trajectory has a configuration specification
+    of type IkParameterization.TranslationDirection5D
+
+    @param openravepy.Trajectory traj: A path or trajectory.
+    @return bool result: Returns True or False.
+    """
+    try:
+        IKP_type = openravepy.IkParameterizationType.TranslationDirection5D
+        group_name = "ikparam_values {0}".format(IKP_type)
+        if traj.GetConfigurationSpecification().GetGroupFromName(group_name):
+            return True
+    except openravepy.openrave_exception:
+        pass
+    return False
+
+
 def ComputeEnabledAABB(kinbody):
     """
     Returns the AABB of the enabled links of a KinBody.
@@ -1102,6 +1203,82 @@ def ComputeUnitTiming(robot, traj, env=None):
     return new_traj
 
 
+def ComputeGeodesicUnitTiming(traj, env=None, alpha=1.0):
+    """
+    Compute the geodesic unit velocity timing of a workspace path or
+    trajectory, also called a path length parameterization.
+
+    The path length is calculated as the sum of all segment lengths,
+    where each segment length = norm( delta_translation^2 +
+                                           alpha^2*delta_orientation^2 )
+
+    Note: Currently only linear velocity interpolation is supported,
+          however OpenRAVE does allow you to specify quadratic
+          interpolation.
+
+    @param traj: Workspace path or trajectory
+    @param env: Environment to create the output trajectory in, defaults
+                to the same environment as the input trajectory.
+    @param alpha: Weighting for delta orientation.
+    @returns: A workspace trajectory with unit velocity timing.
+    """
+    if not IsTrajectoryTypeIkParameterizationTransform6D(traj):
+        raise ValueError("Trajectory is not a workspace trajectory, it "
+                         "must have configuration specification of "
+                         "openravepy.IkParameterizationType.Transform6D")
+
+    num_waypoints = traj.GetNumWaypoints()
+    if num_waypoints <= 1:
+        raise ValueError("Trajectory needs more than 1 waypoint.")
+
+    from openravepy import RaveCreateTrajectory
+
+    if env is None:
+        env = traj.GetEnv()
+
+    # Create a new workspace trajectory with the same spec
+    # as the old one
+    new_traj = openravepy.RaveCreateTrajectory(env, '')
+    new_cspec = openravepy.IkParameterization.\
+                        GetConfigurationSpecificationFromType(
+                          openravepy.IkParameterizationType.Transform6D,
+                          'linear')
+    new_cspec.AddDeltaTimeGroup()
+    new_traj.Init(new_cspec)
+
+    # Get the current pose of the end effector
+    # Note: OpenRAVE pose is [qx,qy,qz,qw, tx,ty,tz]
+    #       The 8th value is velocity.
+    P_ee_prev = traj.GetWaypoint(0)[range(7)]
+
+    for i in range(num_waypoints):
+        P_ee = traj.GetWaypoint(i)[range(7)] # Get 7 pose values
+
+        # Compute the translation delta
+        p0 = P_ee_prev[4:7] # Get the x,y,z translation
+        p1 = P_ee[4:7]
+        delta_translation = numpy.sqrt(numpy.sum((p0-p1)**2))
+
+        # Compute the orientation delta
+        q0 = P_ee_prev[0:4] # Get qx,qy,qz,qw rotation
+        q1 = P_ee[0:4]
+        delta_angle = AngleBetweenQuaternions(q0, q1)
+
+        dist = numpy.sqrt( delta_translation**2 + (alpha**2)*(delta_angle**2) )
+        if i == 0:
+            deltatime = 0.0
+        else:
+            deltatime = dist
+
+        P_ee_prev = P_ee
+
+        # Insert a new waypoint (1x7 pose, velocity)
+        values = numpy.append(P_ee, [deltatime])
+        new_traj.Insert(i, values)
+
+    return new_traj
+
+
 def CheckJointLimits(robot, q):
     """
     Check if a configuration is within a robot's joint position limits.