From 431cee2acb6687c337891e6763f86f49f942c464 Mon Sep 17 00:00:00 2001 From: Stefan Wagner Date: Tue, 21 Jan 2025 10:13:47 +0100 Subject: [PATCH] feat: add multiple robot example --- nova_rerun_bridge/blueprint.py | 75 +++++++++++-------- .../examples/04_move_multiple_robots.py | 47 ++++++++++++ nova_rerun_bridge/trajectory.py | 41 ++++++---- 3 files changed, 116 insertions(+), 47 deletions(-) create mode 100644 nova_rerun_bridge/examples/04_move_multiple_robots.py diff --git a/nova_rerun_bridge/blueprint.py b/nova_rerun_bridge/blueprint.py index b95a6cd..7c9022a 100644 --- a/nova_rerun_bridge/blueprint.py +++ b/nova_rerun_bridge/blueprint.py @@ -5,13 +5,13 @@ from nova_rerun_bridge.consts import TIME_INTERVAL_NAME -def configure_joint_line_colors(): +def configure_joint_line_colors(motion_group: str): """ Log the visualization lines for joint limit boundaries. """ for i in range(1, 7): - prefix = "motion/joint" + prefix = f"motion/{motion_group}/joint" color = colors.colors[i - 1] rr.log( @@ -54,7 +54,7 @@ def configure_joint_line_colors(): ) for i in range(1, 7): - prefix = "motion/joint" + prefix = f"motion/{motion_group}/joint" color = colors.colors[i - 1] rr.log( @@ -97,7 +97,7 @@ def configure_joint_line_colors(): ) -def configure_tcp_line_colors(): +def configure_tcp_line_colors(motion_group: str): """ Configure time series lines for motion data. """ @@ -116,30 +116,34 @@ def configure_tcp_line_colors(): ("tcp_orientation_velocity_limit", [176, 49, 40], 4), ] for name, color, width in series_specs: - rr.log(f"motion/{name}", rr.SeriesLine(color=color, name=name, width=width), timeless=True) + rr.log( + f"motion/{motion_group}/{name}", + rr.SeriesLine(color=color, name=name, width=width), + timeless=True, + ) -def joint_content_lists(): +def joint_content_lists(motion_group: str): """ Generate content lists for joint-related time series. """ - velocity_contents = [f"motion/joint_velocity_{i}" for i in range(1, 7)] - velocity_limits = [f"motion/joint_velocity_lower_limit_{i}" for i in range(1, 7)] + [ - f"motion/joint_velocity_upper_limit_{i}" for i in range(1, 7) - ] + velocity_contents = [f"motion/{motion_group}/joint_velocity_{i}" for i in range(1, 7)] + velocity_limits = [ + f"motion/{motion_group}/joint_velocity_lower_limit_{i}" for i in range(1, 7) + ] + [f"motion/{motion_group}/joint_velocity_upper_limit_{i}" for i in range(1, 7)] - accel_contents = [f"motion/joint_acceleration_{i}" for i in range(1, 7)] - accel_limits = [f"motion/joint_acceleration_lower_limit_{i}" for i in range(1, 7)] + [ - f"motion/joint_acceleration_upper_limit_{i}" for i in range(1, 7) - ] + accel_contents = [f"motion/{motion_group}/joint_acceleration_{i}" for i in range(1, 7)] + accel_limits = [ + f"motion/{motion_group}/joint_acceleration_lower_limit_{i}" for i in range(1, 7) + ] + [f"motion/{motion_group}/joint_acceleration_upper_limit_{i}" for i in range(1, 7)] - pos_contents = [f"motion/joint_position_{i}" for i in range(1, 7)] - pos_limits = [f"motion/joint_position_lower_limit_{i}" for i in range(1, 7)] + [ - f"motion/joint_position_upper_limit_{i}" for i in range(1, 7) + pos_contents = [f"motion/{motion_group}/joint_position_{i}" for i in range(1, 7)] + pos_limits = [f"motion/{motion_group}/joint_position_lower_limit_{i}" for i in range(1, 7)] + [ + f"motion/{motion_group}/joint_position_upper_limit_{i}" for i in range(1, 7) ] - torque_contents = [f"motion/joint_torque_{i}" for i in range(1, 7)] - torque_limits = [f"motion/joint_torque_limit_{i}" for i in range(1, 7)] + torque_contents = [f"motion/{motion_group}/joint_torque_{i}" for i in range(1, 7)] + torque_limits = [f"motion/{motion_group}/joint_torque_limit_{i}" for i in range(1, 7)] return ( velocity_contents, @@ -160,6 +164,7 @@ def get_default_blueprint(motion_group_list: list): # Contents for the Spatial3DView contents = ["motion/**", "collision_scenes/**"] + [f"{group}/**" for group in motion_group_list] + first_motion_group = motion_group_list[0] time_ranges = rrb.VisibleTimeRange( TIME_INTERVAL_NAME, @@ -177,7 +182,7 @@ def get_default_blueprint(motion_group_list: list): pos_limits, torque_contents, torque_limits, - ) = joint_content_lists() + ) = joint_content_lists(first_motion_group) return rrb.Blueprint( rrb.Horizontal( @@ -185,16 +190,19 @@ def get_default_blueprint(motion_group_list: list): rrb.Tabs( rrb.Vertical( rrb.TimeSeriesView( - contents=["motion/tcp_velocity/**", "motion/tcp_velocity_limit/**"], + contents=[ + f"motion/{first_motion_group}/tcp_velocity/**", + f"motion/{first_motion_group}/tcp_velocity_limit/**", + ], name="TCP velocity", time_ranges=time_ranges, plot_legend=plot_legend, ), rrb.TimeSeriesView( contents=[ - "motion/tcp_acceleration/**", - "motion/tcp_acceleration_lower_limit/**", - "motion/tcp_acceleration_upper_limit/**", + f"motion/{first_motion_group}/tcp_acceleration/**", + f"motion/{first_motion_group}/tcp_acceleration_lower_limit/**", + f"motion/{first_motion_group}/tcp_acceleration_upper_limit/**", ], name="TCP acceleration", time_ranges=time_ranges, @@ -202,8 +210,8 @@ def get_default_blueprint(motion_group_list: list): ), rrb.TimeSeriesView( contents=[ - "motion/tcp_orientation_velocity/**", - "motion/tcp_orientation_velocity_limit/**", + f"motion/{first_motion_group}/tcp_orientation_velocity/**", + f"motion/{first_motion_group}/tcp_orientation_velocity_limit/**", ], name="TCP orientation velocity", time_ranges=time_ranges, @@ -211,9 +219,9 @@ def get_default_blueprint(motion_group_list: list): ), rrb.TimeSeriesView( contents=[ - "motion/tcp_orientation_acceleration/**", - "motion/tcp_orientation_acceleration_lower_limit/**", - "motion/tcp_orientation_acceleration_upper_limit/**", + f"motion/{first_motion_group}/tcp_orientation_acceleration/**", + f"motion/{first_motion_group}/tcp_orientation_acceleration_lower_limit/**", + f"motion/{first_motion_group}/tcp_orientation_acceleration_upper_limit/**", ], name="TCP orientation acceleration", time_ranges=time_ranges, @@ -251,13 +259,13 @@ def get_default_blueprint(motion_group_list: list): name="Joint quantities", ), rrb.TimeSeriesView( - contents="motion/time", + contents=f"motion/{first_motion_group}/time", name="Time trajectory", time_ranges=time_ranges, plot_legend=plot_legend, ), rrb.TimeSeriesView( - contents="motion/location_on_trajectory", + contents=f"motion/{first_motion_group}/location_on_trajectory", name="Location on trajectory", time_ranges=time_ranges, plot_legend=plot_legend, @@ -274,7 +282,8 @@ def send_blueprint(motion_group_list: list): Configure logging blueprints for visualization. """ - configure_tcp_line_colors() - configure_joint_line_colors() + for motion_group in motion_group_list: + configure_tcp_line_colors(motion_group) + configure_joint_line_colors(motion_group) rr.send_blueprint(get_default_blueprint(motion_group_list)) diff --git a/nova_rerun_bridge/examples/04_move_multiple_robots.py b/nova_rerun_bridge/examples/04_move_multiple_robots.py new file mode 100644 index 0000000..0c9c65b --- /dev/null +++ b/nova_rerun_bridge/examples/04_move_multiple_robots.py @@ -0,0 +1,47 @@ +import asyncio + +from nova import Controller, Nova +from nova.actions import jnt, ptp + +from nova_rerun_bridge import NovaRerunBridge + +""" +Example: Move multiple robots simultaneously. + +Prerequisites: +- A cell with two robots: one named "ur" and another named "kuka". +""" + + +async def move_robot(controller: Controller, nova_bridge: NovaRerunBridge): + async with controller[0] as motion_group: + home_joints = await motion_group.joints() + tcp_names = await motion_group.tcp_names() + tcp = tcp_names[0] + + current_pose = await motion_group.tcp_pose(tcp) + target_pose = current_pose @ (100, 0, 0, 0, 0, 0) + actions = [jnt(home_joints), ptp(target_pose), jnt(home_joints)] + + trajectory = await motion_group.plan(actions, tcp) + await nova_bridge.log_trajectory(trajectory, tcp, motion_group, time_offset=0) + + await motion_group.plan_and_execute(actions, tcp) + + +async def main(): + nova = Nova() + nova_bridge = NovaRerunBridge(nova) + await nova_bridge.setup_blueprint() + cell = nova.cell() + ur = await cell.controller("ur") + kuka = await cell.controller("kuka") + await asyncio.gather( + move_robot(ur, nova_bridge=nova_bridge), move_robot(kuka, nova_bridge=nova_bridge) + ) + await nova.close() + await nova_bridge.cleanup() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/nova_rerun_bridge/trajectory.py b/nova_rerun_bridge/trajectory.py index d505e8d..4a34036 100644 --- a/nova_rerun_bridge/trajectory.py +++ b/nova_rerun_bridge/trajectory.py @@ -45,6 +45,7 @@ def log_motion( # Process trajectory points log_trajectory( motion_id=motion_id, + motion_group=motion_group, robot=robot, visualizer=visualizer, trajectory=trajectory, @@ -53,13 +54,18 @@ def log_motion( ) -def log_trajectory_path(motion_id: str, trajectory: List[models.TrajectorySample]): +def log_trajectory_path( + motion_id: str, trajectory: List[models.TrajectorySample], motion_group: str +): points = [ [p.tcp_pose.position.x, p.tcp_pose.position.y, p.tcp_pose.position.z] for p in trajectory ] - rr.log("motion/trajectory", rr.LineStrips3D([points], colors=[[1.0, 1.0, 1.0, 1.0]])) + rr.log( + f"motion/{motion_group}/trajectory", + rr.LineStrips3D([points], colors=[[1.0, 1.0, 1.0, 1.0]]), + ) - rr.log("logs", rr.TextLog(f"{motion_id}", level=rr.TextLogLevel.INFO)) + rr.log("logs", rr.TextLog(f"{motion_group}/{motion_id}", level=rr.TextLogLevel.INFO)) def get_times_column( @@ -72,6 +78,7 @@ def get_times_column( def log_trajectory( motion_id: str, + motion_group: str, robot: DHRobot, visualizer: RobotVisualizer, trajectory: List[models.TrajectorySample], @@ -85,7 +92,7 @@ def log_trajectory( times_column = get_times_column(trajectory, timer_offset) - log_trajectory_path(motion_id, trajectory) + log_trajectory_path(motion_id, trajectory, motion_group) # Calculate and log joint positions line_segments_batch = [] @@ -94,7 +101,7 @@ def log_trajectory( line_segments_batch.append(joint_positions) rr.send_columns( - "motion/dh_parameters", + f"motion/{motion_group}/dh_parameters", times=[times_column], components=[ rr.LineStrips3D.indicator(), @@ -107,16 +114,16 @@ def log_trajectory( visualizer.log_robot_geometries(trajectory, times_column) # Log TCP pose/orientation - log_tcp_pose(trajectory, times_column) + log_tcp_pose(trajectory, motion_group, times_column) # Log joint data - log_joint_data(trajectory, times_column, optimizer_config) + log_joint_data(trajectory, motion_group, times_column, optimizer_config) # Log scalar data - log_scalar_values(trajectory, times_column, optimizer_config) + log_scalar_values(trajectory, motion_group, times_column, optimizer_config) -def log_tcp_pose(trajectory: List[models.TrajectorySample], times_column): +def log_tcp_pose(trajectory: List[models.TrajectorySample], motion_group, times_column): """ Log TCP pose (position + orientation) data. """ @@ -142,7 +149,7 @@ def log_tcp_pose(trajectory: List[models.TrajectorySample], times_column): tcp_rotations.append(rr.RotationAxisAngle(axis=axis_angle, angle=angle)) rr.send_columns( - "motion/tcp_position", + f"motion/{motion_group}/tcp_position", times=[times_column], components=[ rr.Transform3D.indicator(), @@ -153,7 +160,10 @@ def log_tcp_pose(trajectory: List[models.TrajectorySample], times_column): def log_joint_data( - trajectory: List[models.TrajectorySample], times_column, optimizer_config: models.OptimizerSetup + trajectory: List[models.TrajectorySample], + motion_group, + times_column, + optimizer_config: models.OptimizerSetup, ) -> None: """ Log joint-related data (position, velocity, acceleration, torques) from a trajectory as columns. @@ -212,14 +222,17 @@ def log_joint_data( for i in range(num_joints): if data[i]: rr.send_columns( - f"motion/joint_{data_type}_{i + 1}", + f"motion/{motion_group}/joint_{data_type}_{i + 1}", times=[times_column], components=[rr.components.ScalarBatch(data[i])], ) def log_scalar_values( - trajectory: List[models.TrajectorySample], times_column, optimizer_config: models.OptimizerSetup + trajectory: List[models.TrajectorySample], + motion_group, + times_column, + optimizer_config: models.OptimizerSetup, ): """ Log scalar values such as TCP velocity, acceleration, orientation velocity/acceleration, time, and location. @@ -287,7 +300,7 @@ def log_scalar_values( for key, values in scalar_data.items(): if values: rr.send_columns( - f"motion/{key}", + f"motion/{motion_group}/{key}", times=[times_column], components=[rr.components.ScalarBatch(values)], )