feat(RPS-1027): separated plan and execute in motion group (#16)

Co-authored-by: cbiering <[email protected]>

examples/02_plan_and_execute.py

@@ -4,8 +4,6 @@
 from math import pi
 import asyncio
 
-from nova.core.movement_controller import move_forward
-
 
 async def main():
     nova = Nova()
@@ -39,7 +37,8 @@ async def main():
             jnt(home_joints),
         ]
 
-        await motion_group.run(actions, tcp=tcp, movement_controller=move_forward)
+        joint_trajectory = await motion_group.plan(actions, tcp)
+        await motion_group.execute(joint_trajectory, tcp, actions=actions)
 
 
 if __name__ == "__main__":

examples/03_move_and_set_ios.py

@@ -11,12 +11,13 @@ async def main():
     nova = Nova()
     cell = nova.cell()
     controllers = await cell.controllers()
+    controller = controllers[0]
 
     # Define a home position
     home_joints = (0, -pi / 4, -pi / 4, -pi / 4, pi / 4, 0)
 
     # Connect to the controller and activate motion groups
-    async with controllers[0] as motion_group:
+    async with controller[0] as motion_group:
         tcp_names = await motion_group.tcp_names()
         tcp = tcp_names[0]
 
@@ -32,16 +33,7 @@ async def main():
         ]
 
         # io_value = await controller.read_io("digital_out[0]")
-
-        # plan_response = await motion_group.plan(trajectory, tcp="Flange")
-        # print(plan_response)
-
-        def print_motion(motion):
-            print(motion)
-
-        await motion_group.run(actions, tcp=tcp, initial_movement_consumer=print_motion)
-        await motion_group.run(actions, tcp=tcp)
-        await motion_group.run(ptp(target_pose), tcp=tcp)
+        await motion_group.plan_and_execute(actions, tcp)
 
 
 if __name__ == "__main__":

examples/04_move_multiple_robots.py

@@ -1,4 +1,4 @@
-from nova import Nova, Controller, speed_up_movement_controller
+from nova import Nova, Controller
 from nova.actions import ptp, jnt
 from math import pi
 import asyncio
@@ -15,7 +15,7 @@ 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.run(actions, tcp=tcp, movement_controller=speed_up_movement_controller)
+        await motion_group.plan_and_execute(actions, tcp)
 
 
 async def main():

examples/05_selection_motion_group_activation.py

@@ -27,7 +27,7 @@ async def move_robot(motion_group: MotionGroup, tcp: str):
         ptp(home_pose),
     ]
 
-    await motion_group.run(actions, tcp=tcp)
+    await motion_group.plan_and_execute(actions, tcp=tcp)
 
 
 async def main():
@@ -37,7 +37,7 @@ async def main():
     kuka = await cell.controller("kuka")
     tcp = "Flange"
 
-    flange_state = await ur[0].get_state(tcp=tcp)
+    flange_state = await ur[0].get_state(tcp)
     print(flange_state)
 
     # activate all motion groups
@@ -46,11 +46,11 @@ async def main():
 
     # activate motion group 0
     async with ur.motion_group(0) as mg_0:
-        await move_robot(mg_0)
+        await move_robot(mg_0, tcp)
 
     # activate motion group 0
     async with ur[0] as mg_0:
-        await move_robot(mg_0)
+        await move_robot(mg_0, tcp)
 
     # activate motion group 0 from two different controllers
     async with ur[0] as ur_0_mg, kuka[0] as kuka_0_mg:

nova/core/motion_group.py

@@ -45,49 +45,66 @@ def current_motion(self) -> str:
         #    raise ValueError("No MotionId attached. There is no planned motion available.")
         return self._current_motion
 
-    async def plan(self, actions: list[Action], tcp: str) -> wb.models.JointTrajectory:
-        current_joints = await self.joints(tcp=tcp)
-        robot_setup = await self._get_optimizer_setup(tcp=tcp)
-        motion_commands = CombinedActions(items=actions).to_motion_command()
+    async def plan(self, actions: list[Action] | Action, tcp: str) -> wb.models.JointTrajectory:
+        """Plan a trajectory for the given actions
+
+        Args:
+            actions (list[Action] | Action): The actions to be planned. Can be a single action or a list of actions.
+                Only motion actions are considered for planning.
+            tcp (str): The identifier of the tool center point (TCP)
 
+        Returns:
+            wb.models.JointTrajectory: The planned joint trajectory
+        """
+        if not isinstance(actions, list):
+            actions = [actions]
+
+        if len(actions) == 0:
+            raise ValueError("No actions provided")
+
+        motion_commands = CombinedActions(items=actions).to_motion_command()
+        joints = await self.joints()
+        robot_setup = await self._get_optimizer_setup(tcp=tcp)
         request = wb.models.PlanTrajectoryRequest(
             robot_setup=robot_setup,
             motion_group=self.motion_group_id,
-            start_joint_position=current_joints.joints,
             motion_commands=motion_commands,
+            start_joint_position=joints.joints,
             tcp=tcp,
         )
 
