Merge pull request #169 from open-dynamic-robot-initiative/fkloss/pos…

…tsub

Some improvements of trifingerpro_post_submission

CHANGELOG.md

@@ -12,6 +12,9 @@
 - Check camera brightness in `trifinger_post_submission.py` to auto-detect
   broken light panels.
 - Python generator `utils.min_jerk_trajectory` for arbitrary min. jerk trajectories.
+- Add flags to `trifinger_post_submission.py` to make push sensor test fatal (disabled
+  by default) and to set the threshold for the test.
+- Add flag to `trifinger_post_submission.py` to skip camera tests.
 
 ### Changed
 - Upgrade to robot_interfaces v2 (not yet released).

pyproject.toml

@@ -67,6 +67,7 @@ extend-ignore = [
     "PLR0913",
     "PTH123",
     "RET505",
+    "SIM108",
     "TRY003",
     "TRY400",
 ]

scripts/trifingerpro_post_submission.py

@@ -192,7 +192,106 @@ def end_stop_check(robot: robot_fingers.Robot, log: logging.Logger) -> None:
         )
 
 
-def run_self_test(robot: robot_fingers.Robot, log: logging.Logger) -> None:
+def _move(
+    robot: robot_fingers.Robot,
+    initial_position: npt.NDArray,
+    goal_position: npt.NDArray,
+    steps: int = 700,
+) -> robot_interfaces.trifinger.Observation:
+    """Move from initial to goal position on a smooth trajectory.
+
+    Returns:
+        Robot observation from last step.
+    """
+    for _position in min_jerk_trajectory(initial_position, goal_position, steps):
+        action = robot.Action(position=_position)
+        t = robot.frontend.append_desired_action(action)
+        robot.frontend.wait_until_timeindex(t)
+    return robot.frontend.get_observation(t)
+
+
+def _push_sensor_test(
+    log: logging.Logger,
+    current_tip_force: npt.NDArray,
+    no_contact_reference: npt.NDArray,
+    desired_position: npt.NDArray,
+    actual_position: npt.NDArray,
+    *,
+    expect_contact: bool,
+    log_error: bool,
+    push_sensor_contact_delta_threshold: float = 0.1,
+) -> bool:
+    """Test the push sensors.
+
+    This test checks if either all finger tip push sensors report a contact
+    (expect_contact=True) or all of them report no contact (expect_contact=False).
+
+    Args:
+        log: Logger instance for logging the results.
+        current_tip_force: Measured tip forces of all fingers.
+        no_contact_reference: Reference value with tip force of all fingers if there is
+            no contact.
+        desired_position: Desired joint positions (only for logging in case of failure).
+        actual_position: Actual joint positions (only for logging in case of failure).
+        expect_contact: Whether contact is expected or not in the current situation.
+        log_error: Whether to log a failure as error (otherwise it will be logged as
+            warning).
+        push_sensor_contact_delta_threshold: Threshold on difference between current tip
+            force and non-contact reference; used to determine contact.
+
+    Returns:
+        True if test passed, False if not.
+    """
+    # we assume a contact if the difference between current values and no-contact
+    # reference is greater than the threshold
+    reference_delta = current_tip_force - no_contact_reference
+    above_threshold = reference_delta > push_sensor_contact_delta_threshold
+    # for this test all fingers should match the expected state (i.e. contact or not)
+    if expect_contact:
+        passed = all(above_threshold)
+    else:
+        passed = not any(above_threshold)
+
+    if passed:
+        if expect_contact:
+            msg = "Push sensor test passed (contact)."
+        else:
+            msg = "Push sensor test passed (non-contact)."
+        log.info(
+            SM(
+                msg,
+                sensor_value=current_tip_force,
+                no_contact_reference=no_contact_reference,
+                threshold=push_sensor_contact_delta_threshold,
+            )
+        )
+    else:
+        if expect_contact:
+            msg = "Push sensor reports low value in contact situation."
+        else:
+            msg = "Push sensor reports high value in non-contact situation."
+        _log_meth = log.error if log_error else log.warning
+        _log_meth(
+            SM(
+                msg,
+                sensor_value=current_tip_force,
+                no_contact_reference=no_contact_reference,
+                threshold=push_sensor_contact_delta_threshold,
+                desired_position=desired_position,
+                actual_position=actual_position,
+            )
+        )
+
+    return passed
+
+
+def run_self_test(
+    robot: robot_fingers.Robot,
+    log: logging.Logger,
+    *,
+    fatal_push_sensor_test: bool,
+    push_sensor_threshold: float,
+) -> None:
     position_tolerance = 0.2
 
     def _fail_if_position_not_reached(goal: npt.NDArray, actual: npt.NDArray) -> None:
