diff --git a/examples/manipulation-demo-no-binary.launch.py b/examples/manipulation-demo-no-binary.launch.py new file mode 100755 index 000000000..494deea7b --- /dev/null +++ b/examples/manipulation-demo-no-binary.launch.py @@ -0,0 +1,59 @@ +# Copyright (C) 2025 Robotec.AI +# +# 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 launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +# TODO (mkotynia) think about separation of launches +def generate_launch_description(): + launch_moveit = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + "src/examples/rai-manipulation-demo/Project/Examples/panda_moveit_config_demo.launch.py", + ] + ) + ) + + launch_robotic_manipulation = Node( + package="robotic_manipulation", + executable="robotic_manipulation", + # name="robotic_manipulation_node", + output="screen", + parameters=[ + {"use_sim_time": True}, + ], + ) + + launch_openset = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + FindPackageShare("rai_bringup"), + "/launch/openset.launch.py", + ] + ), + ) + + return LaunchDescription( + [ + launch_openset, + launch_moveit, + launch_robotic_manipulation, + ] + ) diff --git a/poetry.lock b/poetry.lock index 93059b345..2d22b986d 100644 --- a/poetry.lock +++ b/poetry.lock @@ -5700,6 +5700,22 @@ torchaudio = "^2.3.1" type = "directory" url = "src/rai_asr" +[[package]] +name = "rai-sim" +version = "0.0.1" +description = "Package to run simulations" +optional = false +python-versions = "^3.10, <3.13" +files = [] +develop = true + +[package.dependencies] +PyYAML = "*" + +[package.source] +type = "directory" +url = "src/rai_sim" + [[package]] name = "rai-tts" version = "1.0.0" @@ -8302,4 +8318,4 @@ cffi = ["cffi (>=1.11)"] [metadata] lock-version = "2.0" python-versions = "^3.10, <3.13" -content-hash = "242e440c4ce4b31fa629d198a3d79b0854e84284d013d819a4b7a24e633a1706" +content-hash = "a53906ce2c798e5e0a02c7db25cf00cf36e021186a79429ba1bd8f0836b12db2" diff --git a/pyproject.toml b/pyproject.toml index b313dd299..863309947 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -17,6 +17,7 @@ python = "^3.10, <3.13" rai = {path = "src/rai_core", develop = true} rai_asr = {path = "src/rai_asr", develop = true} rai_tts = {path = "src/rai_tts", develop = true} +rai_sim = {path = "src/rai_sim", develop = true} langchain-core = "^0.3" langchain = "*" diff --git a/src/rai_core/rai/communication/ros2/connectors.py b/src/rai_core/rai/communication/ros2/connectors.py index 452dae22e..af7953d78 100644 --- a/src/rai_core/rai/communication/ros2/connectors.py +++ b/src/rai_core/rai/communication/ros2/connectors.py @@ -69,6 +69,7 @@ def __init__( self._topic_api = ROS2TopicAPI(self._node) self._service_api = ROS2ServiceAPI(self._node) self._actions_api = ROS2ActionAPI(self._node) + self._tf_buffer = Buffer(node=self._node) self._executor = MultiThreadedExecutor() self._executor.add_node(self._node) @@ -179,16 +180,15 @@ def get_transform( source_frame: str, timeout_sec: float = 5.0, ) -> TransformStamped: - tf_buffer = Buffer(node=self._node) - tf_listener = TransformListener(tf_buffer, self._node) + tf_listener = TransformListener(self._tf_buffer, self._node) transform_available = self.wait_for_transform( - tf_buffer, target_frame, source_frame, timeout_sec + self._tf_buffer, target_frame, source_frame, timeout_sec ) if not transform_available: raise LookupException( f"Could not find transform from {source_frame} to {target_frame} in {timeout_sec} seconds" ) - transform: TransformStamped = tf_buffer.lookup_transform( + transform: TransformStamped = self._tf_buffer.lookup_transform( target_frame, source_frame, rclpy.time.Time(), diff --git a/src/rai_sim/README.md b/src/rai_sim/README.md new file mode 100644 index 000000000..f6e82bcb6 --- /dev/null +++ b/src/rai_sim/README.md @@ -0,0 +1,17 @@ +## RAI Sim + +## Description + +The RAI Sim is a package providing interface to implement connection with a specific simulation. + +### Components + +- `SimulationConnector` - An interface for connecting with a specific simulation. It manages scene setup, spawning, despawning objects, getting current state of the scene. + +- `SimulationConfig` - base config class to specify the entities to be spawned. For each simulation connector there should be specified custom simulation config specifying additional parameters needed to run and connect with the simulation. + +- `SceneState` - stores the current info about spawned entities + +### Example implementation + +- `O3DExROS2Connector` - An implementation of SimulationConnector for working with simulation based on O3DE and ROS2. diff --git a/src/rai_sim/pyproject.toml b/src/rai_sim/pyproject.toml new file mode 100644 index 000000000..9042be051 --- /dev/null +++ b/src/rai_sim/pyproject.toml @@ -0,0 +1,23 @@ +[tool.poetry] +name = "rai-sim" +version = "0.0.1" +description = "Package to run simulations" +authors = ["Magdalena Kotynia ", "Kacper Dąbrowski ", "Jakub Matejczyk "] +readme = "README.md" +classifiers = [ + "Programming Language :: Python :: 3", + "Development Status :: 4 - Beta", + "License :: OSI Approved :: Apache Software License", +] +packages = [ + { include = "rai_sim", from = "." }, +] + +[tool.poetry.dependencies] +python = "^3.10, <3.13" +PyYAML = "*" + + +[build-system] +requires = ["poetry-core"] +build-backend = "poetry.core.masonry.api" diff --git a/src/rai_sim/rai_sim/__init__.py b/src/rai_sim/rai_sim/__init__.py new file mode 100644 index 000000000..f792966c5 --- /dev/null +++ b/src/rai_sim/rai_sim/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2025 Robotec.AI +# +# 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. + +"""RAI Simulations package.""" diff --git a/src/rai_sim/rai_sim/o3de/o3de_bridge.py b/src/rai_sim/rai_sim/o3de/o3de_bridge.py new file mode 100644 index 000000000..45e377d39 --- /dev/null +++ b/src/rai_sim/rai_sim/o3de/o3de_bridge.py @@ -0,0 +1,306 @@ +# Copyright (C) 2025 Robotec.AI +# +# 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. + +import logging +import shlex +import signal +import subprocess +import time +from pathlib import Path +from typing import Any, Dict, Optional + +import yaml +from geometry_msgs.msg import Point, Pose, Quaternion +from rai.communication.ros2.connectors import ROS2ARIConnector, ROS2ARIMessage +from tf2_geometry_msgs import do_transform_pose + +from rai_sim.simulation_bridge import ( + Entity, + PoseModel, + Rotation, + SceneState, + SimulationBridge, + SimulationConfig, + SpawnedEntity, + Translation, +) + + +class O3DExROS2SimulationConfig(SimulationConfig): + binary_path: Path + robotic_stack_command: str + + @classmethod + def load_config( + cls, base_config_path: Path, connector_config_path: Path + ) -> "O3DExROS2SimulationConfig": + base_config = SimulationConfig.load_base_config(base_config_path) + + with open(connector_config_path) as f: + connector_content: dict[str, Any] = yaml.safe_load(f) + return cls(**base_config.model_dump(), **connector_content) + + +class O3DExROS2Bridge(SimulationBridge[O3DExROS2SimulationConfig]): + def __init__( + self, connector: ROS2ARIConnector, logger: Optional[logging.Logger] = None + ): + super().__init__(logger=logger) + self.connector = connector + self.current_sim_process = None + self.current_robotic_stack_process = None + self.current_binary_path = None + + def shutdown(self): + self._shutdown_binary() + self._shutdown_robotic_stack() + + def _shutdown_binary(self): + if not self.current_sim_process: + return + self.current_sim_process.send_signal(signal.SIGINT) + self.current_sim_process.wait() + + if self.current_sim_process.poll() is None: + self.logger.error( + f"Parent process PID: {self.current_sim_process.pid} is still running." + ) + raise RuntimeError( + f"Failed to terminate main process PID {self.current_sim_process.pid}" + ) + + self.current_sim_process = None + + def _shutdown_robotic_stack(self): + if not self.current_robotic_stack_process: + return + + self.current_robotic_stack_process.send_signal(signal.SIGINT) + self.current_robotic_stack_process.wait() + + if self.current_robotic_stack_process.poll() is None: + self.logger.error( + f"Parent process PID: {self.current_robotic_stack_process.pid} is still running." + ) + raise RuntimeError( + f"Failed to terminate robotic stack process PID {self.current_robotic_stack_process.pid}" + ) + + def get_available_spawnable_names(self) -> list[str]: + msg = ROS2ARIMessage({}) + response = self._try_service_call( + msg, + target="get_available_spawnable_names", + msg_type="gazebo_msgs/srv/GetWorldProperties", + ) + # NOTE (mkotynia) There is a bug in the gazebo_msgs/srv/GetWorldProperties service - payload.success is not set to True even if the service call is successful. It was reported to Kacper Dąbrowski and he is going to fix it. + # PR fixing the bug: https://github.com/o3de/o3de-extras/pull/828 + # TODO (mkotynia) uncomment check if response.payload.success when the bug is fixed and remove workaround check if response.payload.model_names. + + # if response.payload.success: + # return response.payload.model_names + if response.payload.model_names: + return response.payload.model_names + else: + raise RuntimeError( + f"Failed to get available spawnable names. Response: {response.payload.status_message}" + ) + + def _spawn_entity(self, entity: Entity): + pose = do_transform_pose( + self.to_ros2_pose(entity.pose), + self.connector.get_transform("odom", "world"), + ) + + msg_content: Dict[str, Any] = { + "name": entity.prefab_name, + "xml": "", + "robot_namespace": entity.name, + "initial_pose": { + "position": { + "x": pose.position.x, # type: ignore + "y": pose.position.y, # type: ignore + "z": pose.position.z, # type: ignore + }, + "orientation": { + "x": pose.orientation.x, # type: ignore + "y": pose.orientation.y, # type: ignore + "z": pose.orientation.z, # type: ignore + "w": pose.orientation.w, # type: ignore + }, + }, + } + + msg = ROS2ARIMessage(payload=msg_content) + response = self._try_service_call( + msg, target="spawn_entity", msg_type="gazebo_msgs/srv/SpawnEntity" + ) + if response and response.payload.success: + self.spawned_entities.append( + SpawnedEntity(id=response.payload.status_message, **entity.model_dump()) + ) + else: + raise RuntimeError( + f"Failed to spawn entity {entity.name}. Response: {response.payload.status_message}" + ) + + def _despawn_entity(self, entity: SpawnedEntity): + msg_content = {"name": entity.id} + + msg = ROS2ARIMessage(payload=msg_content) + + response = self._try_service_call( + msg, target="delete_entity", msg_type="gazebo_msgs/srv/DeleteEntity" + ) + if response.payload.success: + self.spawned_entities.remove(entity) + else: + raise RuntimeError( + f"Failed to delete entity {entity.name}. Response: {response.payload.status_message}" + ) + + def get_object_pose(self, entity: SpawnedEntity) -> PoseModel: + object_frame = entity.name + "/" + ros2_pose = do_transform_pose( + Pose(), self.connector.get_transform(object_frame + "odom", object_frame) + ) + ros2_pose = do_transform_pose( + ros2_pose, self.connector.get_transform("world", "odom") + ) + return self.from_ros2_pose(ros2_pose) + + def get_scene_state(self) -> SceneState: + """ + Get the current scene state. + """ + if not self.current_sim_process: + raise RuntimeError("Simulation is not running.") + entities: list[SpawnedEntity] = [] + for entity in self.spawned_entities: + current_pose = self.get_object_pose(entity) + entities.append( + SpawnedEntity( + id=entity.id, + name=entity.name, + prefab_name=entity.prefab_name, + pose=current_pose, + ) + ) + return SceneState(entities=entities) + + def setup_scene(self, simulation_config: O3DExROS2SimulationConfig): + if self.current_binary_path != simulation_config.binary_path: + if self.current_sim_process: + self.shutdown() + self._launch_binary(simulation_config.binary_path) + self._launch_robotic_stack(simulation_config.robotic_stack_command) + self.current_binary_path = simulation_config.binary_path + + else: + while self.spawned_entities: + self._despawn_entity(self.spawned_entities[0]) + + for entity in simulation_config.entities: + self._spawn_entity(entity) + + def _launch_binary(self, binary_path: Path): + command = [binary_path.as_posix()] + self.logger.info(f"Running command: {command}") + self.current_sim_process = subprocess.Popen( + command, + ) + if not self._has_process_started(process=self.current_sim_process): + raise RuntimeError("Process did not start in time.") + + def _launch_robotic_stack(self, robotic_stack_command: str): + command = shlex.split(robotic_stack_command) + self.logger.info(f"Running command: {command}") + self.current_robotic_stack_process = subprocess.Popen( + command, + ) + if not self._has_process_started(self.current_robotic_stack_process): + raise RuntimeError("Process did not start in time.") + + def _has_process_started(self, process: subprocess.Popen[Any], timeout: int = 15): + start_time = time.time() + while time.time() - start_time < timeout: + if process.poll() is None: + self.logger.info(f"Process started with PID {process.pid}") + return True + time.sleep(1) + return False + + def _try_service_call( + self, msg: ROS2ARIMessage, target: str, msg_type: str, n_retries: int = 3 + ) -> ROS2ARIMessage: + if n_retries < 1: + raise ValueError("Number of retries must be at least 1") + for _ in range(n_retries): + try: + response = self.connector.service_call( + msg, target=target, msg_type=msg_type + ) + except Exception as e: + error_message = f"Error while calling service {target} with msg_type {msg_type}: {e}" + self.logger.error(error_message) + raise RuntimeError(error_message) + if response.payload.success: + return response + self.logger.warning( + f"Retrying {target}, response success: {response.payload.success}" + ) + return response # type: ignore + + # NOTE (mkotynia) probably to be refactored, other bridges may also want to use pose conversion to/from ROS2 format + def to_ros2_pose(self, pose: PoseModel) -> Pose: + """ + Converts pose in PoseModel format to pose in ROS2 Pose format. + """ + position = Point( + x=pose.translation.x, y=pose.translation.y, z=pose.translation.z + ) + + if pose.rotation is not None: + orientation = Quaternion( + x=pose.rotation.x, + y=pose.rotation.y, + z=pose.rotation.z, + w=pose.rotation.w, + ) + else: + orientation = Quaternion() + + ros2_pose = Pose(position=position, orientation=orientation) + + return ros2_pose + + def from_ros2_pose(self, pose: Pose) -> PoseModel: + """ + Converts ROS2 pose to PoseModel format + """ + + translation = Translation( + x=pose.position.x, # type: ignore + y=pose.position.y, # type: ignore + z=pose.position.z, # type: ignore + ) + + rotation = Rotation( + x=pose.orientation.x, # type: ignore + y=pose.orientation.y, # type: ignore + z=pose.orientation.z, # type: ignore + w=pose.orientation.w, # type: ignore + ) + + return PoseModel(translation=translation, rotation=rotation) diff --git a/src/rai_sim/rai_sim/simulation_bridge.py b/src/rai_sim/rai_sim/simulation_bridge.py new file mode 100644 index 000000000..93959382d --- /dev/null +++ b/src/rai_sim/rai_sim/simulation_bridge.py @@ -0,0 +1,119 @@ +# Copyright (C) 2025 Robotec.AI +# +# 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. + +import logging +from abc import ABC, abstractmethod +from pathlib import Path +from typing import Generic, List, Optional, TypeVar + +import yaml +from pydantic import BaseModel, field_validator + + +class Translation(BaseModel): + x: float + y: float + z: float + + +class Rotation(BaseModel): + x: float + y: float + z: float + w: float + + +class PoseModel(BaseModel): + translation: Translation + rotation: Optional[Rotation] + + +class Entity(BaseModel): + name: str + prefab_name: str + pose: PoseModel + + +class SpawnedEntity(Entity): + id: str + + +class SimulationConfig(BaseModel): + """ + Setup of simulation - arrangemenet of objects in the environment. + """ + + # NOTE (mkotynia) can be extended by other attributes + entities: List[Entity] + + @field_validator("entities") + @classmethod + def check_unique_names(cls, entities: List[Entity]) -> List[Entity]: + names = [entity.name for entity in entities] + if len(names) != len(set(names)): + raise ValueError("Each entity must have a unique name.") + return entities + + @classmethod + def load_base_config(cls, base_config_path: Path) -> "SimulationConfig": + with open(base_config_path) as f: + content = yaml.safe_load(f) + return cls(**content) + + +class SceneState(BaseModel): + """ + Info about current entities' state in the scene. + """ + + # NOTE (mkotynia) can be extended by other attributes + entities: List[SpawnedEntity] + + +SimulationConfigT = TypeVar("SimulationConfigT", bound=SimulationConfig) + + +class SimulationBridge(ABC, Generic[SimulationConfigT]): + """ + Responsible for communication with simulation. + """ + + def __init__(self, logger: Optional[logging.Logger] = None): + self.spawned_entities: List[ + SpawnedEntity + ] = [] # list of spawned entities with their initial poses + if logger is None: + self.logger = logging.getLogger(__name__) + else: + self.logger = logger + + @abstractmethod + def setup_scene(self, simulation_config: SimulationConfigT): + pass + + @abstractmethod + def _spawn_entity(self, entity: Entity): + pass + + @abstractmethod + def _despawn_entity(self, entity: SpawnedEntity): + pass + + @abstractmethod + def get_object_pose(self, entity: SpawnedEntity) -> PoseModel: + pass + + @abstractmethod + def get_scene_state(self) -> SceneState: + pass