Merge pull request #167 from open-dynamic-robot-initiative/fkloss/min…

…_jerk_and_self_test

Add min_jerk_trajectory() and improve self-test

CHANGELOG.md

@@ -11,6 +11,7 @@
   if a lens comes loose.
 - 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.
 
 ### Changed
 - Upgrade to robot_interfaces v2 (not yet released).
@@ -37,6 +38,8 @@
 - `trifinger_backend`: Use proper timeseries history length if no limit for
   number of actions is specified.
 - pybind11 build error on Ubuntu 22.04
+- Improve the push sensor test in `trifinger_post_submission` to avoid frequent
+  false positives.
 
 
 ## [1.0.0] - 2022-06-28

demos/demo_trifingerpro.py

@@ -8,33 +8,52 @@
 
 import robot_interfaces
 import robot_fingers
+from robot_fingers.utils import min_jerk_trajectory
 
 
 def run_choreography(frontend):
     """Move the legs in some hard-coded choreography."""
 
-    def perform_step(position):
+    def perform_step(start, goal):
         # one step should take 1 second, so repeat action 1000 times
-        for _ in range(1000):
+        STEPS = 1000
+
+        # use min_jerk_trajectory to generate smooth trajectories (we could also just
+        # use `Action(position=goal)` and append that for several steps until the goal
+        # is reached but it would result in harsher movements).
+        for step_position in min_jerk_trajectory(start, goal, STEPS):
             t = frontend.append_desired_action(
-                robot_interfaces.trifinger.Action(position=position)
+                robot_interfaces.trifinger.Action(position=step_position)
             )
             frontend.wait_until_timeindex(t)
 
-    pose_initial = [0, 0.9, -1.7]
-    pose_intermediate = [0.75, 1.2, -2.3]
-    pose_up = [1.3, 1.5, -2.6]
-    pose_other_side_up = [-0.8, 1.5, -1.7]
+        # return final position
+        observation = frontend.get_observation(t)
+        return observation.position
+
+    # Desired joint positions for the steps.  Specify for a single finger and multiply
+    # by 3, so we get a list of length 9 with identical joint positions for all three
+    # fingers.
+    pose_initial = [0, 0.9, -1.7] * 3
+    pose_intermediate = [0.75, 1.2, -2.3] * 3
+    pose_up = [1.3, 1.5, -2.6] * 3
+    pose_other_side_up = [-0.8, 1.5, -1.7] * 3
 
     time_printer = robot_fingers.utils.TimePrinter()
 
+    # send a zero-torque action to start the backend loop, so we can get the initial
+    # position
+    t = frontend.append_desired_action(robot_interfaces.trifinger.Action())
+    observation = frontend.get_observation(t)
+    current_position = observation.position
+
     while True:
         # initial pose
-        perform_step(pose_initial * 3)
-        perform_step(pose_intermediate * 3)
-        perform_step(pose_up * 3)
-        perform_step(pose_initial * 3)
-        perform_step(pose_other_side_up * 3)
+        current_position = perform_step(current_position, pose_initial)
+        current_position = perform_step(current_position, pose_intermediate)
+        current_position = perform_step(current_position, pose_up)
+        current_position = perform_step(current_position, pose_initial)
+        current_position = perform_step(current_position, pose_other_side_up)
 
         # print current date/time every hour, so we can roughly see how long it
         # ran in case it crashes during a long-run-test.

pyproject.toml

@@ -14,6 +14,7 @@ module = [
     "pinocchio",
     "plotext",
     "progressbar",
+    "pytest",
     "rclpy.*",
     "scipy.*",
     "setuptools",
@@ -36,3 +37,45 @@ module = [
 ]
 ignore_missing_imports = true
 
+
+[tool.ruff]
+# enable all and disable a few via ignore below
+select = ["ALL"]
+extend-ignore = [
+    "BLE",
+    "COM",
+    "EM",
+    "FBT",
+    "INP",
+    "PTH",
+    "S",
+    "T20",
+    "UP",
+    "A003",
+    "ANN101",
+    "ANN102",
+    "ANN401",
+    "D105",
+    "D107",
+    "D205",
+    "D212",
+    "D417",
+    "G004",
+    "I001",
+    "N806",
+    "PLR0913",
+    "PTH123",
+    "RET505",
+    "TRY003",
+    "TRY400",
+]
+target-version = "py310"
+
+[tool.ruff.pydocstyle]
+convention = "google"
+
+[tool.ruff.per-file-ignores]
+"*.pyi" = ["ALL"]
+"__init__.py" = ["F401"]  # unused imports
+"test/*" = ["D", "ANN", "PLR2004"]
+"scripts/*" = ["D"]

robot_fingers/utils.py

@@ -1,12 +1,15 @@
+"""Utility classes/functions for the robot_fingers package."""
 import time
+import typing as t
+
+import numpy as np
+import numpy.typing as npt
 
 
 class TimePrinter:
-    """
-    Regularly print the current date/time and the passed duration in hours.
-    """
+    """Regularly print the current date/time and the passed duration in hours."""
 
-    def __init__(self, interval_s: float = 3600.0):
+    def __init__(self, interval_s: float = 3600.0) -> None:
         """Initialise.
 
         Args:
@@ -20,7 +23,7 @@ def __init__(self, interval_s: float = 3600.0):
 
         self._last_time_print = 0.0
 
-    def update(self):
+    def update(self) -> None:
         """Print time if the interval_s has passed since the last call."""
         now = time.time()
         if now - self._last_time_print > self.interval_s:
@@ -30,3 +33,36 @@ def update(self):
                 "{} ({} h since {})".format(time_str, duration_h, self.start_time_str)
             )
             self._last_time_print = now
+
+
+def min_jerk_trajectory(
+    start_position: npt.ArrayLike, end_position: npt.ArrayLike, num_steps: int
+) -> t.Iterator[npt.NDArray]:
+    """Generator for computing minimum jerk trajectories.
+
+    Example:
+        To move from ``[0, 0, 0]`` to ``[1, 2, 3]`` over 1000 time steps:
+
+        .. code-block:: Python
+
+            for pos in min_jerk_trajectory([0, 0, 0], [1, 2, 3], 1000):
+                robot.append_desired_action(Action(position=pos)
+
+    Args:
+        start_position: Joint positions where the trajectory starts.
+        end_position: Joint positions where the trajectory ends.
+        num_steps: Number of steps for the trajectory.
+    """
+    if num_steps < 1:
+        raise ValueError("num_steps must be >= 1")
+
+    start_position = np.asarray(start_position)
+    end_position = np.asarray(end_position)
+    position_delta = end_position - start_position
+
+    for i in range(num_steps):
+        alpha = i / num_steps
+        step_position = start_position + position_delta * (
+            10.0 * alpha**3 - 15.0 * alpha**4 + 6.0 * alpha**5
+        )
+        yield step_position