Merge pull request #210 from DavidB-CMU/master

Added helper functions to check if the robot is at a goal configuration.

Author: mkoval

src/prpy/util.py

@@ -680,77 +680,141 @@ def FindCatkinResource(package, relative_path):
                       relative_path))
 
 
-def IsAtTrajectoryStart(robot, trajectory):
+def IsAtTrajectoryWaypoint(robot, trajectory, waypoint_idx):
     """
-    Check if robot's DOFs match the start configuration of a trajectory.
-
-    This function examines the current DOF values of the specified robot and
-    compares these values to the first waypoint of the specified trajectory.
-    If every DOF value specified in the trajectory differs by less than the
-    DOF resolution of the specified joint/axis then it will return True.
-    Otherwise, it returns False.
-
-    @param robot: the robot whose active DOFs will be checked
-    @param trajectory: the trajectory whose start configuration will be checked
-    @returns: True if the robot's active DOFs match the given trajectory
-              False if one or more active DOFs differ by DOF resolution
+    Check if robot is at a particular waypoint in a trajectory.
+
+    This function examines the current DOF values of the specified
+    robot and compares these values to the first waypoint of the
+    specified trajectory. If the DOF values specified in the trajectory
+    differ by less than the DOF resolution of the specified joint/axis
+    then it will return True. Otherwise, it returns False.
+
+    NOTE: This is used in ExecuteTrajectory(),
+                            IsAtTrajectoryStart(), and
+                              IsAtTrajectoryEnd()
+
+    @param robot: The robot whose active DOFs will be checked.
+    @param trajectory: The trajectory containing the waypoint
+                       to be checked.
+    @returns: True The robot is at the desired position.
+              False One or more joints differ by DOF resolution.
     """
+    if trajectory.GetNumWaypoints() == 0:
+        raise ValueError('Trajectory has 0 waypoints!')
+
     cspec = trajectory.GetConfigurationSpecification()
     needs_base = HasAffineDOFs(cspec)
     needs_joints = HasJointDOFs(cspec)
 
     if needs_base and needs_joints:
-        raise ValueError('Trajectories with affine and joint DOFs are not supported')
+        raise ValueError('Trajectories with affine and joint DOFs are '
+                         'not supported')
 
     if trajectory.GetEnv() != robot.GetEnv():
-        raise ValueError('The environment attached to the trajectory does not match the environment attached to the robot')
-
+        raise ValueError('The environment attached to the trajectory '
+                         'does not match the environment attached to '
+                         'the robot in IsAtTrajectoryStart().')
     if needs_base:
-        doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
-        current_pose = openravepy.RaveGetAffineDOFValuesFromTransform(robot.GetTransform(), doft)
+        rtf = robot.GetTransform()
+        doft = openravepy.DOFAffine.X | \
+               openravepy.DOFAffine.Y | \
+               openravepy.DOFAffine.RotationAxis
+        curr_pose = openravepy.RaveGetAffineDOFValuesFromTransform(rtf, doft)
         start_transform = numpy.eye(4)
-        start_transform = cspec.ExtractTransform(start_transform, trajectory.GetWaypoint(0), robot)
-        traj_start = openravepy.RaveGetAffineDOFValuesFromTransform(start_transform, doft)
-
+        waypoint = trajectory.GetWaypoint(0)
+        start_t = cspec.ExtractTransform(start_transform, waypoint, robot)
+        traj_start = openravepy.RaveGetAffineDOFValuesFromTransform(start_t, \
+                                                                    doft)
         # Compare translation distance
-        trans_delta_value = abs(current_pose[:2] - traj_start[:2])
+        trans_delta_value = abs(curr_pose[:2] - traj_start[:2])
         trans_resolution = robot.GetAffineTranslationResolution()[:2]
         if trans_delta_value[0] > trans_resolution[0] or \
            trans_delta_value[1] > trans_resolution[1]:
             return False
 
         # Compare rotation distance
-        rot_delta_value = abs(wrap_to_interval(current_pose[2] - traj_start[2]))
-        rot_resolution = robot.GetAffineRotationAxisResolution()[2] # Rotation about z?
-        if rot_delta_value > rot_resolution:
+        rot_delta_value = abs(wrap_to_interval(curr_pose[2] - traj_start[2]))
+        rot_res = robot.GetAffineRotationAxisResolution()[2] # Rot about z?
+        if rot_delta_value > rot_res:
             return False
 
     else:
-        # Get used indices and starting configuration from trajectory.
+        # Get joint indices used in the trajectory,
+        # and the joint positions at this waypoint
+        waypoint = trajectory.GetWaypoint(waypoint_idx)
         dof_indices, _ = cspec.ExtractUsedIndices(robot)
