feat(iiwa): add an iiwa14 config

crisp_py/config/robots/iiwa14_r820.yaml

@@ -0,0 +1,25 @@
+# KUKA LBR iiwa 14 R820 driven through lbr_fri_ros2_stack.
+#
+# The stack's default robot_name is "lbr", which is both the ROS namespace and the
+# prefix on every joint and link name. Change all of them together if the bringup
+# uses a different robot_name.
+#
+# Note for anyone driving this arm through KUKA's Fast Robot Interface: FRI's torque
+# mode expects a joint position command and a torque overlay in the same packet, so a
+# controller claiming only the effort interfaces is not sufficient on its own. The
+# position half has to be supplied by another active controller.
+namespace: lbr
+
+robot_config:
+  robot_type: iiwa
+  base_frame: lbr_link_0
+  target_frame: lbr_link_ee
+
+  # Torque overlays are written by the Cartesian controller; the joint-space one is
+  # a CartesianController with zero task gains, so it is named separately.
+  default_controller: cartesian_impedance_controller
+  cartesian_impedance_controller_name: cartesian_impedance_controller
+  joint_trajectory_controller_name: joint_impedance_controller
+
+  publish_frequency: 50.0
+  time_to_home: 10.0

crisp_py/robot/robot_config.py

@@ -202,8 +202,18 @@ class KinovaConfig(RobotConfig):
 class IiwaConfig(RobotConfig):
     """Configuration specific to KUKA Iiwa robots.
 
-    Provides default values for frame names, joint names, and home configuration
-    specifically for Iiwa robots.
+    Defaults follow the naming used by lbr_fri_ros2_stack, which is the common ROS 2
+    driver for these arms, with its default robot_name of "lbr".
+
+    The home configuration is [0, 30, 0, -75, 0, 75, 0] degrees. It is chosen for the
+    iiwa14 R820 and validated on hardware: every joint sits at least 45 degrees from
+    its limit, and the manipulability measure sqrt(det(J J^T)) is 0.126.
+
+    Two comparisons worth keeping in mind when changing it. The Franka home pose that
+    the other configurations in this module use puts A4 at -135 degrees, which is
+    outside the +/-120 degree A4 limit of every iiwa in the range, so it cannot be
+    commanded. The all-zeros candle pose is fully singular, manipulability exactly 0,
+    which makes it a poor place to start a Cartesian impedance controller.
     """
 
     joint_names: list = field(
@@ -218,17 +228,11 @@ class IiwaConfig(RobotConfig):
         ]
     )
     home_config: list = field(
-        default_factory=lambda: [
-            np.pi / 2,
-            -np.pi / 4,
-            0,
-            -3 * np.pi / 4,
-            0,
-            np.pi / 2,
-            np.pi / 4,
-        ]
+        default_factory=lambda: list(np.deg2rad([0.0, 30.0, 0.0, -75.0, 0.0, 75.0, 0.0]))
     )
-    base_frame: str = "world"
+    # lbr_fri_ros2_stack publishes lbr_link_0 as the root of the arm and does not
+    # publish a "world" frame, so a target pose expressed in "world" has no transform.
+    base_frame: str = "lbr_link_0"
     target_frame: str = "lbr_link_ee"
 
 

examples/example_with_iiwa.py

@@ -1,79 +1,161 @@
-import numpy as np
-from scipy.spatial.transform import Rotation
+"""Follow a figure eight on a KUKA LBR iiwa driven through lbr_fri_ros2_stack.
+
+Start the bringup first, with the arm in a test mode and the enabling switch held.
+
+Unlike the numbered examples this one loads no profile from config/control. Those are
+tuned for a Franka, and this arm is not one: FRI applies the torque overlay on top of
+the cabinet's own joint impedance controller, so the achievable task stiffness is
+bounded by that, and the model compensation is already being done underneath. Leave
+the controller with whatever the bringup configured.
+"""
 
 # %%
-from crisp_py.robot import Pose, Robot
-from crisp_py.robot import IiwaConfig
+import matplotlib.pyplot as plt
+import numpy as np
+
+from crisp_py.robot import Robot, make_robot
 
-robot = Robot(robot_config=IiwaConfig())
+robot = make_robot("iiwa14_r820")
 robot.wait_until_ready()
 
 # %%
 print(robot.end_effector_pose)
+print(robot.joint_values)
 
-# %%
-print("Going to home position...")
-robot.home()
 
 # %%
+def home(robot: Robot) -> None:
+    """Home the arm, keeping the torque overlay owned throughout.
+
+    robot.home() cannot be used here. It switches through the stock switcher, which
+    deactivates every active controller not ending in "broadcaster", and offers no way
+    to exempt one.
+
+    That matters because command interface values persist: the hardware reads the last
+    value written to an interface whether or not a controller still owns it, and
+    lbr_ros2_control NaNs its command interfaces only once, when the hardware component
+    activates. So dropping the Cartesian controller without handing the effort
+    interfaces to something leaves its final torques, tens of Nm, applied for the whole
+    homing move. zero_effort_controller keeps writing an actual zero.
+    """
+    robot.controller_switcher_client.switch_controller(
+        "joint_trajectory_controller",
+        controllers_that_should_be_active=[
+            "zero_effort_controller",
+            "estimated_wrench_interface",
+        ],
+    )
+    robot.joint_trajectory_controller_client.send_joint_config(
+        robot.config.joint_names,
+        robot.config.home_config,
+        robot.config.time_to_home,
+        blocking=True,
+    )
 
