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 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" 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()