-        traj_values = cspec.ExtractJointValues(
-            trajectory.GetWaypoint(0), robot, dof_indices)
+        goal_config = cspec.ExtractJointValues(waypoint, robot, dof_indices)
+
+        # Return false if any joint deviates too much
+        return IsAtConfiguration(robot, goal_config, dof_indices)
 
-        # Get current configuration of robot for used indices.
-        with robot.GetEnv():
-            robot_values = robot.GetDOFValues(dof_indices)
-            dof_resolutions = robot.GetDOFResolutions(dof_indices)
-            
-        # Check deviation in each DOF, using OpenRAVE's SubtractValue function.
-        dof_infos = zip(dof_indices, traj_values, robot_values, dof_resolutions)
-        for dof_index, traj_value, robot_value, dof_resolution in dof_infos:
-            # Look up the Joint and Axis of the DOF from the robot.
-            joint = robot.GetJointFromDOFIndex(dof_index)
-            axis = dof_index - joint.GetDOFIndex()
-            
-            # If any joint deviates too much, return False.
-            delta_value = abs(joint.SubtractValue(traj_value, robot_value, axis))
-            if delta_value > dof_resolution:
-                return False
-
-    # If all joints match, return True.
     return True
 
+
+def IsAtTrajectoryStart(robot, trajectory):
+    """
+    Check if robot is at the configuration specified by
+    the FIRST waypoint in a trajectory.
+    """
+    waypoint_idx = 0
+    return IsAtTrajectoryWaypoint(robot, trajectory, waypoint_idx)
+
+
+def IsAtTrajectoryEnd(robot, trajectory):
+    """
+    Check if robot is at the configuration specified by
+    the LAST waypoint in a trajectory.
+    """
+    waypoint_idx = trajectory.GetNumWaypoints() - 1
+    return IsAtTrajectoryWaypoint(robot, trajectory, waypoint_idx)
+
+
+def IsAtConfiguration(robot, goal_config, dof_indices=None):
+    """
+    Check if robot's joints have reached a desired configuration.
+
+    If the DOF indices are not specified, the robot's active DOF
+    will be used.
+
+    @param robot The robot object.
+    @param goal_config The desired configuration, an array of joint
+                       positions.
+    @param dof_indices The joint index numbers.
+
+    @return boolean Returns True if joints are at goal position,
+                    within DOF resolution.
+    """
+
+    # If DOF indices not specified, use the active DOF by default
+    if dof_indices == None:
+        dof_indices = robot.GetActiveDOFIndices()
+
+    # Get current position of joints
+    with robot.GetEnv():
+        joint_values = robot.GetDOFValues(dof_indices)
+        dof_resolutions = robot.GetDOFResolutions(dof_indices)
+
+    ## If any joint is not at the goal position, return False
+    for i in xrange(0, len(goal_config)):
+        # Get the axis index for this joint, which is 0
+        # for revolute joints or 0-2 for spherical joints.
+        joint = robot.GetJointFromDOFIndex(dof_indices[i])
+        axis_idx = dof_indices[i] - joint.GetDOFIndex()
+        # Use OpenRAVE method to check the configuration
+        # difference value1-value2 for axis i,
+        # taking into account joint limits and wrapping
+        # of continuous joints.
+        delta_value = abs(joint.SubtractValue(joint_values[i], \
+                                              goal_config[i], \
+                                              axis_idx))
+        if delta_value > dof_resolutions[i]:
+            return False
+
+    # If all joints match the goal, return True
+    return True
+
+
 def IsTimedTrajectory(trajectory):
     """
     Returns True if the trajectory is timed.

tests/test_util.py

@@ -1,11 +1,78 @@
+from __future__ import print_function
 import openravepy
 import prpy.util
 import unittest
 
-class IsTimedTrajectoryTests(unittest.TestCase):
+import os # environ, path
+import subprocess
+import sys # stderr
+
+import numpy # allclose, zeros
+
+
+# Add the models included with OpenRAVE to the OPENRAVE_DATA path.
+# These may not be available if the user manually set the OPENRAVE_DATA
+# environmental variable, e.g. through openrave_catkin.
+try:
+    share_path = \
+          subprocess.check_output(['openrave-config', '--share-dir']).strip()
+    os.environ['OPENRAVE_DATA'] = os.path.join(share_path, 'data')
+except subprocess.CalledProcessError as e:
+    print('error: Failed using "openrave-config" to find the default'
+          ' OPENRAVE_DATA path. Loading assets may fail.',
+          file=sys.stderr)
+
+# Initialize OpenRAVE.
+openravepy.RaveInitialize(True)
+openravepy.misc.InitOpenRAVELogging()
+openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal)
+
+
+class TrajectoryTests(unittest.TestCase):
     def setUp(self):
         self.env = openravepy.Environment()
+        self.env.Load('wamtest1.env.xml')
         self.traj = openravepy.RaveCreateTrajectory(self.env, '')
+        self.robot = self.env.GetRobot('BarrettWAM')
+        self.manipulator = self.robot.GetManipulator('arm')
+
+        # Set all 7 DOF of the WAM arm to active
+        with self.env:
+            self.robot.SetActiveDOFs(self.manipulator.GetArmIndices())
+            self.robot.SetActiveManipulator(self.manipulator)
+            self.active_dof_indices = self.robot.GetActiveDOFIndices()
+
+
+    def CreateTrajectory(self, q_start, q_goal):
+        """
+        Create a trajectory between two joint configurations.
+        (a straight line in joint space)
+        """
+        env = self.env
+        robot = self.robot
+        dof_indices = self.active_dof_indices
+
+        traj = openravepy.RaveCreateTrajectory(env, '')
+        cspec = robot.GetActiveConfigurationSpecification('linear')
+
+        # Add first waypoint
+        start_waypoint = numpy.zeros(cspec.GetDOF())
+        cspec.InsertJointValues(start_waypoint, q_start, robot, \
+                                dof_indices, False)
+        traj.Init(cspec)
+        traj.Insert(0, start_waypoint.ravel())
+
+        # Add second waypoint, if different to first
+        if not numpy.allclose(q_start, q_goal):
+            goal_waypoint = numpy.zeros(cspec.GetDOF())
+            cspec.InsertJointValues(goal_waypoint, q_goal, robot, \
+                                    dof_indices, False)
+            traj.Insert(1, goal_waypoint.ravel())
+
+        return traj
+
+
+    # IsTimedTrajectory()
 
     def test_IsTimed_ReturnsTrue(self):
         cspec = openravepy.ConfigurationSpecification()
@@ -26,3 +93,57 @@ def test_IsNotTimed_ReturnsFalse(self):
 
         self.traj.Insert(0, [0.])
         self.assertFalse(prpy.util.IsTimedTrajectory(self.traj))
+
+
+    # IsAtConfiguration()
+
+    def test_IsAtConfiguration_ReturnsTrue(self):
+        curr_config = self.robot.GetDOFValues(self.active_dof_indices)
+        self.assertTrue(prpy.util.IsAtConfiguration(self.robot, curr_config))
+
+    def test_IsAtConfiguration_ReturnsFalse(self):
+        goal_config = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0];
+        self.assertFalse(prpy.util.IsAtConfiguration(self.robot, goal_config))
+
+
+    # IsAtTrajectoryStart()
+
+    def test_IsAtTrajectoryStart_ReturnsTrue(self):
+        current_config = self.robot.GetDOFValues(self.active_dof_indices)
+        goal_config = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0];
+        q0 = current_config
+        q1 = goal_config
+        traj = self.CreateTrajectory(q0, q1)
+        self.assertTrue(prpy.util.IsAtTrajectoryStart(self.robot, traj))
+
+        # Test with only 1 waypoint
+        traj = self.CreateTrajectory(q0, q0)
+        self.assertTrue(prpy.util.IsAtTrajectoryStart(self.robot, traj))
+
+    def test_IsAtTrajectoryStart_ReturnsFalse(self):
+        current_config = self.robot.GetDOFValues(self.active_dof_indices)
+        goal_config = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0];
+        q0 = current_config
+        q1 = goal_config
+        traj = self.CreateTrajectory(q1, q0) # goal is 1st waypoint
+        self.assertFalse(prpy.util.IsAtTrajectoryStart(self.robot, traj))
+
+
+    # IsAtTrajectoryEnd()
+
+    def test_IsAtTrajectoryEnd_ReturnsTrue(self):
+        current_config = self.robot.GetDOFValues(self.active_dof_indices)
+        goal_config = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0];
+        q0 = current_config
+        q1 = goal_config
+        # set last waypoint to current config
+        traj = self.CreateTrajectory(q1, q0)
+        self.assertTrue(prpy.util.IsAtTrajectoryEnd(self.robot, traj))
+
+    def test_IsAtTrajectoryEnd_ReturnsFalse(self):
+        current_config = self.robot.GetDOFValues(self.active_dof_indices)
+        goal_config = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0];
+        q0 = current_config
+        q1 = goal_config
+        traj = self.CreateTrajectory(q0, q1)
+        self.assertFalse(prpy.util.IsAtTrajectoryEnd(self.robot, traj))