-params = [
-    ("task.k_pos_x", 2500.0),
-    ("task.k_pos_y", 2500.0),
-    ("task.k_pos_z", 2500.0),
-    ("task.k_rot_x", 80.0),
-    ("task.k_rot_y", 80.0),
-    ("task.k_rot_z", 80.0),
-    ("nullspace.stiffness", 5.0),
-]
 
-robot.cartesian_controller_parameters_client.set_parameters(params)
-robot.controller_switcher_client.switch_controller("cartesian_impedance_controller")
+print("Going to home position...")
+home(robot)
+home_pose = robot.end_effector_pose.copy()
+print(f"home pose: {home_pose.position}")
 
 # %%
-# Paremeters for the circle
+# The figure is centred on wherever homing actually left the tool rather than on a
+# hardcoded point, so the arm never has to traverse the workspace to start, and the
+# orientation is left at the homing one: commanding an unrelated orientation turns the
+# first move into a large rotation.
+center = home_pose.position.copy()
 radius = 0.1  # [m]
-center = [0.5, 0.0, 0.2]
 ctrl_freq = 50.0
-sin_freq = 0.25  # rot / s
-max_time = 10.0
+sin_freq_y = 0.25  # rot / s
+sin_freq_z = 0.125  # rot / s
+max_time = 8.0
 
 # %%
-# The move_to function will publish a pose to /target_pose while interpolation linearly
-starting_pose = Pose(
-    position=np.array(center) + np.array([radius * np.cos(0.0), radius * np.sin(0.0), 0]),
-    orientation=Rotation.from_euler("xyz", [-180, 0, -180], degrees=True),
+# fri_position_passthrough_controller writes the measured joint positions into the
+# position command every cycle. If the switcher drops it, that command freezes at its
+# last value while the arm keeps moving under the torque overlay, and the cabinet's
+# joint impedance pulls against a setpoint that grows more wrong as the arm moves.
+# estimated_wrench_interface only feeds force_torque_broadcaster; it is named so a
+# BEST_EFFORT partial switch cannot silently cost the wrench topic.
+robot.controller_switcher_client.switch_controller(
+    "cartesian_impedance_controller",
+    controllers_that_should_be_active=[
+        "fri_position_passthrough_controller",
+        "estimated_wrench_interface",
+    ],
 )
 
-robot.move_to(pose=starting_pose, speed=0.15)
-
 # %%
-# The set_target will directly publish the pose to /target_pose
+ee_poses = []
+target_poses = []
+ts = []
 
-print("Starting to draw a circle...")
+print("Starting to draw a figure eight...")
 t = 0.0
 target_pose = robot.end_effector_pose.copy()
 rate = robot.node.create_rate(ctrl_freq)
 
 while t < max_time:
-    x = radius * np.cos(2 * np.pi * sin_freq * t) + center[0]
-    y = radius * np.sin(2 * np.pi * sin_freq * t) + center[1]
-    z = center[2]
-    target_pose.position = np.array([x, y, z])
+    target_pose.position = np.array(
+        [
+            center[0],
+            radius * np.sin(2 * np.pi * sin_freq_y * t) + center[1],
+            radius * np.sin(2 * np.pi * sin_freq_z * t) + center[2],
+        ]
+    )
 
     robot.set_target(pose=target_pose)
+
     rate.sleep()
 
+    ee_poses.append(robot.end_effector_pose.copy())
+    target_poses.append(robot.target_pose.copy())
+    ts.append(t)
+
     t += 1.0 / ctrl_freq
 
+while t < max_time + 1.0:
+    # Let the arm settle. Expect it to keep moving after the last target: the overlay
+    # is soft, so the tool lags the target by a visible margin.
+    rate.sleep()
+
+    ee_poses.append(robot.end_effector_pose.copy())
+    target_poses.append(robot.target_pose.copy())
+    ts.append(t)
 
-print("Done drawing a circle!")
+    t += 1.0 / ctrl_freq
+
+print("Done drawing a figure eight!")
 
 # %%
-print("Going back home.")
-robot.home()
+y_t = [pose.position[1] for pose in target_poses]
+z_t = [pose.position[2] for pose in target_poses]
+y_ee = [pose.position[1] for pose in ee_poses]
+z_ee = [pose.position[2] for pose in ee_poses]
 
 # %%
-robot.shutdown()
+fig, ax = plt.subplots(1, 2, figsize=(10, 5))
+ax[0].plot(y_ee, z_ee, label="current")
+ax[0].plot(y_t, z_t, label="target", linestyle="--")
+ax[0].set_xlabel("$y$")
+ax[0].set_ylabel("$z$")
+ax[0].set_aspect("equal")
+ax[1].plot(ts, z_ee, label="current")
+ax[1].plot(ts, z_t, label="target", linestyle="--")
+ax[1].set_xlabel("$t$")
+ax[1].legend()
+
+for a in ax:
+    a.grid()
+
+fig.tight_layout()
+plt.show()
 
 # %%
+print("Going back home.")
+# The case the helper exists for: coming straight out of Cartesian impedance, so there
+# are real torques in the effort command interfaces to be replaced with zero.
+home(robot)
+
+# %%
+robot.shutdown()