chore: add examples and refactoring (#5)

Co-authored-by: cbiering <christoph.biering@wandelbots.com>

examples/02_plan_and_execute.py

@@ -1,6 +1,7 @@
-from nova import Nova, ptp, jnt, Pose
+from nova import Nova, ptp, jnt, Pose, pi
 import asyncio
-import numpy as np
+
+from nova.core.movement_controller import move_forward
 
 
 async def main():
@@ -9,22 +10,36 @@ async def main():
     controller = await cell.controller("ur")
 
     # Define a home position
-    home_joints = (0, -np.pi / 2, -np.pi / 2, -np.pi / 2, np.pi / 2, 0)
+    home_joints = (0, -pi / 4, -pi / 4, -pi / 4, pi / 4, 0)
 
     # Connect to the controller and activate motion groups
     async with controller:
         motion_group = controller.get_motion_group()
 
         # Get current TCP pose and offset it slightly along the x-axis
         current_pose = await motion_group.tcp_pose("Flange")
-        target_pose = current_pose @ Pose((100, 0, 0, 0, 0, 0))
-
-        actions = [jnt(home_joints), ptp(target_pose), jnt(home_joints)]
+        target_pose = current_pose @ Pose((1, 0, 0, 0, 0, 0))
+
+        actions = [
+            jnt(home_joints),
+            ptp(target_pose),
+            jnt(home_joints),
+            ptp(target_pose @ [200, 0, 0, 0, 0, 0]),
+            jnt(home_joints),
+            ptp(target_pose @ (300, 0, 0, 0, 0, 0)),
+            jnt(home_joints),
+            ptp(target_pose @ Pose((300, 0, 0, 0, 0, 0))),
+            jnt(home_joints),
+            ptp(target_pose @ Pose((400, 0, 0, 0, 0, 0))),
+            jnt(home_joints),
+            ptp(target_pose),
+            jnt(home_joints),
+        ]
 
         # plan_response = await motion_group.plan(trajectory, tcp="Flange")
         # print(plan_response)
 
-        await motion_group.run(actions, tcp="Flange")
+        await motion_group.run(actions, tcp="Flange", movement_controller=move_forward)
 
 
 if __name__ == "__main__":

examples/03_plan_and_execute_ios.py → examples/03_move_and_set_ios.py

@@ -1,9 +1,8 @@
-from nova import Nova, Pose, ptp, jnt
+from nova import Nova, Pose, ptp, jnt, pi
 
 # TODO: public interface
 from nova.types.action import WriteAction
 import asyncio
-import numpy as np
 
 
 async def main():
@@ -12,15 +11,15 @@ async def main():
     controller = await cell.controller("ur")
 
     # Define a home position
-    home_joints = (0, -np.pi / 2, -np.pi / 2, -np.pi / 2, np.pi / 2, 0)
+    home_joints = (0, -pi / 4, -pi / 4, -pi / 4, pi / 4, 0)
 
     # Connect to the controller and activate motion groups
     async with controller:
         motion_group = controller.get_motion_group()
 
         # Get current TCP pose and offset it slightly along the x-axis
         current_pose = await motion_group.tcp_pose("Flange")
-        target_pose = current_pose @ Pose.from_tuple((100, 0, 0, 0, 0, 0))
+        target_pose = current_pose @ Pose((100, 0, 0, 0, 0, 0))
         actions = [
             jnt(home_joints),
             # controller.write_on_path("digital_out[0]", value=False),
@@ -34,10 +33,10 @@ async def main():
         # plan_response = await motion_group.plan(trajectory, tcp="Flange")
         # print(plan_response)
 
-        motion_iter = motion_group.stream_run(actions, tcp="Flange")
-        async for motion_state in motion_iter:
-            print(motion_state)
+        def print_motion(motion):
+            print(motion)
 
+        await motion_group.run(actions, tcp="Flange", initial_movement_consumer=print_motion)
         await motion_group.run(actions, tcp="Flange")
         await motion_group.run(ptp(target_pose), tcp="Flange")
 

examples/04_plan_and_execute_multiple_robots.py → examples/04_move_multiple_robots.py

@@ -1,28 +1,29 @@
-from nova import Nova, ptp, jnt, Pose, Controller
+from nova import Nova, ptp, jnt, Controller, speed_up_movement_controller, pi
 import asyncio
-import numpy as np
-
-
-async def main():
-    nova = Nova()
-    cell = nova.cell()
-    ur = await cell.controller("ur")
-    kuka = await cell.controller("kuka")
-
-    await asyncio.gather(move_robot(ur), move_robot(kuka))
 
 
 async def move_robot(controller: Controller):
-    home_joints = (0, -np.pi / 2, -np.pi / 2, -np.pi / 2, np.pi / 2, 0)
+    home_joints = (0, -pi / 4, -pi / 4, -pi / 4, pi / 4, 0)
 
     async with controller:
         motion_group = controller.get_motion_group()
 
         current_pose = await motion_group.tcp_pose("Flange")
-        target_pose = current_pose @ Pose((100, 0, 0, 0, 0, 0))
+        target_pose = current_pose @ (100, 0, 0, 0, 0, 0)
         actions = [jnt(home_joints), ptp(target_pose), jnt(home_joints)]
 
-        await motion_group.run(actions, tcp="Flange")
+        await motion_group.run(
+            actions, tcp="Flange", movement_controller=speed_up_movement_controller
+        )
+
+
+async def main():
+    nova = Nova()
+    cell = nova.cell()
+    ur = await cell.controller("ur")
+    kuka = await cell.controller("kuka")
+
+    await asyncio.gather(move_robot(ur), move_robot(kuka))
 
 
 if __name__ == "__main__":

nova/__init__.py

@@ -3,6 +3,8 @@
 from nova.core.controller import Controller
 from nova.types.pose import Pose
 from nova.types.action import Action, lin, ptp, jnt, cir
+from nova.core.movement_controller import speed_up as speed_up_movement_controller
+from numpy import pi
 
 __all__ = [
     "Nova",
@@ -15,4 +17,6 @@
     "cir",
     "Action",
     "Pose",
+    "speed_up_movement_controller",
+    "pi",
 ]

nova/core/controller.py

@@ -1,6 +1,5 @@
 from typing import final
 
-import wandelbots_api_client as wb
 from nova.core.motion_group import MotionGroup
 from loguru import logger
 
@@ -16,21 +15,13 @@ def __init__(self, *, api_gateway: ApiGateway, cell: str, controller_host: str):
         self._controller_host = controller_host
         self._motion_groups: dict[str, MotionGroup] = {}
 
-    async def _get_controller(self, host: str) -> wb.models.ControllerInstance | None:
-        controller_list_response = await self._controller_api.list_controllers(cell=self._cell)
-        controller_list = list(controller_list_response.instances)
-        return next((c for c in controller_list if c.host == host), None)
-
     @final
     async def __aenter__(self):
         activate_all_motion_groups_response = (
             await self._motion_group_api.activate_all_motion_groups(
                 cell=self._cell, controller=self._controller_host
             )
         )
-        # TODO: should we store these states? I dont like storing state, then you have to manage them
-        #       stateless looks simpler
-        # TODO: should we deactivate these motions groups? what does wandelscript does?
         motion_groups = activate_all_motion_groups_response.instances
         for mg in motion_groups:
             logger.info(f"Found motion group {mg.motion_group}")
@@ -42,19 +33,14 @@ async def __aenter__(self):
 
     @final
     async def __aexit__(self, exc_type, exc_val, exc_tb):
-        # TODO: should we deactivate these motions groups? what does wandelscript does?
-        pass
+        for motion_group_id in self._motion_groups.keys():
+            logger.info(f"Deactivating motion group {motion_group_id}")
+            await self._motion_group_api.deactivate_motion_group(self._cell, motion_group_id)
+
+        await self._api_gateway.close()
 
     def get_motion_groups(self) -> dict[str, MotionGroup]:
         return self._motion_groups
 
-    def get_motion_group(self, motion_group_id: str = "0") -> MotionGroup:
-        # TODO: I know this doesnt looks good :)
-        #       here are some considerations for a better implementation:
-        #       If possible I would prefer stateless approach,
-        #       so we dont return it from the internal state, but we fetch it from the API with the id
-        #       in that case having str id is more fleixble than having an int id
-        return self._motion_groups[f"{motion_group_id}@{self._controller_host}"]
-
-    def __getitem__(self, item):
-        return self._motion_groups[f"{item}@{self._controller_host}"]
+    def get_motion_group(self, motion_group_id: str = "0") -> MotionGroup | None:
+        return self._motion_groups.get(f"{motion_group_id}@{self._controller_host}", None)

nova/core/exceptions.py

@@ -11,3 +11,13 @@ def __init__(self, controller: str = None):
 class PlanTrajectoryFailed(Exception):
     def __init__(self, error: wb.models.PlanTrajectoryFailedResponse):
         super().__init__(f"Plan trajectory failed: {json.dumps(error.to_dict(), indent=2)}")
+
+
+class InitMovementFailed(Exception):
+    def __init__(self, error: wb.models.InitializeMovementResponseInitResponse):
+        super().__init__(f"Initial movement failed: {json.dumps(error.to_dict(), indent=2)}")
+
+
+class LoadPlanFailed(Exception):
+    def __init__(self, error: wb.models.PlanSuccessfulResponse):
+        super().__init__(f"Load plan failed: {json.dumps(error.to_dict(), indent=2)}")