Fix acceptancetest_joint_limits (#448)

Co-authored-by: Alexander Gutenkunst <[email protected]>

prbt_support/test/acceptance_tests/acceptance_test_joint_limits.md

@@ -17,12 +17,12 @@ limitations under the License.
 # Acceptance Test Joint Limits using the real robot
 
 ## Prerequisites
-  - Properly connect and startup the robot. Make sure an emergency stop is within reach.
+  - Properly connect and startup the robot. Make sure an emergency stop is within reach and the operation mode is T1.
   - Note that the robot will try to move exactly to the limits of each joint. Make sure that collisions are avoided or
     skip the test for certain joints.
 
 ## Starting the joint limit acceptance tests
-  - Press the enabling switch and run `roslaunch prbt_support robot.launch`.
+  - Run `roslaunch prbt_support robot.launch`. Once the startup is complete press the enabling switch.
   - Run `rosrun prbt_support acceptance_test_joint_limits.py`.
   - Please note: Each test must be confirmed before it is executed. Confirm by entering `y` or `n` to skip test.
 

prbt_support/test/acceptance_tests/acceptance_test_joint_limits.py

@@ -31,6 +31,7 @@
 _WAIT_FOR_MESSAGE_TIMEOUT_SEC = 1
 
 # Use axis ranges from data sheet: https://www.pilz.com/download/open/PRBT_6_Operat_Manual_1004685-EN-02.pdf (page 19)
+_JOINT_NAMES = ['prbt_joint_1', 'prbt_joint_2', 'prbt_joint_3', 'prbt_joint_4', 'prbt_joint_5', 'prbt_joint_6']
 _JOINT_LIMITS_DEGREE = {
     'prbt_joint_1': 170,
     'prbt_joint_2': 145,
@@ -42,7 +43,12 @@
 _JOINT_POSITIONS_TOLERANCE = 0.001
 _VELOCITY_SCALE = 0.2
 _JOINT_LIMIT_OVERSTEP = 0.1
-_SELF_COLLISION_JOINT_NAME = 'prbt_joint_5'
+_COLLISION_JOINT_NAMES = [
+                          # Collision if mounted on a table
+                          'prbt_joint_2', \
+                          # Self-Collision if gripper is mounted
+                          'prbt_joint_5'  \
+                          ]
 
 
 class AcceptanceTestJointLimits(unittest.TestCase):
@@ -56,7 +62,7 @@ def setUp(self):
         if not self.client.wait_for_server(timeout=rospy.Duration(_ACTION_SERVER_TIMEOUT_SEC)):
             self.fail('Timed out waiting for action server ' + _CONTROLLER_ACTION_NAME)
 
-        self.joint_names = sorted(_JOINT_LIMITS_DEGREE.keys())
+        self.joint_names = _JOINT_NAMES
         self.home_positions = [0] * len(self.joint_names)
 
         # read joint limits from urdf
@@ -217,13 +223,13 @@ def _joint_limit_overstepping_test(self, joint_name):
         lower_positions = [0] * len(self.joint_names)
         lower_positions[index] = -(radians(limit) + _JOINT_LIMIT_OVERSTEP)
 
-        self.assertFalse(self._execute_trajectory(lower_positions))
+        self._execute_trajectory(lower_positions)  # Controller does not abort trajectory
         self._check_joint_limits()
 
         upper_positions = [0] * len(self.joint_names)
         upper_positions[index] = radians(limit) + _JOINT_LIMIT_OVERSTEP
 
-        self.assertFalse(self._execute_trajectory(upper_positions))
+        self._execute_trajectory(upper_positions)  # Controller does not abort trajectory
         self._check_joint_limits()
 
         self._drive_home()
@@ -232,15 +238,15 @@ def test_joint_limits_reaching(self):
         """ Perform all reaching tests. Before each test ask the user if he wants to skip it.
         """
         for name in self.joint_names:
-            if not name == _SELF_COLLISION_JOINT_NAME:
+            if name not in _COLLISION_JOINT_NAMES:
                 if self._ask_for_permission('joint_limit_reaching_test for ' + name):
                     self._joint_limit_reaching_test(name)
 
     def test_joint_limits_overstepping(self):
         """ Perform all overstepping tests. Before each test ask the user if he wants to skip it.
         """
         for name in self.joint_names:
-            if not name == _SELF_COLLISION_JOINT_NAME:
+            if name not in _COLLISION_JOINT_NAMES:
                 if self._ask_for_permission('joint_limit_overstepping_test for ' + name):
                     self._joint_limit_overstepping_test(name)