From 3882e08732db45c8cd2f91577681e9d352e6fdd9 Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Wed, 26 Aug 2026 13:58:49 +0900 Subject: [PATCH 1/3] fix(iiwa): correct unreachable home pose and unpublished base frame IiwaConfig.home_config was the Franka pose shared by the other configurations in this module. It puts A4 at -135 degrees, outside the +/-120 degree A4 limit of every iiwa in the range, so robot.home() commanded a pose the arm cannot reach. Replace it with [0, 30, 0, -75, 0, 75, 0] degrees, validated on an iiwa14 R820: every joint at least 45 degrees from its limit, manipulability sqrt(det(J J^T)) of 0.126 against 0.044 for the Franka pose and exactly 0 for the all-zeros candle pose, which is fully singular and a poor place to start a Cartesian impedance controller. base_frame was "world". lbr_fri_ros2_stack publishes lbr_link_0 as the root of the arm and no "world" frame, so a target pose expressed in "world" has no transform available. --- crisp_py/robot/robot_config.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/crisp_py/robot/robot_config.py b/crisp_py/robot/robot_config.py index 1dc775d..c43de45 100644 --- a/crisp_py/robot/robot_config.py +++ b/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" From d152d75477f64c9cc9aef54a6ec2c5f64d0fafc0 Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Wed, 26 Aug 2026 13:58:49 +0900 Subject: [PATCH 2/3] feat(iiwa): add an iiwa14 R820 robot config Ready-made profile for the arm driven through lbr_fri_ros2_stack at its default robot_name of "lbr", so Robot.from_yaml("iiwa14_r820") works without hand-assembling a RobotConfig. time_to_home is 10 s rather than the 5 s default: these are slow commissioning moves made under an enabling switch. --- crisp_py/config/robots/iiwa14_r820.yaml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 crisp_py/config/robots/iiwa14_r820.yaml diff --git a/crisp_py/config/robots/iiwa14_r820.yaml b/crisp_py/config/robots/iiwa14_r820.yaml new file mode 100644 index 0000000..4043fc5 --- /dev/null +++ b/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 From 50a245aabb43202cffc7409f2f1fbed81df876dd Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Wed, 26 Aug 2026 14:24:23 +0900 Subject: [PATCH 3/3] docs(examples): make the iiwa example a figure eight on the real config The robot-specific examples are where arm-specific behaviour belongs, so this replaces the circle in example_with_iiwa.py rather than adapting a numbered example; 01_figure_eight.py stays generic and keeps defaulting to fr3. Uses make_robot("iiwa14_r820") instead of assembling an IiwaConfig by hand. Drops the parameter overrides. They set a task stiffness of 2500 N/m, which is an order of magnitude above what this arm can render: the torque overlay acts through the cabinet's own joint impedance, and asking for a stiffer impedance than that coupling cannot be followed no matter the damping. Homing goes through a local helper. robot.home() switches through the stock switcher, which deactivates every active controller not ending in "broadcaster" and offers no way to exempt one. Command interface values persist, and lbr_ros2_control NaNs its command interfaces only once when the hardware component activates, so deactivating the Cartesian controller leaves its final torques applied for the whole homing move rather than invalidating them. zero_effort_controller is held active to keep writing a real zero, and fri_position_passthrough_controller is held across the Cartesian switch for the same reason on the position side. The figure is centred on the pose homing actually reached and keeps that orientation, so the arm does not traverse the workspace to start and the first move is not a large rotation. Tracking is plotted against the target, and robot.target_pose is used rather than the private attribute. --- examples/example_with_iiwa.py | 160 +++++++++++++++++++++++++--------- 1 file changed, 121 insertions(+), 39 deletions(-) diff --git a/examples/example_with_iiwa.py b/examples/example_with_iiwa.py index c695cce..cc711a8 100644 --- a/examples/example_with_iiwa.py +++ b/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()