Merge pull request #17 from wandelbotsgmbh/feature/add-multiple-robot…

…s-example

feat: add multiple robot example

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,43 +182,46 @@ 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(
             rrb.Spatial3DView(contents=contents, name="3D Nova", background=[20, 22, 35]),
             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,
                         plot_legend=plot_legend,
                     ),
                     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,
                         plot_legend=plot_legend,
                     ),
                     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))

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

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)],
             )