From 69da34055c00ab5c5917b6e32e1c2fa6e8f9dc16 Mon Sep 17 00:00:00 2001 From: Christoph Biering <1353438+biering@users.noreply.github.com> Date: Mon, 23 Dec 2024 13:57:15 +0100 Subject: [PATCH] feat(RPS-1027): separated plan and execute in motion group (#16) Co-authored-by: cbiering --- examples/02_plan_and_execute.py | 5 +- examples/03_move_and_set_ios.py | 14 +--- examples/04_move_multiple_robots.py | 4 +- .../05_selection_motion_group_activation.py | 8 +-- nova/core/motion_group.py | 71 ++++++++++++------- tests/core/test_motion_group.py | 5 +- 6 files changed, 60 insertions(+), 47 deletions(-) diff --git a/examples/02_plan_and_execute.py b/examples/02_plan_and_execute.py index deb5e87..ef3453d 100644 --- a/examples/02_plan_and_execute.py +++ b/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__": diff --git a/examples/03_move_and_set_ios.py b/examples/03_move_and_set_ios.py index 821834d..9f214ec 100644 --- a/examples/03_move_and_set_ios.py +++ b/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__": diff --git a/examples/04_move_multiple_robots.py b/examples/04_move_multiple_robots.py index f85c230..f9313fb 100644 --- a/examples/04_move_multiple_robots.py +++ b/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(): diff --git a/examples/05_selection_motion_group_activation.py b/examples/05_selection_motion_group_activation.py index aaf7138..4de61e2 100644 --- a/examples/05_selection_motion_group_activation.py +++ b/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: diff --git a/nova/core/motion_group.py b/nova/core/motion_group.py index 1cd9dbd..8d6d747 100644 --- a/nova/core/motion_group.py +++ b/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 diff --git a/tests/core/test_motion_group.py b/tests/core/test_motion_group.py index 9b8b7d0..1403874 100644 --- a/tests/core/test_motion_group.py +++ b/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