From 98930a6d3f56029b9bb30992146a78b7cd87af90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Mon, 25 Nov 2024 18:26:27 +0100 Subject: [PATCH 1/8] Add simple task benchmarking MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../RoboticManipulation.prefab | 375 +----------------- Project/Prefabs/apple.prefab | 63 +-- ros2_ws/src/rai_mani/package.xml | 18 + ros2_ws/src/rai_mani/rai_mani/__init__.py | 0 ros2_ws/src/rai_mani/rai_mani/benchmark.py | 194 +++++++++ ros2_ws/src/rai_mani/resource/rai_mani | 0 ros2_ws/src/rai_mani/setup.cfg | 4 + ros2_ws/src/rai_mani/setup.py | 26 ++ ros2_ws/src/rai_mani/test/test_copyright.py | 25 ++ ros2_ws/src/rai_mani/test/test_flake8.py | 25 ++ ros2_ws/src/rai_mani/test/test_pep257.py | 23 ++ 11 files changed, 345 insertions(+), 408 deletions(-) create mode 100644 ros2_ws/src/rai_mani/package.xml create mode 100644 ros2_ws/src/rai_mani/rai_mani/__init__.py create mode 100644 ros2_ws/src/rai_mani/rai_mani/benchmark.py create mode 100644 ros2_ws/src/rai_mani/resource/rai_mani create mode 100644 ros2_ws/src/rai_mani/setup.cfg create mode 100644 ros2_ws/src/rai_mani/setup.py create mode 100644 ros2_ws/src/rai_mani/test/test_copyright.py create mode 100644 ros2_ws/src/rai_mani/test/test_flake8.py create mode 100644 ros2_ws/src/rai_mani/test/test_pep257.py diff --git a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab index 6e2ee9f..cb89290 100755 --- a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab +++ b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab @@ -74,17 +74,7 @@ }, "EditorEntitySortComponent": { "$type": "EditorEntitySortComponent", - "Id": 10415323403644773154, - "Child Entity Order": [ - "Instance_[31862996159500]/ContainerEntity", - "Instance_[4776417133434]/ContainerEntity", - "Instance_[28506111443834]/ContainerEntity", - "Instance_[665169744802]/ContainerEntity", - "Instance_[21971102006665]/ContainerEntity", - "Instance_[21988281875849]/ContainerEntity", - "Instance_[28142970011017]/ContainerEntity", - "Instance_[22142900698505]/ContainerEntity" - ] + "Id": 10415323403644773154 }, "EditorInspectorComponent": { "$type": "EditorInspectorComponent", @@ -106,6 +96,24 @@ "$type": "EditorVisibilityComponent", "Id": 17635043088532716457 }, + "ROS2SpawnerEditorComponent": { + "$type": "ROS2SpawnerEditorComponent", + "Id": 17096929799984361852, + "Controller": { + "Configuration": { + "Editor entity id": "Entity_[20588120919860]", + "Spawnables": { + "apple": { + "assetId": { + "guid": "{907BD09D-7E5D-5322-82BF-7BE3242A1EF6}", + "subId": 1196145064 + }, + "assetHint": "prefabs/apple.spawnable" + } + } + } + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 6284558809503441846, @@ -1026,296 +1034,6 @@ } }, "Instances": { - "Instance_[21971102006665]": { - "Source": "Prefabs/red_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.792099952697754 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.1538021564483643 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113156914710999 - } - ] - }, - "Instance_[21988281875849]": { - "Source": "Prefabs/green_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.0950982570648193 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.513070821762085 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113155126571655 - } - ] - }, - "Instance_[22142900698505]": { - "Source": "Prefabs/blue_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9361906051635744 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 3.1104562282562256 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113156318664551 - } - ] - }, - "Instance_[28142970011017]": { - "Source": "Prefabs/black_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.990317106246948 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.8433172702789307 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113155126571655 - } - ] - }, - "Instance_[28506111443834]": { - "Source": "Prefabs/carrot.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9181195497512815 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.309497117996216 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.725767195224762 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[31862996159500]": { - "Source": "Prefabs/tomato.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.007845401763916 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.221667766571045 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7324118614196777 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/UniformScale", - "value": 1.2999999523162842 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[4776417133434]": { - "Source": "Prefabs/corn.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9860502481460571 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.6697561740875244 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.731939435005188 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/0", - "value": -174.7096405029297 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/1", - "value": 89.15192413330078 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/2", - "value": -90.00092315673828 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/UniformScale", - "value": 1.7000000476837158 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, "Instance_[6618318911619]": { "Source": "Assets/SceneAssets.prefab", "Patches": [ @@ -1326,61 +1044,6 @@ } ] }, - "Instance_[665169744802]": { - "Source": "Prefabs/apple.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.1052191257476807 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.648794174194336 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7312459945678711 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, "Instance_[712990187248]": { "Source": "Prefabs/PandaRobot.prefab", "Patches": [ diff --git a/Project/Prefabs/apple.prefab b/Project/Prefabs/apple.prefab index 0e834e1..d3b041e 100755 --- a/Project/Prefabs/apple.prefab +++ b/Project/Prefabs/apple.prefab @@ -15,7 +15,7 @@ "$type": "EditorEntitySortComponent", "Id": 10306973598365841266, "Child Entity Order": [ - "Entity_[534760181612]" + "Entity_[27267717522721]" ] }, "EditorInspectorComponent": { @@ -162,60 +162,19 @@ "$type": "EditorVisibilityComponent", "Id": 17099443271266381888 }, - "TransformComponent": { - "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", - "Id": 17921041119350952090, - "Parent Entity": "Entity_[534760181612]" - } - } - }, - "Entity_[534760181612]": { - "Id": "Entity_[534760181612]", - "Name": "apple", - "Components": { - "EditorDisabledCompositionComponent": { - "$type": "EditorDisabledCompositionComponent", - "Id": 18044469881547845778 - }, - "EditorEntityIconComponent": { - "$type": "EditorEntityIconComponent", - "Id": 12292306854165822588 - }, - "EditorEntitySortComponent": { - "$type": "EditorEntitySortComponent", - "Id": 8314993489357136941, - "Child Entity Order": [ - "Entity_[27267717522721]" - ] - }, - "EditorInspectorComponent": { - "$type": "EditorInspectorComponent", - "Id": 17137814805974204638, - "ComponentOrderEntryArray": [ - { - "ComponentId": 8647185006201569915 - } - ] - }, - "EditorLockComponent": { - "$type": "EditorLockComponent", - "Id": 4475825274964774522 - }, - "EditorOnlyEntityComponent": { - "$type": "EditorOnlyEntityComponent", - "Id": 2442758587829817613 - }, - "EditorPendingCompositionComponent": { - "$type": "EditorPendingCompositionComponent", - "Id": 11225710462403311240 - }, - "EditorVisibilityComponent": { - "$type": "EditorVisibilityComponent", - "Id": 16780360982239074981 + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 9671399038384703516, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", - "Id": 8647185006201569915, + "Id": 17921041119350952090, "Parent Entity": "ContainerEntity", "Transform Data": { "UniformScale": 0.05000000074505806 diff --git a/ros2_ws/src/rai_mani/package.xml b/ros2_ws/src/rai_mani/package.xml new file mode 100644 index 0000000..70cb158 --- /dev/null +++ b/ros2_ws/src/rai_mani/package.xml @@ -0,0 +1,18 @@ + + + + rai_mani + 0.0.0 + TODO: Package description + kdabrowski + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2_ws/src/rai_mani/rai_mani/__init__.py b/ros2_ws/src/rai_mani/rai_mani/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/rai_mani/rai_mani/benchmark.py b/ros2_ws/src/rai_mani/rai_mani/benchmark.py new file mode 100644 index 0000000..bbe94f9 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/benchmark.py @@ -0,0 +1,194 @@ +import rclpy +from rclpy.node import Node +from rclpy.client import Client +from gazebo_msgs.srv import SpawnEntity, DeleteEntity +from rclpy.task import Future +from rclpy.executors import MultiThreadedExecutor + +from std_msgs.msg import String +from moveit import moveit +from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped + +from tf2_ros import Buffer, TransformListener +import tf2_geometry_msgs + +from rai_interfaces.srv import ManipulatorMoveTo + +class ScenarioManager(Node): + def __init__(self, scenario_type): + super().__init__('scenario_manager') + self.scenario_type = scenario_type + + self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity') + self.delete_client = self.create_client(DeleteEntity, '/delete_entity') + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) + + while not self.spawn_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + while not self.delete_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + self.timer = self.create_timer(0.5, self.timer_callback) + self.scenario = None + + def timer_callback(self): + if self.scenario == None: + self.scenario = self.scenario_type(self.spawn_client, self.delete_client, self) + self.scenario._pre_step() + progress, terminated = self.scenario.step() + self.get_logger().info(f'Task progress: {progress}') + if terminated: + self.get_logger().info('Scenario terminated') + self.scenario = None + +class Scenario: + def __init__(self, spawn_client: Client, delete_client: Client, node: ScenarioManager): + self.spawn_client = spawn_client + self.delete_client = delete_client + self.node = node + self.entities = {} + self.steps = 0 + + self.reset() + + def __del__(self): + for name in self.entities: + self.delete_entity(name) + + def reset(self): + self.steps = 0 + for name in self.entities: + self.delete_entity(name) + self.entities = {} + + def spawn_entity(self, prefab_name: str, name: str, pose: Pose): + req = SpawnEntity.Request() + req.name = prefab_name + req.xml = '' + req.robot_namespace = name + req.initial_pose = pose + + self.spawn_client.call_async(req).add_done_callback(lambda future: self.entity_spawned_callback(future, name)) + + def delete_entity(self, name: str): + req = DeleteEntity.Request() + req.name = self.entities[name] + + self.delete_client.call_async(req) + + def get_entity_pose(self, name: str): + pose = PoseStamped() + entity_frame = name + '/' + pose.header.frame_id = entity_frame + pose = self.node.tf2_buffer.transform(pose, entity_frame + 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + return pose.pose + + def _pre_step(self): + self.steps += 1 + + def step(self): + return 0.0, False + + def entity_spawned_callback(self, future: Future, name: str): + result = future.result() + if result.success: + self.node.get_logger().info(f'Entity spawned: {name} ({result.status_message})') + self.entities[name] = result.status_message + else: + self.node.get_logger().error(f'Failed to spawn entity: {result.status_message}') + +class TestScenario(Scenario): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + super().__init__(spawn_client, delete_client, node) + self.manipulator_client = node.create_client(ManipulatorMoveTo, '/manipulator_move_to') + self.manipulator_busy = False + + def reset(self): + super().reset() + + for i in range(5): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=float(i)/10 + 0.15, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + + self.spawn_entity('apple', f'apple{i}', pose_transformed.pose) + + self.manipulator_busy = False + self.manipulator_queue = [] + + def pose_transformed(self, pose: Pose): + pose_stamped = PoseStamped() + pose_stamped.pose = pose + pose_stamped.header.frame_id = 'odom' + pose = self.node.tf2_buffer.transform(pose_stamped, 'world', timeout=rclpy.time.Duration(seconds=5.0)).pose + pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + return pose + + def move_to_the_left(self, name: str): + pose = self.get_entity_pose(name) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose.position.y -= 0.6 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def calculate_progress(self): + return sum(1 for name in self.entities if self.pose_transformed(self.get_entity_pose(name)).position.y < 0.0) / len(self.entities) + + def step(self): + if len(self.entities) == 0: # The entities are not spawned yet + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0: + for name in self.entities: + pose = self.pose_transformed(self.get_entity_pose(name)) + if pose.position.y > 0.0: + self.move_to_the_left(name) + break + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 1.0 and not self.manipulator_busy + + +def main(args=None): + rclpy.init(args=args) + + manager = ScenarioManager(TestScenario) + + executor = MultiThreadedExecutor(2) + executor.add_node(manager) + executor.spin() + + manager.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ros2_ws/src/rai_mani/resource/rai_mani b/ros2_ws/src/rai_mani/resource/rai_mani new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/rai_mani/setup.cfg b/ros2_ws/src/rai_mani/setup.cfg new file mode 100644 index 0000000..74703e5 --- /dev/null +++ b/ros2_ws/src/rai_mani/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rai_mani +[install] +install_scripts=$base/lib/rai_mani diff --git a/ros2_ws/src/rai_mani/setup.py b/ros2_ws/src/rai_mani/setup.py new file mode 100644 index 0000000..147d258 --- /dev/null +++ b/ros2_ws/src/rai_mani/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'rai_mani' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='kdabrowski', + maintainer_email='kacper.dabrowski@robotec.ai', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'benchmark = rai_mani.benchmark:main' + ], + }, +) diff --git a/ros2_ws/src/rai_mani/test/test_copyright.py b/ros2_ws/src/rai_mani/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ros2_ws/src/rai_mani/test/test_flake8.py b/ros2_ws/src/rai_mani/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ros2_ws/src/rai_mani/test/test_pep257.py b/ros2_ws/src/rai_mani/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From bcd5d6f75d89caf8a33eccad61d519aa77458665 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 3 Jan 2025 13:31:15 +0100 Subject: [PATCH 2/8] Running the agent automatically on scenarios MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- ros2_ws/src/rai_mani/rai_mani/benchmark.py | 181 +----------------- ros2_ws/src/rai_mani/rai_mani/manager.py | 145 ++++++++++++++ .../rai_mani/scenarios/move_to_the_left.py | 102 ++++++++++ .../rai_mani/scenarios/place_on_top.py | 118 ++++++++++++ .../rai_mani/scenarios/scenario_base.py | 103 ++++++++++ ros2_ws/src/rai_mani/rai_mani/showcase.py | 23 +++ 6 files changed, 496 insertions(+), 176 deletions(-) create mode 100644 ros2_ws/src/rai_mani/rai_mani/manager.py create mode 100644 ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py create mode 100644 ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py create mode 100644 ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py create mode 100644 ros2_ws/src/rai_mani/rai_mani/showcase.py diff --git a/ros2_ws/src/rai_mani/rai_mani/benchmark.py b/ros2_ws/src/rai_mani/rai_mani/benchmark.py index bbe94f9..e595fae 100644 --- a/ros2_ws/src/rai_mani/rai_mani/benchmark.py +++ b/ros2_ws/src/rai_mani/rai_mani/benchmark.py @@ -1,187 +1,16 @@ import rclpy -from rclpy.node import Node -from rclpy.client import Client -from gazebo_msgs.srv import SpawnEntity, DeleteEntity -from rclpy.task import Future -from rclpy.executors import MultiThreadedExecutor - -from std_msgs.msg import String -from moveit import moveit -from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped - -from tf2_ros import Buffer, TransformListener -import tf2_geometry_msgs - -from rai_interfaces.srv import ManipulatorMoveTo - -class ScenarioManager(Node): - def __init__(self, scenario_type): - super().__init__('scenario_manager') - self.scenario_type = scenario_type - - self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity') - self.delete_client = self.create_client(DeleteEntity, '/delete_entity') - self.tf2_buffer = Buffer() - self.tf2_listener = TransformListener(self.tf2_buffer, self) - - while not self.spawn_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - while not self.delete_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - self.timer = self.create_timer(0.5, self.timer_callback) - self.scenario = None - - def timer_callback(self): - if self.scenario == None: - self.scenario = self.scenario_type(self.spawn_client, self.delete_client, self) - self.scenario._pre_step() - progress, terminated = self.scenario.step() - self.get_logger().info(f'Task progress: {progress}') - if terminated: - self.get_logger().info('Scenario terminated') - self.scenario = None - -class Scenario: - def __init__(self, spawn_client: Client, delete_client: Client, node: ScenarioManager): - self.spawn_client = spawn_client - self.delete_client = delete_client - self.node = node - self.entities = {} - self.steps = 0 - - self.reset() - - def __del__(self): - for name in self.entities: - self.delete_entity(name) - - def reset(self): - self.steps = 0 - for name in self.entities: - self.delete_entity(name) - self.entities = {} - - def spawn_entity(self, prefab_name: str, name: str, pose: Pose): - req = SpawnEntity.Request() - req.name = prefab_name - req.xml = '' - req.robot_namespace = name - req.initial_pose = pose - - self.spawn_client.call_async(req).add_done_callback(lambda future: self.entity_spawned_callback(future, name)) - - def delete_entity(self, name: str): - req = DeleteEntity.Request() - req.name = self.entities[name] - - self.delete_client.call_async(req) - - def get_entity_pose(self, name: str): - pose = PoseStamped() - entity_frame = name + '/' - pose.header.frame_id = entity_frame - pose = self.node.tf2_buffer.transform(pose, entity_frame + 'odom', timeout=rclpy.time.Duration(seconds=5.0)) - return pose.pose - - def _pre_step(self): - self.steps += 1 - def step(self): - return 0.0, False +from rai_mani.manager import RaiBenchmarkManager - def entity_spawned_callback(self, future: Future, name: str): - result = future.result() - if result.success: - self.node.get_logger().info(f'Entity spawned: {name} ({result.status_message})') - self.entities[name] = result.status_message - else: - self.node.get_logger().error(f'Failed to spawn entity: {result.status_message}') - -class TestScenario(Scenario): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): - super().__init__(spawn_client, delete_client, node) - self.manipulator_client = node.create_client(ManipulatorMoveTo, '/manipulator_move_to') - self.manipulator_busy = False - - def reset(self): - super().reset() - - for i in range(5): - pose = PoseStamped() - pose.header.frame_id = 'world' - pose.pose.position = Point(x=0.4, y=float(i)/10 + 0.15, z=0.1) - pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - - pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) - - self.spawn_entity('apple', f'apple{i}', pose_transformed.pose) - - self.manipulator_busy = False - self.manipulator_queue = [] - - def pose_transformed(self, pose: Pose): - pose_stamped = PoseStamped() - pose_stamped.pose = pose - pose_stamped.header.frame_id = 'odom' - pose = self.node.tf2_buffer.transform(pose_stamped, 'world', timeout=rclpy.time.Duration(seconds=5.0)).pose - pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) - return pose - - def move_to_the_left(self, name: str): - pose = self.get_entity_pose(name) - pose.position.z += 0.1 - - req = ManipulatorMoveTo.Request() - req.initial_gripper_state = True - req.target_pose.pose = self.pose_transformed(pose) - req.final_gripper_state = False - self.manipulator_queue.append(req) - - pose.position.y -= 0.6 - req = ManipulatorMoveTo.Request() - req.initial_gripper_state = False - req.target_pose.pose = self.pose_transformed(pose) - req.final_gripper_state = True - self.manipulator_queue.append(req) - - def move_callback(self, future: Future): - result = future.result() - if result.success: - self.node.get_logger().debug(f'Move performed') - else: - self.node.get_logger().error(f'Failed to perform move') - self.manipulator_busy = False - - def calculate_progress(self): - return sum(1 for name in self.entities if self.pose_transformed(self.get_entity_pose(name)).position.y < 0.0) / len(self.entities) - - def step(self): - if len(self.entities) == 0: # The entities are not spawned yet - return 0.0, False - - progress = self.calculate_progress() - - if not self.manipulator_busy: - if len(self.manipulator_queue) == 0: - for name in self.entities: - pose = self.pose_transformed(self.get_entity_pose(name)) - if pose.position.y > 0.0: - self.move_to_the_left(name) - break - - if len(self.manipulator_queue) > 0: - req = self.manipulator_queue.pop(0) - self.manipulator_busy = True - self.manipulator_client.call_async(req).add_done_callback(self.move_callback) - - return progress, progress >= 1.0 and not self.manipulator_busy +from rai_mani.scenarios.move_to_the_left import MoveToTheLeft +from rai_mani.scenarios.place_on_top import PlaceOnTop +from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) - manager = ScenarioManager(TestScenario) + manager = RaiBenchmarkManager([MoveToTheLeft, PlaceOnTop]) executor = MultiThreadedExecutor(2) executor.add_node(manager) diff --git a/ros2_ws/src/rai_mani/rai_mani/manager.py b/ros2_ws/src/rai_mani/rai_mani/manager.py new file mode 100644 index 0000000..32fa339 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/manager.py @@ -0,0 +1,145 @@ +import rclpy +from rclpy.node import Node +from gazebo_msgs.srv import SpawnEntity, DeleteEntity +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion + +from tf2_ros import Buffer, TransformListener + +from rai_interfaces.srv import ManipulatorMoveTo + +from langchain_core.messages import HumanMessage + +from rai.agents.conversational_agent import create_conversational_agent +from rai.node import RaiBaseNode +from rai.tools.ros.manipulation import GetObjectPositionsTool, MoveToPointTool +from rai.tools.ros.native import GetCameraImage, Ros2GetTopicsNamesAndTypesTool +from rai.utils.model_initialization import get_llm_model + +from threading import Thread + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class ScenarioManager(Node): + """ + A class responsible for playing the scenarios + """ + def __init__(self, scenario_types): + """ + Initializes the ScenarioManager + + Args: + scenario_types: A list of scenario classes to play + """ + super().__init__('scenario_manager') + self.scenario_types = scenario_types + + self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity') + self.delete_client = self.create_client(DeleteEntity, '/delete_entity') + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) + + while not self.spawn_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + while not self.delete_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + self.timer = self.create_timer(1.0, self.timer_callback) + self.scenario: ScenarioBase = None + self.current_scenario = 0 + self.agent_thread: Thread = None + self.manipulator_ready = False + self.scores = [] + + def _init_scenario(self): + self.scenario = self.scenario_types[self.current_scenario](self.spawn_client, self.delete_client, self) + self.manipulator_ready = False + request = ManipulatorMoveTo.Request() + request.target_pose.pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + request.target_pose.pose.position = Point(x=0.2, y=0.0, z=0.2) + def callback(future: Future): + self.manipulator_ready = True + self.scenario.reset() + self.scenario.manipulator_client.call_async(request).add_done_callback(callback) + + def _terminate_scenario(self): + self.get_logger().info(f'Scenario terminated with score {self.scores[-1]}') + self.scenario = None + if self.current_scenario == len(self.scenario_types) - 1: + self.get_logger().info(f'All scenarios are completed, with scores: {self.scores}') + self.executor.shutdown() + self.current_scenario = (self.current_scenario + 1) % len(self.scenario_types) + self.manipulator_ready = False + + def timer_callback(self): + if self.scenario == None: + self._init_scenario() + + if not self.manipulator_ready: + return + + progress, terminated = self.scenario.step() + self.get_logger().info(f'Task progress: {progress}') + if terminated and not (self.agent_thread and self.agent_thread.is_alive()): + self.scores.append(progress) + self._terminate_scenario() + return + if self.agent_thread and not self.agent_thread.is_alive(): + self.get_logger().info('Agent failed to fulfill the task, terminating the scenario.') + self.scores.append(progress) + self._terminate_scenario() + +class RaiBenchmarkManager(ScenarioManager): + """ + A class responsible for playing the scenarios and running the conversational agent for each scenario + """ + def __init__(self, scenario_types): + super().__init__(scenario_types) + self.agent = None + + def _init_scenario(self): + super()._init_scenario() + + self.rai_node = RaiBaseNode(node_name="manipulation_demo") + self.rai_node.declare_parameter("conversion_ratio", 1.0) + self.rai_node.qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE + + tools = [ + GetObjectPositionsTool( + node=self.rai_node, + target_frame="panda_link0", + source_frame="RGBDCamera5", + camera_topic="/color_image5", + depth_topic="/depth_image5", + camera_info_topic="/color_camera_info5", + ), + MoveToPointTool(node=self.rai_node, manipulator_frame="panda_link0"), + GetCameraImage(node=self.rai_node), + Ros2GetTopicsNamesAndTypesTool(node=self.rai_node), + ] + + llm = get_llm_model(model_type="complex_model") + + system_prompt = """ + You are a robotic arm with interfaces to detect and manipulate objects. + Here are the coordinates information: + x - front to back (positive is forward) + y - left to right (positive is right) + z - up to down (positive is up) + + Before starting the task, make sure to grab the camera image to understand the environment. + """ + + self.agent = create_conversational_agent( + llm=llm, + tools=tools, + system_prompt=system_prompt, + ) + + def run_agent(): + self.agent.invoke({"messages": [HumanMessage(content=self.scenario.get_prompt())]})["messages"][-1].pretty_print() + + self.agent_thread = Thread(target=run_agent) + self.agent_thread.start() + diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py new file mode 100644 index 0000000..9280962 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py @@ -0,0 +1,102 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class MoveToTheLeft(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + super().__init__(spawn_client, delete_client, node) + + def get_prompt(self): + return "Please move all the apples to the left side of the table, about -0.30 in Y direction." + + def reset(self): + super().reset() + + for i in range(5): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=float(i)/10 + 0.15, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + + prefab = 'apple' + self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) + + def calculate_progress(self): + num_apples = sum(1 for name in self.entities if name.startswith('apple')) + num_good_apples = sum(1 for name in self.entities if name.startswith('apple') and self.pose_transformed(self.get_entity_pose(name)).position.y < 0.0) + return num_good_apples / num_apples + + def step(self): + if len(self.entities) == 0: # The entities are not spawned yet + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 1.0 + +class MoveToTheLeftAuto(MoveToTheLeft): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + super().__init__(spawn_client, delete_client, node) + self.manipulator_busy = False + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def move_to_the_left(self, name: str): + pose = self.get_entity_pose(name) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose.position.y -= 0.6 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: # The entities are not spawned yet + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0: + for name in self.entities: + pose = self.pose_transformed(self.get_entity_pose(name)) + if pose.position.y > 0.0: + self.node.get_logger().info(f'Moving {name} to the left from {pose.position.y}') + self.move_to_the_left(name) + break + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 1.0 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py new file mode 100644 index 0000000..256f514 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py @@ -0,0 +1,118 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class PlaceOnTop(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + self.top_object = None + self.bot_object = None + + super().__init__(spawn_client, delete_client, node) + + def get_prompt(self): + return "Place the yellow cube on top of the blue cube. Remember to increase the Z position of the 'drop' task by around 0.2 to avoid collision." + + def reset(self): + super().reset() + + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + for i in range(5): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=float(i)/8 - 0.25, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + + prefab = prefabs[i % len(prefabs)] + if prefab == 'yellow_cube': + self.top_object = f'{prefab}{i}' + elif prefab == 'blue_cube': + self.bot_object = f'{prefab}{i}' + + self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) + + def calculate_progress(self): + if self.top_object == None or self.bot_object == None: + return 0.0 + + top_pose = self.pose_transformed(self.get_entity_pose(self.top_object)) + bot_pose = self.pose_transformed(self.get_entity_pose(self.bot_object)) + + goal_position = bot_pose.position + goal_position.z += 0.05 + + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2)**0.5 + + return max(0.0, 1.0 - 10.0 * distance(top_pose.position, goal_position)) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.8 and not self.manipulator_busy + +class PlaceOnTopAuto(PlaceOnTop): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def place_on_top(self, bot_object: str, top_object: str): + pose = self.get_entity_pose(top_object) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose = self.get_entity_pose(bot_object) + pose.position.z += 0.2 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.place_on_top(self.bot_object, self.top_object) + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py new file mode 100644 index 0000000..54662a5 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py @@ -0,0 +1,103 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node + +from gazebo_msgs.srv import SpawnEntity, DeleteEntity +from rclpy.task import Future +from geometry_msgs.msg import Pose, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +class ScenarioBase: + """ + Base class for a scenario. A scenario is a task that the agent has to perform. + """ + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + self.spawn_client = spawn_client + self.delete_client = delete_client + self.manipulator_client = node.create_client(ManipulatorMoveTo, '/manipulator_move_to') + self.node = node + self.entities: dict[str, str] = {} + + def __del__(self): + for name in self.entities: + self.delete_entity(name) + + def get_prompt(self) -> str: + """ + Returns a prompt that describes the task that the agent has to perform. + """ + return "Please do something interesting" + + def reset(self) -> None: + """ + Resets the scenario to its initial state. + + This method is called before the scenario is started. + """ + + for name in self.entities: + self.delete_entity(name) + self.entities = {} + + def spawn_entity(self, prefab_name: str, name: str, pose: Pose) -> None: + """ + Spawns an entity in the simulation. + + Args: + prefab_name: The name of the prefab to spawn. + name: The name of the entity, by which it can be later referenced. + pose: The pose of the entity. + """ + req = SpawnEntity.Request() + req.name = prefab_name + req.xml = '' + req.robot_namespace = name + req.initial_pose = pose + + self.spawn_client.call_async(req).add_done_callback(lambda future: self.entity_spawned_callback(future, name)) + + def delete_entity(self, name: str) -> None: + req = DeleteEntity.Request() + req.name = self.entities[name] + + self.delete_client.call_async(req) + + def get_entity_pose(self, name: str) -> Pose: + """ + Returns the pose of an entity. + """ + pose = PoseStamped() + entity_frame = name + '/' + pose.header.frame_id = entity_frame + pose = self.node.tf2_buffer.transform(pose, entity_frame + 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + return pose.pose + + def pose_transformed(self, pose: Pose) -> Pose: + """ + Transforms the pose into the frame of the panda robot. + """ + pose_stamped = PoseStamped() + pose_stamped.pose = pose + pose_stamped.header.frame_id = 'odom' + pose = self.node.tf2_buffer.transform(pose_stamped, 'world', timeout=rclpy.time.Duration(seconds=5.0)).pose + pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + return pose + + def step(self) -> tuple[float, bool]: + """ + Performs a step in the scenario. + + Returns: + progress: A float between 0.0 and 1.0 that represents the progress of the task. + terminated: A boolean that indicates whether the task is terminated. + """ + return 0.0, False + + def entity_spawned_callback(self, future: Future, name: str): + result = future.result() + if result.success: + self.node.get_logger().info(f'Entity spawned: {name} ({result.status_message})') + self.entities[name] = result.status_message + else: + self.node.get_logger().error(f'Failed to spawn entity: {result.status_message}') \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/showcase.py b/ros2_ws/src/rai_mani/rai_mani/showcase.py new file mode 100644 index 0000000..96c91e3 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/showcase.py @@ -0,0 +1,23 @@ +import rclpy + +from rai_mani.manager import ScenarioManager + +from rai_mani.scenarios.move_to_the_left import MoveToTheLeftAuto +from rai_mani.scenarios.place_on_top import PlaceOnTopAuto + +from rclpy.executors import MultiThreadedExecutor + +def main(args=None): + rclpy.init(args=args) + + manager = ScenarioManager([MoveToTheLeftAuto, PlaceOnTopAuto]) + + executor = MultiThreadedExecutor(2) + executor.add_node(manager) + executor.spin() + + manager.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 67485190bc4c79c7c90d8f45072ef9175277c8eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 3 Jan 2025 13:46:17 +0100 Subject: [PATCH 3/8] Add showcase to the setup script MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- ros2_ws/src/rai_mani/setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2_ws/src/rai_mani/setup.py b/ros2_ws/src/rai_mani/setup.py index 147d258..ad1e520 100644 --- a/ros2_ws/src/rai_mani/setup.py +++ b/ros2_ws/src/rai_mani/setup.py @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'benchmark = rai_mani.benchmark:main' + 'benchmark = rai_mani.benchmark:main', + 'showcase = rai_mani.showcase:main' ], }, ) From d31d1e023bfa58f6e18070319c96bf6791628252 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 3 Jan 2025 13:48:21 +0100 Subject: [PATCH 4/8] Fix place_on_top scenario MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py index 256f514..c886bab 100644 --- a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py @@ -60,7 +60,7 @@ def step(self): progress = self.calculate_progress() - return progress, progress >= 0.8 and not self.manipulator_busy + return progress, progress >= 0.8 class PlaceOnTopAuto(PlaceOnTop): def __init__(self, spawn_client: Client, delete_client: Client, node: Node): From 33cfeaf507b08d8b65825ca972c3bc626a49ea3d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 7 Jan 2025 17:48:31 +0100 Subject: [PATCH 5/8] Add ReplaceTypes scenario MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../rai_mani/scenarios/replace_types.py | 178 ++++++++++++++++++ ros2_ws/src/rai_mani/rai_mani/showcase.py | 3 +- 2 files changed, 180 insertions(+), 1 deletion(-) create mode 100644 ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py new file mode 100644 index 0000000..b4d924a --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py @@ -0,0 +1,178 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class ReplaceTypes(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + self.vegetable_poses = [] + self.toy_poses = [] + self.current_index = 0 + + super().__init__(spawn_client, delete_client, node) + + def get_prompt(self): + return "Replace the objects in such a way that all the toys are in the places of vegetables and vice versa." + + def reset(self): + super().reset() + + self.vegetable_poses = [] + self.toy_poses = [] + self.current_index = 0 + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + for i in range(4): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=float(i)/8 - 0.25, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + + prefab = prefabs[i % len(prefabs)] + + self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) + if self._get_type(prefab) == 'vegetable': + self.vegetable_poses.append(pose.pose) + elif self._get_type(prefab) == 'toy': + self.toy_poses.append(pose.pose) + + def _get_type(self, object_name: str): + if object_name.startswith('apple') or object_name.startswith('carrot'): + return 'vegetable' + elif object_name.startswith('yellow_cube') or object_name.startswith('blue_cube'): + return 'toy' + else: + return None + + def _min_distance(self, entity: str): + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2)**0.5 + pose = self.pose_transformed(self.get_entity_pose(entity)) + min_distance = float('inf') + if self._get_type(entity) == 'vegetable': + for toy_pose in self.toy_poses: + min_distance = min(min_distance, distance(pose.position, toy_pose.position)) + elif self._get_type(entity) == 'toy': + for vegetable_pose in self.vegetable_poses: + min_distance = min(min_distance, distance(pose.position, vegetable_pose.position)) + return min_distance + + def calculate_progress(self): + if self.vegetable_poses == [] or self.toy_poses == []: + return 0.0 + + progress = 0.0 + + for entity in self.entities: + progress += max(0.0, 1.0 - self._min_distance(entity) * 10.0) + + return progress / len(self.entities) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.8 + +class ReplaceTypesAuto(ReplaceTypes): + def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def replace(self, a: str, b: str): + from copy import deepcopy + pose_a = self.pose_transformed(self.get_entity_pose(a)) + pose_a.position.z += 0.1 + + buffer_pose = deepcopy(pose_a) + buffer_pose.position.x = 0.4 + buffer_pose.position.y = -0.5 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = pose_a + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = buffer_pose + req.final_gripper_state = True + self.manipulator_queue.append(req) + + pose_b = self.pose_transformed(self.get_entity_pose(b)) + pose_b.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = pose_b + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = pose_a + req.final_gripper_state = True + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = buffer_pose + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = pose_b + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) < 4: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + vegetables = [] + toys = [] + for entity in self.entities: + if self._get_type(entity) == 'vegetable': + vegetables.append(entity) + elif self._get_type(entity) == 'toy': + toys.append(entity) + + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.replace(vegetables[self.current_index], toys[self.current_index]) + self.current_index = (self.current_index + 1) % len(vegetables) + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/showcase.py b/ros2_ws/src/rai_mani/rai_mani/showcase.py index 96c91e3..5ff09a4 100644 --- a/ros2_ws/src/rai_mani/rai_mani/showcase.py +++ b/ros2_ws/src/rai_mani/rai_mani/showcase.py @@ -4,13 +4,14 @@ from rai_mani.scenarios.move_to_the_left import MoveToTheLeftAuto from rai_mani.scenarios.place_on_top import PlaceOnTopAuto +from rai_mani.scenarios.replace_types import ReplaceTypesAuto from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) - manager = ScenarioManager([MoveToTheLeftAuto, PlaceOnTopAuto]) + manager = ScenarioManager([ReplaceTypesAuto, MoveToTheLeftAuto, PlaceOnTopAuto]) executor = MultiThreadedExecutor(2) executor.add_node(manager) From dee2cfef6ec8c630da7fe2a015820bad32b4e4e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Tue, 7 Jan 2025 17:53:10 +0100 Subject: [PATCH 6/8] Update level and prefabs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../RoboticManipulation.prefab | 429 +++++++++++------- Project/Prefabs/black_cube.prefab | 10 +- Project/Prefabs/blue_cube.prefab | 10 +- Project/Prefabs/carrot.prefab | 10 + Project/Prefabs/corn.prefab | 28 +- Project/Prefabs/green_cube.prefab | 10 +- Project/Prefabs/red_cube.prefab | 10 +- Project/Prefabs/tomato.prefab | 28 +- Project/Prefabs/toy_box.prefab | 10 + 9 files changed, 333 insertions(+), 212 deletions(-) diff --git a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab index cb89290..3855f73 100755 --- a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab +++ b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab @@ -109,6 +109,62 @@ "subId": 1196145064 }, "assetHint": "prefabs/apple.spawnable" + }, + "blue_cube": { + "assetId": { + "guid": "{ABC61B93-E050-5329-A236-10E2F61DD52E}", + "subId": 2673158184 + }, + "assetHint": "prefabs/blue_cube.spawnable" + }, + "carrot": { + "assetId": { + "guid": "{3CAB1590-BD87-5D20-8449-55960FB963DD}", + "subId": 213700765 + }, + "assetHint": "prefabs/carrot.spawnable" + }, + "corn": { + "assetId": { + "guid": "{210B3F78-B336-5F56-B8E8-ED839F4A63D9}", + "subId": 4099978870 + }, + "assetHint": "prefabs/corn.spawnable" + }, + "green_cube": { + "assetId": { + "guid": "{BFD1C1E8-64BB-5E2D-B406-A4B03C0E02FF}", + "subId": 406036830 + }, + "assetHint": "prefabs/green_cube.spawnable" + }, + "red_cube": { + "assetId": { + "guid": "{52687F52-5090-5832-B9B5-26821B074D3B}", + "subId": 1726443591 + }, + "assetHint": "prefabs/red_cube.spawnable" + }, + "tomato": { + "assetId": { + "guid": "{0F88F7B6-2DEF-54D7-A211-BE4DC5290223}", + "subId": 3484548826 + }, + "assetHint": "prefabs/tomato.spawnable" + }, + "toy_box": { + "assetId": { + "guid": "{2AB5818A-C3FB-566F-9FB1-36E95ADFB627}", + "subId": 3956998235 + }, + "assetHint": "prefabs/toy_box.spawnable" + }, + "yellow_cube": { + "assetId": { + "guid": "{183805C8-E2D9-577E-A69A-E11F92395190}", + "subId": 3747011399 + }, + "assetHint": "prefabs/black_cube.spawnable" } } } @@ -1091,168 +1147,6 @@ "op": "remove", "path": "/Entities/Entity_[751644892912]/Components/EditorEntitySortComponent/Child Entity Order/3" }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorArticulationLinkComponent/ArticulationConfiguration/Is Limited", - "value": false - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", - "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", - "value": 1 - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", - "value": "physx/rubber.physicsmaterial" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", - "value": 0.05999999865889549 - }, - { - "op": "add", - "path": "/Entities/Entity_[755939860208]/Components/EditorColliderComponent", - "value": { - "$type": "EditorColliderComponent", - "Id": 13507121597919349583, - "ColliderConfiguration": { - "CollisionLayer": { - "Index": 0 - }, - "CollisionGroupId": { - "GroupId": "{00000000-0000-0000-0000-000000000000}" - }, - "Visible": false, - "Trigger": false, - "Simulated": true, - "DummySimulated": false, - "InSceneQueries": true, - "Exclusive": true, - "Position": [ - 0.0, - -0.004999999888241291, - 0.05000000074505806 - ], - "Rotation": [ - 0.0, - 0.0, - 0.0, - 1.0 - ], - "MaterialSlots": { - "Slots": [ - { - "Name": "Entire object", - "MaterialAsset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "QueueLoad", - "assetHint": "" - } - } - ] - }, - "propertyVisibilityFlags": 255, - "ColliderTag": "", - "RestOffset": 0.0, - "ContactOffset": 0.019999999552965164 - }, - "ShapeConfiguration": { - "ShapeType": 1, - "Sphere": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Radius": 0.5 - }, - "Box": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Configuration": [ - 0.07000000029802322, - 0.009999999776482582, - 0.009999999776482582 - ] - }, - "Capsule": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Height": 1.0, - "Radius": 0.25 - }, - "Cylinder": { - "Configuration": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "CookedData": "", - "Type": 0 - }, - "Subdivision": 16, - "Height": 1.0, - "Radius": 1.0 - }, - "PhysicsAsset": { - "Asset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "QueueLoad", - "assetHint": "" - }, - "Configuration": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "PhysicsAsset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "PreLoad", - "assetHint": "" - }, - "AssetScale": [ - 1.0, - 1.0, - 1.0 - ], - "UseMaterialsFromAsset": true, - "SubdivisionLevel": 4 - } - }, - "HasNonUniformScale": false, - "SubdivisionLevel": 4 - }, - "DebugDrawSettings": { - "LocallyEnabled": true - }, - "ComponentMode": {}, - "HasNonUniformScale": false - } - }, { "op": "replace", "path": "/Entities/Entity_[760234827504]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/AssetScale/0", @@ -1343,11 +1237,6 @@ "path": "/Entities/Entity_[816069402352]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", "value": "assets/urdfimporter/1440394708_panda/finger.dae.azmodel" }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Frame Name", - "value": "RGBDCamera3" - }, { "op": "replace", "path": "/Entities/Entity_[820364369648]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Publish Transform", @@ -2220,8 +2109,205 @@ }, { "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorArticulationLinkComponent/ArticulationConfiguration/Is Limited", - "value": false + "path": "/Entities/Entity_[747349925616]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", + "value": "panda_link2" + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/0/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/1/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/2/1", + "value": -1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/3/1", + "value": -2.450000047683716 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/4/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/6/1", + "value": -0.10000000149011612 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/7/1", + "value": 0.019999999552965164 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/8/1", + "value": 0.019999999552965164 + }, + { + "op": "add", + "path": "/Entities/Entity_[755939860208]/Components/EditorColliderComponent", + "value": { + "$type": "EditorColliderComponent", + "Id": 13507121597919349583, + "ColliderConfiguration": { + "CollisionLayer": { + "Index": 0 + }, + "CollisionGroupId": { + "GroupId": "{00000000-0000-0000-0000-000000000000}" + }, + "Visible": false, + "Trigger": false, + "Simulated": true, + "DummySimulated": false, + "InSceneQueries": true, + "Exclusive": true, + "Position": [ + 0.0, + -0.004999999888241291, + 0.05000000074505806 + ], + "Rotation": [ + 0.0, + 0.0, + 0.0, + 1.0 + ], + "MaterialSlots": { + "Slots": [ + { + "Name": "Entire object", + "MaterialAsset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "QueueLoad", + "assetHint": "" + } + } + ] + }, + "propertyVisibilityFlags": 255, + "ColliderTag": "", + "RestOffset": 0.0, + "ContactOffset": 0.019999999552965164 + }, + "ShapeConfiguration": { + "ShapeType": 1, + "Sphere": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Radius": 0.5 + }, + "Box": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Configuration": [ + 0.07000000029802322, + 0.009999999776482582, + 0.009999999776482582 + ] + }, + "Capsule": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Height": 1.0, + "Radius": 0.25 + }, + "Cylinder": { + "Configuration": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "CookedData": "", + "Type": 0 + }, + "Subdivision": 16, + "Height": 1.0, + "Radius": 1.0 + }, + "PhysicsAsset": { + "Asset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "QueueLoad", + "assetHint": "" + }, + "Configuration": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "PhysicsAsset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "PreLoad", + "assetHint": "" + }, + "AssetScale": [ + 1.0, + 1.0, + 1.0 + ], + "UseMaterialsFromAsset": true, + "SubdivisionLevel": 4 + } + }, + "HasNonUniformScale": false, + "SubdivisionLevel": 4 + }, + "DebugDrawSettings": { + "LocallyEnabled": true + }, + "ComponentMode": {}, + "HasNonUniformScale": false + } + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 1 + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "physx/rubber.physicsmaterial" + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", + "value": 0.05999999865889549 }, { "op": "add", @@ -2384,11 +2470,6 @@ "op": "replace", "path": "/Entities/Entity_[807479467760]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", "value": "panda_leftfinger" - }, - { - "op": "replace", - "path": "/Entities/Entity_[747349925616]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", - "value": "panda_link2" } ] }, diff --git a/Project/Prefabs/black_cube.prefab b/Project/Prefabs/black_cube.prefab index 0c12470..8b73b82 100755 --- a/Project/Prefabs/black_cube.prefab +++ b/Project/Prefabs/black_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[9165443789406]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/blue_cube.prefab b/Project/Prefabs/blue_cube.prefab index 2642738..28198ea 100755 --- a/Project/Prefabs/blue_cube.prefab +++ b/Project/Prefabs/blue_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[1152437262922]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/carrot.prefab b/Project/Prefabs/carrot.prefab index 4d5cf32..85ece79 100755 --- a/Project/Prefabs/carrot.prefab +++ b/Project/Prefabs/carrot.prefab @@ -379,6 +379,16 @@ "$type": "EditorVisibilityComponent", "Id": 11409618612434829098 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 10470715774139935384, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 18391610517830378569, diff --git a/Project/Prefabs/corn.prefab b/Project/Prefabs/corn.prefab index 4323b65..1740612 100755 --- a/Project/Prefabs/corn.prefab +++ b/Project/Prefabs/corn.prefab @@ -226,9 +226,9 @@ }, "Configuration": { "Scale": [ - 0.10199999809265137, - 0.10199999809265137, - 0.10199999809265137 + 0.05999999865889549, + 0.05999999865889549, + 0.05999999865889549 ], "PhysicsAsset": { "assetId": { @@ -260,20 +260,20 @@ "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.0003767325251828879, - -0.0012333319755271077, - -0.0012046174379065633 + -0.0002216073771705851, + -0.0007254894007928669, + -0.0007085984689183533 ], "Inertia tensor": [ - 0.0001341252209385857, + 0.000046410106733674183, 0.0, 0.0, 0.0, - 0.00013628810120280832, + 0.000047158508095890284, 0.0, 0.0, 0.0, - 0.00003456683407421224 + 0.00001196084213006543 ] } }, @@ -281,6 +281,16 @@ "$type": "EditorVisibilityComponent", "Id": 12802023452715689730 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 16221295209102460307, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 10754169405606624831, diff --git a/Project/Prefabs/green_cube.prefab b/Project/Prefabs/green_cube.prefab index 9ca8d53..d48039f 100755 --- a/Project/Prefabs/green_cube.prefab +++ b/Project/Prefabs/green_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[5420232307294]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/red_cube.prefab b/Project/Prefabs/red_cube.prefab index 3f4195d..a79ae4b 100755 --- a/Project/Prefabs/red_cube.prefab +++ b/Project/Prefabs/red_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[485314884190]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/tomato.prefab b/Project/Prefabs/tomato.prefab index f32ffbe..a436eda 100755 --- a/Project/Prefabs/tomato.prefab +++ b/Project/Prefabs/tomato.prefab @@ -107,9 +107,9 @@ }, "Configuration": { "Scale": [ - 0.05199999734759331, - 0.05199999734759331, - 0.05199999734759331 + 0.03999999910593033, + 0.03999999910593033, + 0.03999999910593033 ], "PhysicsAsset": { "assetId": { @@ -141,20 +141,20 @@ "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.00014860315422993153, - 0.00003289386222604662, - 0.0016063579823821783 + -0.00011431012535467744, + 0.00002530297206249088, + 0.001235659932717681 ], "Inertia tensor": [ - 0.00004531557351583615, + 0.000026813961085281335, 0.0, 0.0, 0.0, - 0.0000491143437102437, + 0.000029061740860925056, 0.0, 0.0, 0.0, - 0.000052105468057561666 + 0.00003083164119743742 ] } }, @@ -162,6 +162,16 @@ "$type": "EditorVisibilityComponent", "Id": 4910412103323004378 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 1369596616863894338, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 14610634284382918424, diff --git a/Project/Prefabs/toy_box.prefab b/Project/Prefabs/toy_box.prefab index c350171..903fc13 100755 --- a/Project/Prefabs/toy_box.prefab +++ b/Project/Prefabs/toy_box.prefab @@ -277,6 +277,16 @@ "$type": "EditorVisibilityComponent", "Id": 9097636690885332082 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 5197297354537371193, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 4486871887464016633, From f62b86829c9a0937596957a636a7a14813a0bb6e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jan 2025 14:59:55 +0100 Subject: [PATCH 7/8] Add object position randomisation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- ros2_ws/src/rai_mani/rai_mani/benchmark.py | 3 +- ros2_ws/src/rai_mani/rai_mani/manager.py | 32 +++++++++++++++---- .../rai_mani/scenarios/move_to_the_left.py | 10 +++--- .../rai_mani/scenarios/place_on_top.py | 26 ++++----------- .../rai_mani/scenarios/replace_types.py | 30 +++++++---------- .../rai_mani/scenarios/scenario_base.py | 29 +++++++++++++++-- ros2_ws/src/rai_mani/rai_mani/showcase.py | 2 +- 7 files changed, 78 insertions(+), 54 deletions(-) diff --git a/ros2_ws/src/rai_mani/rai_mani/benchmark.py b/ros2_ws/src/rai_mani/rai_mani/benchmark.py index e595fae..efb8bde 100644 --- a/ros2_ws/src/rai_mani/rai_mani/benchmark.py +++ b/ros2_ws/src/rai_mani/rai_mani/benchmark.py @@ -4,13 +4,14 @@ from rai_mani.scenarios.move_to_the_left import MoveToTheLeft from rai_mani.scenarios.place_on_top import PlaceOnTop +from rai_mani.scenarios.replace_types import ReplaceTypes from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) - manager = RaiBenchmarkManager([MoveToTheLeft, PlaceOnTop]) + manager = RaiBenchmarkManager([PlaceOnTop, MoveToTheLeft, ReplaceTypes], list(range(3))) executor = MultiThreadedExecutor(2) executor.add_node(manager) diff --git a/ros2_ws/src/rai_mani/rai_mani/manager.py b/ros2_ws/src/rai_mani/rai_mani/manager.py index 32fa339..feb9064 100644 --- a/ros2_ws/src/rai_mani/rai_mani/manager.py +++ b/ros2_ws/src/rai_mani/rai_mani/manager.py @@ -21,22 +21,27 @@ from rai_mani.scenarios.scenario_base import ScenarioBase +import random + class ScenarioManager(Node): """ A class responsible for playing the scenarios """ - def __init__(self, scenario_types): + def __init__(self, scenario_types, seeds = []): """ Initializes the ScenarioManager Args: scenario_types: A list of scenario classes to play + seeds: A list of seeds to use for each scenario """ super().__init__('scenario_manager') self.scenario_types = scenario_types + self.seeds = seeds self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity') self.delete_client = self.create_client(DeleteEntity, '/delete_entity') + self.manipulator_client = self.create_client(ManipulatorMoveTo, '/manipulator_move_to') self.tf2_buffer = Buffer() self.tf2_listener = TransformListener(self.tf2_buffer, self) @@ -51,13 +56,17 @@ def __init__(self, scenario_types): self.agent_thread: Thread = None self.manipulator_ready = False self.scores = [] - + def _init_scenario(self): - self.scenario = self.scenario_types[self.current_scenario](self.spawn_client, self.delete_client, self) + self.scenario = self.scenario_types[self.current_scenario](self.spawn_client, self.delete_client, self.manipulator_client, self) self.manipulator_ready = False request = ManipulatorMoveTo.Request() request.target_pose.pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) request.target_pose.pose.position = Point(x=0.2, y=0.0, z=0.2) + if self.current_scenario < len(self.seeds): + random.seed(self.seeds[self.current_scenario]) + else: + random.seed(42) def callback(future: Future): self.manipulator_ready = True self.scenario.reset() @@ -66,9 +75,19 @@ def callback(future: Future): def _terminate_scenario(self): self.get_logger().info(f'Scenario terminated with score {self.scores[-1]}') self.scenario = None + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) if self.current_scenario == len(self.scenario_types) - 1: self.get_logger().info(f'All scenarios are completed, with scores: {self.scores}') - self.executor.shutdown() + request = ManipulatorMoveTo.Request() + request.target_pose.pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + request.target_pose.pose.position = Point(x=0.2, y=0.0, z=0.2) + def callback(future: Future): + self.manipulator_ready = True + self.executor.shutdown() + self.manipulator_client.call_async(request).add_done_callback(callback) + self.timer.cancel() + return self.current_scenario = (self.current_scenario + 1) % len(self.scenario_types) self.manipulator_ready = False @@ -80,7 +99,6 @@ def timer_callback(self): return progress, terminated = self.scenario.step() - self.get_logger().info(f'Task progress: {progress}') if terminated and not (self.agent_thread and self.agent_thread.is_alive()): self.scores.append(progress) self._terminate_scenario() @@ -94,8 +112,8 @@ class RaiBenchmarkManager(ScenarioManager): """ A class responsible for playing the scenarios and running the conversational agent for each scenario """ - def __init__(self, scenario_types): - super().__init__(scenario_types) + def __init__(self, scenario_types, seeds = []): + super().__init__(scenario_types, seeds) self.agent = None def _init_scenario(self): diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py index 9280962..55a15d6 100644 --- a/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py @@ -10,11 +10,11 @@ from rai_mani.scenarios.scenario_base import ScenarioBase class MoveToTheLeft(ScenarioBase): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): - super().__init__(spawn_client, delete_client, node) + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) def get_prompt(self): - return "Please move all the apples to the left side of the table, about -0.30 in Y direction." + return "There are 5 apples on the right half of the table. Their Y position is positive. Move each of them to the left half of the table, such that they dont collide with each other. The Y position of the apples should be negative after the task is completed. First grab one apple using the 'grab' tool, and then drop it in the appropriate position using the 'drop' tool. Repeat this procedure for each apple." def reset(self): super().reset() @@ -44,8 +44,8 @@ def step(self): return progress, progress >= 1.0 class MoveToTheLeftAuto(MoveToTheLeft): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): - super().__init__(spawn_client, delete_client, node) + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) self.manipulator_busy = False def reset(self): diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py index c886bab..ac89625 100644 --- a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py @@ -10,11 +10,11 @@ from rai_mani.scenarios.scenario_base import ScenarioBase class PlaceOnTop(ScenarioBase): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): self.top_object = None self.bot_object = None - super().__init__(spawn_client, delete_client, node) + super().__init__(spawn_client, delete_client, manipulator_client, node) def get_prompt(self): return "Place the yellow cube on top of the blue cube. Remember to increase the Z position of the 'drop' task by around 0.2 to avoid collision." @@ -23,21 +23,9 @@ def reset(self): super().reset() prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] - for i in range(5): - pose = PoseStamped() - pose.header.frame_id = 'world' - pose.pose.position = Point(x=0.4, y=float(i)/8 - 0.25, z=0.1) - pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - - pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) - - prefab = prefabs[i % len(prefabs)] - if prefab == 'yellow_cube': - self.top_object = f'{prefab}{i}' - elif prefab == 'blue_cube': - self.bot_object = f'{prefab}{i}' - - self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) + self.spawn_entities_in_random_positions(prefabs, prefabs) + self.bot_object = 'blue_cube' + self.top_object = 'yellow_cube' def calculate_progress(self): if self.top_object == None or self.bot_object == None: @@ -63,10 +51,10 @@ def step(self): return progress, progress >= 0.8 class PlaceOnTopAuto(PlaceOnTop): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): self.manipulator_busy = False - super().__init__(spawn_client, delete_client, node) + super().__init__(spawn_client, delete_client, manipulator_client, node) def reset(self): super().reset() diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py index b4d924a..5bcd42e 100644 --- a/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py @@ -10,12 +10,12 @@ from rai_mani.scenarios.scenario_base import ScenarioBase class ReplaceTypes(ScenarioBase): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): self.vegetable_poses = [] self.toy_poses = [] self.current_index = 0 - super().__init__(spawn_client, delete_client, node) + super().__init__(spawn_client, delete_client, manipulator_client, node) def get_prompt(self): return "Replace the objects in such a way that all the toys are in the places of vegetables and vice versa." @@ -27,21 +27,13 @@ def reset(self): self.toy_poses = [] self.current_index = 0 prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] - for i in range(4): - pose = PoseStamped() - pose.header.frame_id = 'world' - pose.pose.position = Point(x=0.4, y=float(i)/8 - 0.25, z=0.1) - pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - - pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) - - prefab = prefabs[i % len(prefabs)] - - self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) - if self._get_type(prefab) == 'vegetable': - self.vegetable_poses.append(pose.pose) - elif self._get_type(prefab) == 'toy': - self.toy_poses.append(pose.pose) + self.spawn_entities_in_random_positions(prefabs, prefabs) + for entity in prefabs: + if self._get_type(entity) == 'vegetable': + self.vegetable_poses.append(self.pose_transformed(self.get_entity_pose(entity))) + elif self._get_type(entity) == 'toy': + self.toy_poses.append(self.pose_transformed(self.get_entity_pose(entity))) + return def _get_type(self, object_name: str): if object_name.startswith('apple') or object_name.startswith('carrot'): @@ -84,10 +76,10 @@ def step(self): return progress, progress >= 0.8 class ReplaceTypesAuto(ReplaceTypes): - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): self.manipulator_busy = False - super().__init__(spawn_client, delete_client, node) + super().__init__(spawn_client, delete_client, manipulator_client, node) def reset(self): super().reset() diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py index 54662a5..d7f3775 100644 --- a/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py @@ -8,14 +8,16 @@ from rai_interfaces.srv import ManipulatorMoveTo +import random + class ScenarioBase: """ Base class for a scenario. A scenario is a task that the agent has to perform. """ - def __init__(self, spawn_client: Client, delete_client: Client, node: Node): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Node, node: Node): self.spawn_client = spawn_client self.delete_client = delete_client - self.manipulator_client = node.create_client(ManipulatorMoveTo, '/manipulator_move_to') + self.manipulator_client = manipulator_client self.node = node self.entities: dict[str, str] = {} @@ -56,6 +58,29 @@ def spawn_entity(self, prefab_name: str, name: str, pose: Pose) -> None: req.initial_pose = pose self.spawn_client.call_async(req).add_done_callback(lambda future: self.entity_spawned_callback(future, name)) + + def spawn_entities_in_random_positions(self, prefab_names: list[str], names: list[str]) -> None: + """ + Spawns entities randomly positione around the table. + + Args: + prefab_names: A list of prefab names to spawn. + names: A list of names for the entities, by which they can be later referenced. + """ + grid = [ + (x / 10.0 + 0.4, y / 10.0) for x in range(-1, 2) for y in range(-3, 4) + ] + + positions = random.sample(grid, k=len(prefab_names)) + for prefab_name, name, position in zip(prefab_names, names, positions): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position.x = position[0] + pose.pose.position.y = position[1] + pose.pose.position.z = 0.05 + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + self.spawn_entity(prefab_name, name, pose_transformed.pose) def delete_entity(self, name: str) -> None: req = DeleteEntity.Request() diff --git a/ros2_ws/src/rai_mani/rai_mani/showcase.py b/ros2_ws/src/rai_mani/rai_mani/showcase.py index 5ff09a4..7939975 100644 --- a/ros2_ws/src/rai_mani/rai_mani/showcase.py +++ b/ros2_ws/src/rai_mani/rai_mani/showcase.py @@ -11,7 +11,7 @@ def main(args=None): rclpy.init(args=args) - manager = ScenarioManager([ReplaceTypesAuto, MoveToTheLeftAuto, PlaceOnTopAuto]) + manager = ScenarioManager([PlaceOnTopAuto, MoveToTheLeftAuto, ReplaceTypesAuto], list(range(3))) executor = MultiThreadedExecutor(2) executor.add_node(manager) From 3ba1c4f64c7065548ce8ed5305b35cf8bd4ffa7c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Fri, 10 Jan 2025 16:33:01 +0100 Subject: [PATCH 8/8] Add a new scenario MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- ros2_ws/src/rai_mani/rai_mani/benchmark.py | 3 +- ros2_ws/src/rai_mani/rai_mani/manager.py | 2 + .../rai_mani/scenarios/longest_object.py | 105 ++++++++++++++++++ ros2_ws/src/rai_mani/rai_mani/showcase.py | 3 +- 4 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py diff --git a/ros2_ws/src/rai_mani/rai_mani/benchmark.py b/ros2_ws/src/rai_mani/rai_mani/benchmark.py index efb8bde..c4ff0d2 100644 --- a/ros2_ws/src/rai_mani/rai_mani/benchmark.py +++ b/ros2_ws/src/rai_mani/rai_mani/benchmark.py @@ -5,13 +5,14 @@ from rai_mani.scenarios.move_to_the_left import MoveToTheLeft from rai_mani.scenarios.place_on_top import PlaceOnTop from rai_mani.scenarios.replace_types import ReplaceTypes +from rai_mani.scenarios.longest_object import LongestObject from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) - manager = RaiBenchmarkManager([PlaceOnTop, MoveToTheLeft, ReplaceTypes], list(range(3))) + manager = RaiBenchmarkManager([PlaceOnTop, LongestObject, MoveToTheLeft, ReplaceTypes], list(range(4))) executor = MultiThreadedExecutor(2) executor.add_node(manager) diff --git a/ros2_ws/src/rai_mani/rai_mani/manager.py b/ros2_ws/src/rai_mani/rai_mani/manager.py index feb9064..febfe40 100644 --- a/ros2_ws/src/rai_mani/rai_mani/manager.py +++ b/ros2_ws/src/rai_mani/rai_mani/manager.py @@ -22,6 +22,7 @@ from rai_mani.scenarios.scenario_base import ScenarioBase import random +import time class ScenarioManager(Node): """ @@ -156,6 +157,7 @@ def _init_scenario(self): ) def run_agent(): + time.sleep(1) self.agent.invoke({"messages": [HumanMessage(content=self.scenario.get_prompt())]})["messages"][-1].pretty_print() self.agent_thread = Thread(target=run_agent) diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py new file mode 100644 index 0000000..8f477cf --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py @@ -0,0 +1,105 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class LongestObject(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def get_prompt(self): + return "Put the longest object from the table into the toy box." + + def reset(self): + super().reset() + + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + self.spawn_entities_in_random_positions(prefabs, prefabs) + + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=-0.5, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)).pose + self.spawn_entity('toy_box', 'toy_box', pose_transformed) + + def calculate_progress(self): + if len(self.entities) == 0: + return 0.0 + + carrot_position = self.pose_transformed(self.get_entity_pose('carrot')).position + toy_box_position = self.pose_transformed(self.get_entity_pose('toy_box')).position + + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2)**0.5 + + return max(0.0, 1.0 - 3.0 * distance(carrot_position, toy_box_position)) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.5 + +class LongestObjectAuto(LongestObject): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def place_on_top(self, bot_object: str, top_object: str): + pose = self.get_entity_pose(top_object) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose = self.get_entity_pose(bot_object) + pose.position.z += 0.2 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.place_on_top('toy_box', 'carrot') + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/showcase.py b/ros2_ws/src/rai_mani/rai_mani/showcase.py index 7939975..cafb265 100644 --- a/ros2_ws/src/rai_mani/rai_mani/showcase.py +++ b/ros2_ws/src/rai_mani/rai_mani/showcase.py @@ -5,13 +5,14 @@ from rai_mani.scenarios.move_to_the_left import MoveToTheLeftAuto from rai_mani.scenarios.place_on_top import PlaceOnTopAuto from rai_mani.scenarios.replace_types import ReplaceTypesAuto +from rai_mani.scenarios.longest_object import LongestObjectAuto from rclpy.executors import MultiThreadedExecutor def main(args=None): rclpy.init(args=args) - manager = ScenarioManager([PlaceOnTopAuto, MoveToTheLeftAuto, ReplaceTypesAuto], list(range(3))) + manager = ScenarioManager([PlaceOnTopAuto, LongestObjectAuto, MoveToTheLeftAuto, ReplaceTypesAuto], list(range(4))) executor = MultiThreadedExecutor(2) executor.add_node(manager)