@@ -204,14 +303,6 @@ def _fail_if_position_not_reached(goal: npt.NDArray, actual: npt.NDArray) -> Non
                 actual_position=observation.position,
             )
 
-    def _has_tip_sensor_contact(
-        current_tip_force: npt.NDArray,
-        no_contact_reference: npt.NDArray,
-    ) -> bool:
-        contact_delta_threshold = 0.1
-        reference_delta = current_tip_force - no_contact_reference
-        return any(reference_delta > contact_delta_threshold)
-
     initial_pose = np.array([0.0, 1.1, -1.9] * 3)
 
     reachable_goals = np.array(
@@ -239,38 +330,31 @@ def _has_tip_sensor_contact(
     no_contact_tip_force = observation.tip_force
 
     for goal in reachable_goals:
-        for _position in min_jerk_trajectory(observation.position, goal, 700):
-            action = robot.Action(position=_position)
-            t = robot.frontend.append_desired_action(action)
-            robot.frontend.wait_until_timeindex(t)
-        observation = robot.frontend.get_observation(t)
+        observation = _move(robot, observation.position, goal)
 
         # verify that goal is reached
         _fail_if_position_not_reached(goal, observation.position)
 
-        if _has_tip_sensor_contact(observation.tip_force, no_contact_tip_force):
-            fail(
+        if (
+            not _push_sensor_test(
                 log,
-                "Push sensor reports high value in non-contact situation.",
-                sensor_value=observation.tip_force,
-                no_contact_reference=no_contact_tip_force,
-                desired_position=goal,
-                actual_position=observation.position,
+                observation.tip_force,
+                no_contact_tip_force,
+                goal,
+                observation.position,
+                expect_contact=False,
+                log_error=fatal_push_sensor_test,
+                push_sensor_contact_delta_threshold=push_sensor_threshold,
             )
+            and fatal_push_sensor_test
+        ):
+            sys.exit(1)
 
     for goal in unreachable_goals:
         # move to initial position first
-        for _position in min_jerk_trajectory(observation.position, initial_pose, 700):
-            action = robot.Action(position=_position)
-            t = robot.frontend.append_desired_action(action)
-            robot.frontend.wait_until_timeindex(t)
-        observation = robot.frontend.get_observation(t)
-
-        for _position in min_jerk_trajectory(observation.position, goal, 700):
-            action = robot.Action(position=_position)
-            t = robot.frontend.append_desired_action(action)
-            robot.frontend.wait_until_timeindex(t)
-        observation = robot.frontend.get_observation(t)
+        observation = _move(robot, observation.position, initial_pose)
+        # now try to move to the unreachable goal
+        observation = _move(robot, observation.position, goal)
 
         # verify that goal is not reached
         if np.linalg.norm(goal - observation.position) < position_tolerance:
@@ -281,15 +365,20 @@ def _has_tip_sensor_contact(
                 actual_position=observation.position,
             )
 
-        if not _has_tip_sensor_contact(observation.tip_force, no_contact_tip_force):
-            fail(
+        if (
+            not _push_sensor_test(
                 log,
-                "Push sensor reports low value in contact situation.",
-                sensor_value=observation.tip_force,
-                no_contact_reference=no_contact_tip_force,
-                desired_position=goal,
-                actual_position=observation.position,
+                observation.tip_force,
+                no_contact_tip_force,
+                goal,
+                observation.position,
+                expect_contact=True,
+                log_error=fatal_push_sensor_test,
+                push_sensor_contact_delta_threshold=push_sensor_threshold,
             )
+            and fatal_push_sensor_test
+        ):
+            sys.exit(1)
 
     print("Test successful.")
 
@@ -320,10 +409,7 @@ def reset_object(robot, trajectory_file):
     # move to start position with a smooth trajectory
     t = robot.frontend.get_current_timeindex()
     observation = robot.frontend.get_observation(t)
-    for _position in min_jerk_trajectory(observation.position, positions[0], 700):
-        action = robot.Action(position=_position)
-        t = robot.frontend.append_desired_action(action)
-        robot.frontend.wait_until_timeindex(t)
+    _move(robot, observation.position, positions[0])
 
     for position in positions:
         action = robot.Action(position=position)
@@ -631,6 +717,26 @@ def main():
         action="store_true",
         help="Skip the robot self-test.",
     )
+    parser.add_argument(
+        "--skip-camera-test",
+        action="store_true",
+        help="Skip the camera self-test.",
+    )
+    parser.add_argument(
+        "--fatal-push-sensor-test",
+        action="store_true",
+        help="""Fail self-test if the push-sensor test fails.  This test has a high
+            probability of false positives and is therefore disabled by default.  If not
+            enabled, the test is still performed but a failure will only be reported as
+            warning and not result in failing the self-test.
+        """,
+    )
+    parser.add_argument(
+        "--push-sensor-threshold",
+        type=float,
+        default=0.1,
+        help="Threshold for the push sensor test.",
+    )
     args = parser.parse_args()
 
     # configure logger
@@ -674,7 +780,12 @@ def main():
         print("End stop test")
         end_stop_check(robot, logging.getLogger("end_stop_test"))
         print("Position reachability test")
-        run_self_test(robot, logging.getLogger("self_test"))
+        run_self_test(
+            robot,
+            logging.getLogger("self_test"),
+            fatal_push_sensor_test=args.fatal_push_sensor_test,
+            push_sensor_threshold=args.push_sensor_threshold,
+        )
 
     if args.reset:
         if args.object == "cube":
@@ -690,28 +801,31 @@ def main():
     # terminate the robot
     del robot
 
-    print("Check cameras")
-    camera_observations = record_camera_observations(args.object, num_observations=30)
-
-    if not check_camera_brightness(
-        camera_observations, logging.getLogger("camera_brightness")
-    ):
-        sys.exit(2)
-
-    if not check_camera_sharpness(
-        camera_observations, logging.getLogger("camera_sharpness")
-    ):
-        sys.exit(2)
-
-    if args.object in ["cube", "cuboid"]:
-        print("Check object detection")
-        if not check_object_detection_noise(
-            args.object,
-            camera_observations,
-            logging.getLogger("object_detection"),
+    if not args.skip_camera_test:
+        print("Check cameras")
+        camera_observations = record_camera_observations(
+            args.object, num_observations=30
+        )
+
+        if not check_camera_brightness(
+            camera_observations, logging.getLogger("camera_brightness")
         ):
             sys.exit(2)
 
+        if not check_camera_sharpness(
+            camera_observations, logging.getLogger("camera_sharpness")
+        ):
+            sys.exit(2)
+
+        if args.object in ["cube", "cuboid"]:
+            print("Check object detection")
+            if not check_object_detection_noise(
+                args.object,
+                camera_observations,
+                logging.getLogger("object_detection"),
+            ):
+                sys.exit(2)
+
 
 if __name__ == "__main__":
     main()