feat: add log out of workspace

nova_rerun_bridge/examples/09_plan_and_execute_failed.py

@@ -0,0 +1,56 @@
+import asyncio
+
+from nova import MotionSettings
+from nova.actions import jnt, ptp
+from nova.core.exceptions import PlanTrajectoryFailed
+from nova.core.nova import Nova
+from nova.types import Pose
+
+from nova_rerun_bridge import NovaRerunBridge
+
+
+async def test():
+    async with Nova() as nova, NovaRerunBridge(nova) as bridge:
+        await bridge.setup_blueprint()
+
+        cell = nova.cell()
+        controllers = await cell.controllers()
+        controller = controllers[0]
+
+        # Connect to the controller and activate motion groups
+        async with controller[0] as motion_group:
+            home_joints = await motion_group.joints()
+            tcp_names = await motion_group.tcp_names()
+            tcp = tcp_names[0]
+
+            # Get current TCP pose and offset it slightly along the x-axis
+            current_pose = await motion_group.tcp_pose(tcp)
+            target_pose = current_pose @ Pose((1, 0, 0, 0, 0, 0))
+
+            actions = [
+                jnt(home_joints),
+                ptp(target_pose),
+                ptp(target_pose @ [-100, 0, 0, 0, 0, 0]),
+                ptp(target_pose @ [-10000, 0, 0, 0, 0, 0]),
+                jnt(home_joints),
+            ]
+
+            # you can update the settings of the action
+            for action in actions:
+                action.settings = MotionSettings(tcp_velocity_limit=200)
+
+            try:
+                joint_trajectory = await motion_group.plan(actions, tcp)
+                await bridge.log_trajectory(joint_trajectory, tcp, motion_group)
+                await motion_group.execute(joint_trajectory, tcp, actions=actions)
+            except PlanTrajectoryFailed as e:
+                # Optionally visualize the failed planning attempt if available
+                await bridge.log_trajectory(e.error.joint_trajectory, tcp, motion_group)
+                await bridge.log_error_feedback(e.error.error_feedback)
+                return
+
+        await nova.close()
+
+
+if __name__ == "__main__":
+    asyncio.run(test())

nova_rerun_bridge/nova_reun_bridge.py

@@ -8,6 +8,10 @@
 from nova import MotionGroup
 from nova.api import models
 from nova.core.nova import Nova
+from wandelbots_api_client.models import (
+    FeedbackOutOfWorkspace,
+    PlanTrajectoryFailedResponseErrorFeedback,
+)
 
 from nova_rerun_bridge.blueprint import send_blueprint
 from nova_rerun_bridge.collision_scene import log_collision_scenes
@@ -165,6 +169,25 @@ async def log_trajectory(
             load_plan_response.motion, timing_mode=timing_mode, time_offset=time_offset
         )
 
+    async def log_error_feedback(
+        self, error_feedback: PlanTrajectoryFailedResponseErrorFeedback
+    ) -> None:
+        if isinstance(error_feedback.actual_instance, FeedbackOutOfWorkspace):
+            rr.log(
+                "motion/errors/FeedbackOutOfWorkspace",
+                rr.Points3D(
+                    [
+                        error_feedback.actual_instance.invalid_tcp_pose.position[0],
+                        error_feedback.actual_instance.invalid_tcp_pose.position[1],
+                        error_feedback.actual_instance.invalid_tcp_pose.position[2],
+                    ],
+                    radii=rr.Radius.ui_points([5.0]),
+                    colors=[(255, 0, 0, 255)],
+                    labels=["Out of Workspace"],
+                ),
+                timeless=True,
+            )
+
     async def start_streaming(self, motion_group: MotionGroup) -> None:
         """Start streaming real-time robot state to Rerun viewer."""
         if motion_group in self._streaming_tasks: