Squashed commit of the following:

commit d30fdf7 Author: github-actions <actions@users.noreply.github.com> Date: Fri Feb 7 15:25:56 2025 +0000 0.21.0 Automatically generated by python-semantic-release commit 11bb47a Author: dsonnemann <dirk.sonnemann@gmail.com> Date: Fri Feb 7 16:24:39 2025 +0100 feat: streaming execution first draft (#43) Co-authored-by: Dirk Sonnemann <dirk.sonnemann@wandelbots.com> commit 488fcd8 Author: github-actions <actions@users.noreply.github.com> Date: Fri Feb 7 14:43:10 2025 +0000 0.20.0 Automatically generated by python-semantic-release commit 73e87d2 Author: Christoph Biering <1353438+biering@users.noreply.github.com> Date: Fri Feb 7 14:41:47 2025 +0000 feat: propagate robot state in motion group (#47) Co-authored-by: cbiering <christoph.biering@wandelbots.com> commit be5fe50 Author: github-actions <actions@users.noreply.github.com> Date: Fri Feb 7 13:10:21 2025 +0000 0.19.1 Automatically generated by python-semantic-release commit f64cb39 Author: Christoph Biering <1353438+biering@users.noreply.github.com> Date: Fri Feb 7 13:09:02 2025 +0000 chore: added active tcp to abstract robot (#46) Co-authored-by: cbiering <christoph.biering@wandelbots.com> commit bf8a885 Author: github-actions <actions@users.noreply.github.com> Date: Fri Feb 7 11:25:39 2025 +0000 0.19.0 Automatically generated by python-semantic-release commit 352b17b Author: Christoph Biering <1353438+biering@users.noreply.github.com> Date: Fri Feb 7 11:24:17 2025 +0000 feat: get active_tcp_name & added motion_settings to var name (#44) Co-authored-by: cbiering <christoph.biering@wandelbots.com>
wandelbotsgmbh · Feb 7, 2025 · 8ab3fbe · 8ab3fbe
1 parent c229c78
commit 8ab3fbe
Show file tree

Hide file tree

Showing 19 changed files with 177 additions and 65 deletions.
diff --git a/.github/workflows/ci-dev.yaml b/.github/workflows/ci-dev.yaml
@@ -28,6 +28,9 @@ jobs:
       - name: Check formatting with ruff
         run: poetry run ruff format --check .
 
+      - name: Check import order
+        run: poetry run ruff check --select I
+
       - name: Check ruff for linting
         run: poetry run ruff check .
 

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,42 @@
 # CHANGELOG
 
 
+## v0.21.0 (2025-02-07)
+
+### Features
+
+* feat: streaming execution first draft (#43)
+
+Co-authored-by: Dirk Sonnemann <dirk.sonnemann@wandelbots.com> ([`11bb47a`](https://github.com/wandelbotsgmbh/wandelbots-nova/commit/11bb47a94e4370c69291af59eb8b425e693a7c5d))
+
+
+## v0.20.0 (2025-02-07)
+
+### Features
+
+* feat: propagate robot state in motion group (#47)
+
+Co-authored-by: cbiering <christoph.biering@wandelbots.com> ([`73e87d2`](https://github.com/wandelbotsgmbh/wandelbots-nova/commit/73e87d24cc6ebede177a074cdbf5ef76a956b9dd))
+
+
+## v0.19.1 (2025-02-07)
+
+### Chores
+
+* chore: added active tcp to abstract robot (#46)
+
+Co-authored-by: cbiering <christoph.biering@wandelbots.com> ([`f64cb39`](https://github.com/wandelbotsgmbh/wandelbots-nova/commit/f64cb393164cb92ae8ec5173ca32ce912c390420))
+
+
+## v0.19.0 (2025-02-07)
+
+### Features
+
+* feat: get active_tcp_name & added motion_settings to var name (#44)
+
+Co-authored-by: cbiering <christoph.biering@wandelbots.com> ([`352b17b`](https://github.com/wandelbotsgmbh/wandelbots-nova/commit/352b17b5ee63e3e747b7ef19f85226b644db3b07))
+
+
 ## v0.18.0 (2025-02-06)
 
 ### Features

diff --git a/examples/02_plan_and_execute.py b/examples/02_plan_and_execute.py
@@ -52,7 +52,8 @@ async def main():
             action.settings = MotionSettings(tcp_velocity_limit=200)
 
         joint_trajectory = await motion_group.plan(actions, tcp)
-        await motion_group.execute(joint_trajectory, tcp, actions=actions)
+        async for _ in motion_group.execute(joint_trajectory, tcp, actions=actions):
+            pass
 
         await cell.delete_robot_controller(controller.controller_id)
 

diff --git a/examples/03_move_and_set_ios.py b/examples/03_move_and_set_ios.py
@@ -17,9 +17,6 @@
 
 
 async def main():
-    def on_movement(motion_state):
-        print(motion_state)
-
     async with Nova() as nova:
         cell = nova.cell()
         controller = await cell.ensure_virtual_robot_controller(
@@ -44,7 +41,8 @@ def on_movement(motion_state):
                 jnt(home_joints),
             ]
 
-            await motion_group.plan_and_execute(actions, tcp, on_movement=on_movement)
+            async for motion_state in motion_group.plan_and_execute(actions, tcp):
+                print(motion_state)
 
             io_value = await controller.read("tool_out[0]")
             print(io_value)

diff --git a/examples/04_move_multiple_robots.py b/examples/04_move_multiple_robots.py
@@ -25,7 +25,8 @@ async def move_robot(controller: Controller):
         target_pose = current_pose @ (100, 0, 0, 0, 0, 0)
         actions = [jnt(home_joints), ptp(target_pose), jnt(home_joints)]
 
-        await motion_group.plan_and_execute(actions, tcp)
+        async for _ in motion_group.plan_and_execute(actions, tcp):
+            pass
 
 
 async def main():

diff --git a/examples/05_selection_motion_group_activation.py b/examples/05_selection_motion_group_activation.py
@@ -28,7 +28,8 @@ async def move_robot(motion_group: MotionGroup, tcp: str):
         ptp(home_pose),
     ]
 
-    await motion_group.plan_and_execute(actions, tcp=tcp)
+    async for _ in motion_group.plan_and_execute(actions, tcp=tcp):
+        pass
 
 
 async def main():

diff --git a/nova/__init__.py b/nova/__init__.py
@@ -3,7 +3,7 @@
 from nova.core.motion_group import MotionGroup
 from nova.core.movement_controller import speed_up as speed_up_movement_controller
 from nova.core.nova import Cell, Nova
-from nova.motion_settings import MotionSettings
+from nova.types import MotionSettings
 
 __all__ = [
     "Nova",

diff --git a/nova/actions.py b/nova/actions.py
@@ -4,8 +4,8 @@
 import pydantic
 import wandelbots_api_client as wb
 
-from nova.motion_settings import MotionSettings
 from nova.types.collision_scene import CollisionScene
+from nova.types.motion_settings import MotionSettings
 from nova.types.pose import Pose
 from nova.types.state import MotionState
 
@@ -511,5 +511,5 @@ class MovementControllerContext(pydantic.BaseModel):
     [ExecuteTrajectoryResponseStream], ExecuteTrajectoryRequestStream
 ]
 MovementController = Callable[
-    [MovementControllerContext, Callable[[MotionState], None] | None], MovementControllerFunction
+    [MovementControllerContext, Callable[[MotionState | None], None]], MovementControllerFunction
 ]
diff --git a/nova/core/motion_group.py b/nova/core/motion_group.py
@@ -8,7 +8,7 @@
 from nova.core.movement_controller import motion_group_state_to_motion_state, move_forward
 from nova.core.robot_cell import AbstractRobot
 from nova.gateway import ApiGateway
-from nova.types import InitialMovementStream, LoadPlanResponse, MotionState, Pose
+from nova.types import InitialMovementStream, LoadPlanResponse, MotionState, Pose, RobotState
 
 MAX_JOINT_VELOCITY_PREPARE_MOVE = 0.2
 START_LOCATION_OF_MOTION = 0.0
@@ -72,7 +72,7 @@ async def _execute(
         joint_trajectory: wb.models.JointTrajectory,
         tcp: str,
         actions: list[Action],
-        on_movement: Callable[[MotionState], None],
+        on_movement: Callable[[MotionState | None], None],
         movement_controller: MovementController | None,
     ):
         if movement_controller is None:
@@ -176,19 +176,25 @@ async def stop(self):
         except ValueError as e:
             logger.debug(f"No motion to stop for {self}: {e}")
 
-    async def get_state(self, tcp: str | None = None) -> wb.models.MotionGroupStateResponse:
+    async def get_state(self, tcp: str | None = None) -> RobotState:
         response = await self._api_gateway.motion_group_infos_api.get_current_motion_group_state(
             cell=self._cell, motion_group=self.motion_group_id, tcp=tcp
         )
-        return response
+        return RobotState(
+            pose=Pose(response.state.tcp_pose), joints=tuple(response.state.joint_position.joints)
+        )
 
     async def joints(self) -> tuple:
         state = await self.get_state()
-        return tuple(state.state.joint_position.joints)
+        if state.joints is None:
+            raise ValueError(
+                f"No joint positions available for motion group {self._motion_group_id}"
+            )
+        return state.joints
 
     async def tcp_pose(self, tcp: str | None = None) -> Pose:
         state = await self.get_state(tcp=tcp)
-        return Pose(state.state.tcp_pose)
+        return state.pose
 
     async def tcps(self) -> list[wb.models.RobotTcp]:
         """Get the available tool center points (TCPs)"""
@@ -200,3 +206,13 @@ async def tcps(self) -> list[wb.models.RobotTcp]:
     async def tcp_names(self) -> list[str]:
         """Get the names of the available tool center points (TCPs)"""
         return [tcp.id for tcp in await self.tcps()]
+
+    async def active_tcp(self) -> wb.models.RobotTcp:
+        active_tcp = await self._api_gateway.motion_group_infos_api.get_active_tcp(
+            cell=self._cell, motion_group=self.motion_group_id
+        )
+        return active_tcp
+
+    async def active_tcp_name(self) -> str:
+        active_tcp = await self.active_tcp()
+        return active_tcp.id
diff --git a/nova/core/movement_controller.py b/nova/core/movement_controller.py
@@ -43,7 +43,7 @@ def motion_group_state_to_motion_state(
 
 
 def move_forward(
-    context: MovementControllerContext, on_movement: Callable[[MotionState], None] | None
+    context: MovementControllerContext, on_movement: Callable[[MotionState | None], None]
 ) -> MovementControllerFunction:
     """
     movement_controller is an async function that yields requests to the server.
@@ -77,21 +77,22 @@ async def movement_controller(
             instance = execute_trajectory_response.actual_instance
 
             # Send the current location to the consumer
-            if isinstance(instance, wb.models.Movement) and instance.movement and on_movement:
+            if isinstance(instance, wb.models.Movement) and instance.movement:
                 motion_state = movement_to_motion_state(instance)
                 if motion_state:
                     on_movement(motion_state)
 
             # Stop when standstill indicates motion ended
             if isinstance(instance, wb.models.Standstill):
                 if instance.standstill.reason == wb.models.StandstillReason.REASON_MOTION_ENDED:
+                    on_movement(None)
                     return
 
     return movement_controller
 
 
 def speed_up(
-    context: MovementControllerContext, on_movement: Callable[[MotionState], None] | None
+    context: MovementControllerContext, on_movement: Callable[[MotionState | None], None]
 ) -> MovementControllerFunction:
     async def movement_controller(
         response_stream: ExecuteTrajectoryResponseStream,
@@ -120,15 +121,16 @@ async def movement_controller(
         async for execute_trajectory_response in response_stream:
             counter += 1
             instance = execute_trajectory_response.actual_instance
-            # Send the current location to the consumer
-            if isinstance(instance, wb.models.Movement) and on_movement:
+            # Send the current location to the consume
+            if isinstance(instance, wb.models.Movement):
                 motion_state = movement_to_motion_state(instance)
                 if motion_state:
                     on_movement(motion_state)
 
             # Terminate the generator
             if isinstance(instance, wb.models.Standstill):
                 if instance.standstill.reason == wb.models.StandstillReason.REASON_MOTION_ENDED:
+                    on_movement(None)
                     return
 
             if isinstance(instance, wb.models.PlaybackSpeedResponse):

diff --git a/nova/core/robot_cell.py b/nova/core/robot_cell.py
@@ -27,9 +27,10 @@
 import pydantic
 from loguru import logger
 
+from nova import api
 from nova.actions import Action, MovementController
-from nova.api import models
-from nova.types import MotionState, Pose
+from nova.types import MotionState, Pose, RobotState
+from nova.utils import Callerator
 
 
 class RobotCellError(Exception):
@@ -225,7 +226,7 @@ def execution_duration(self) -> float:
         return self._execution_duration
 
     @abstractmethod
-    async def _plan(self, actions: list[Action], tcp: str) -> models.JointTrajectory:
+    async def _plan(self, actions: list[Action], tcp: str) -> api.models.JointTrajectory:
         """Plan a trajectory for the given actions
 
         Args:
@@ -237,7 +238,7 @@ async def _plan(self, actions: list[Action], tcp: str) -> models.JointTrajectory
             wb.models.JointTrajectory: The planned joint trajectory
         """
 
-    async def plan(self, actions: list[Action] | Action, tcp: str) -> models.JointTrajectory:
+    async def plan(self, actions: list[Action] | Action, tcp: str) -> api.models.JointTrajectory:
         """Plan a trajectory for the given actions
 
         Args:
@@ -259,10 +260,10 @@ async def plan(self, actions: list[Action] | Action, tcp: str) -> models.JointTr
     @abstractmethod
     async def _execute(
         self,
-        joint_trajectory: models.JointTrajectory,
+        joint_trajectory: api.models.JointTrajectory,
         tcp: str,
         actions: list[Action],
-        on_movement: Callable[[MotionState], None],
+        on_movement: Callable[[MotionState | None], None],
         movement_controller: MovementController | None,
     ):
         """Execute a planned motion
@@ -277,10 +278,10 @@ async def _execute(
 
     async def execute(
         self,
-        joint_trajectory: models.JointTrajectory,
+        joint_trajectory: api.models.JointTrajectory,
         tcp: str,
         actions: list[Action] | Action | None,
-        on_movement: Callable[[MotionState], None] | None = None,
+        on_movement: Callable[[MotionState | None], None] | None = None,
         movement_controller: MovementController | None = None,
     ):
         """Execute a planned motion
@@ -299,30 +300,40 @@ async def execute(
 
         self._motion_recording.append([])
 
-        def _on_movement(motion_state_: MotionState):
-            self._motion_recording[-1].append(motion_state_)
+        def _on_movement(motion_state_: MotionState | None):
+            if motion_state_ is not None:
+                self._motion_recording[-1].append(motion_state_)
             if on_movement:
                 on_movement(motion_state_)
 
-        await self._execute(
-            joint_trajectory,
-            tcp,
-            actions,
-            movement_controller=movement_controller,
-            on_movement=_on_movement,
+        callerator = Callerator(_on_movement)
+        execution_task = asyncio.create_task(
+            self._execute(
+                joint_trajectory,
+                tcp,
+                actions,
+                movement_controller=movement_controller,
+                on_movement=callerator,
+            )
         )
+        async for motion_state in callerator.stream():
+            yield motion_state
+        await execution_task
 
     async def plan_and_execute(
         self,
         actions: list[Action] | Action,
         tcp: str,
-        on_movement: Callable[[MotionState], None] | None = None,
+        on_movement: Callable[[MotionState | None], None] | None = None,
     ):
         joint_trajectory = await self.plan(actions, tcp)
-        await self.execute(joint_trajectory, tcp, actions, on_movement, movement_controller=None)
+        async for motion_state in self.execute(
+            joint_trajectory, tcp, actions, on_movement, movement_controller=None
+        ):
+            yield motion_state
 
     @abstractmethod
-    async def get_state(self, tcp: str | None = None) -> models.MotionGroupStateResponse:
+    async def get_state(self, tcp: str | None = None) -> RobotState:
         """Current state (pose, joints) of the robot based on the tcp.
 
         Args:
@@ -354,7 +365,7 @@ async def tcp_pose(self, tcp: str | None = None) -> Pose:
         """
 
     @abstractmethod
-    async def tcps(self) -> list[models.RobotTcp]:
+    async def tcps(self) -> list[api.models.RobotTcp]:
         """Return all TCPs that are configured on the robot with corresponding offset from flange as pose
 
         Returns: the TCPs of the robot
@@ -369,6 +380,22 @@ async def tcp_names(self) -> list[str]:
 
         """
 
+    @abstractmethod
+    async def active_tcp(self) -> api.models.RobotTcp:
+        """Return the active TCP of the robot
+
+        Returns: the active TCP
+
+        """
+
+    @abstractmethod
+    async def active_tcp_name(self) -> str:
+        """Return the name of the active TCP of the robot
+
+        Returns: the name of the active TCP
+
+        """
+
     @abstractmethod
     async def stop(self):
         """Stop behaviour of the robot"""

diff --git a/nova/types/__init__.py b/nova/types/__init__.py
@@ -4,6 +4,7 @@
 import wandelbots_api_client as wb
 
 from nova.types.collision_scene import CollisionScene
+from nova.types.motion_settings import MotionSettings
 from nova.types.pose import Pose
 from nova.types.state import MotionState, RobotState
 from nova.types.vector3d import Vector3d
@@ -21,4 +22,5 @@
     "InitialMovementConsumer",
     "MotionState",
     "RobotState",
+    "MotionSettings",
 ]