-        motion_api_client = self._api_gateway.motion_api
-        plan_response = await motion_api_client.plan_trajectory(
+        plan_trajectory_response = await self._motion_api_client.plan_trajectory(
             cell=self._cell, plan_trajectory_request=request
         )
-
         if isinstance(
-            plan_response.response.actual_instance, wb.models.PlanTrajectoryFailedResponse
+            plan_trajectory_response.response.actual_instance,
+            wb.models.PlanTrajectoryFailedResponse,
         ):
-            failed_response = plan_response.response.actual_instance
-            raise PlanTrajectoryFailed(failed_response)
-
-        return plan_response.response.actual_instance
+            raise PlanTrajectoryFailed(plan_trajectory_response.response.actual_instance)
+        return plan_trajectory_response.response.actual_instance
 
-    async def run(
+    async def execute(
         self,
-        actions: list[Action] | Action,
+        joint_trajectory: wb.models.JointTrajectory,
         tcp: str,
+        actions: list[Action] | Action | None = None,
         # collision_scene: dts.CollisionScene | None,
-        response_rate_in_ms: int = 200,
         movement_controller: MovementController = move_forward,
         initial_movement_consumer: InitialMovementConsumer | None = None,
     ):
-        if not isinstance(actions, list):
-            actions = [actions]
-
-        if len(actions) == 0:
-            raise ValueError("No actions provided")
+        """Execute a planned motion
 
-        # PLAN MOTION
-        joint_trajectory = await self.plan(actions, tcp)
+        Args:
+            joint_trajectory (wb.models.JointTrajectory): The planned joint trajectory
+            tcp (str): The identifier of the tool center point (TCP)
+            actions (list[Action] | Action | None): The actions to be executed. Defaults to None.
+            movement_controller (MovementController): The movement controller to be used. Defaults to move_forward
+            initial_movement_consumer (InitialMovementConsumer): A consumer for the initial movement
+        """
+        if actions is None:
+            actions = []
+        elif not isinstance(actions, list):
+            actions = [actions]
 
         # LOAD MOTION
         load_plan_response = await self._load_planned_motion(joint_trajectory, tcp)
@@ -96,8 +113,8 @@ async def run(
         number_of_joints = await self._get_number_of_joints()
         joints_velocities = [MAX_JOINT_VELOCITY_PREPARE_MOVE] * number_of_joints
         movement_stream = await self.move_to_start_position(joints_velocities, load_plan_response)
-        if initial_movement_consumer is not None:
-            async for move_to_response in movement_stream:
+        async for move_to_response in movement_stream:
+            if initial_movement_consumer is not None:
                 initial_movement_consumer(move_to_response)
 
         # EXECUTE MOTION
@@ -107,6 +124,10 @@ async def run(
         _movement_controller = movement_controller(movement_controller_context)
         await self._api_gateway.motion_api.execute_trajectory(self._cell, _movement_controller)
 
+    async def plan_and_execute(self, actions: list[Action] | Action, tcp: str):
+        joint_trajectory = await self.plan(actions, tcp)
+        await self.execute(joint_trajectory, tcp, actions=actions)
+
     async def _get_number_of_joints(self) -> int:
         spec = await self._api_gateway.motion_group_infos_api.get_motion_group_specification(
             cell=self._cell, motion_group=self.motion_group_id

tests/core/test_motion_group.py

@@ -21,8 +21,9 @@ async def test_motion_group():
 
     async with controller:
         motion_group = controller[0]
-        state = await motion_group.get_state("Flange")
+        tcp = "Flange"
+        state = await motion_group.get_state(tcp)
         print(state)
 
-        await motion_group.run(actions=actions, tcp="Flange")
+        await motion_group.plan_and_execute(actions, tcp)
         assert True