From 31d2afe6e2d522b5f5e2558499bc7ef3761a3e14 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 30 Sep 2024 23:30:04 +0300 Subject: [PATCH] docs(env_manager): Update docstrings and refactor code - Added detailed docstrings to the env_manager package for improved documentation and clarity. - Refactored code in multiple files to enhance readability and maintainability. --- .../env_manager/models/configs/camera.py | 67 +++ .../env_manager/models/configs/light.py | 26 + .../env_manager/models/configs/objects.py | 118 ++++ .../env_manager/models/configs/robot.py | 44 ++ .../env_manager/models/configs/scene.py | 40 ++ .../env_manager/models/configs/terrain.py | 19 + .../env_manager/models/lights/random_sun.py | 128 +++- .../env_manager/models/lights/sun.py | 75 +++ .../env_manager/models/objects/model.py | 35 +- .../models/objects/primitives/box.py | 46 +- .../models/objects/primitives/cylinder.py | 49 +- .../models/objects/primitives/plane.py | 25 +- .../models/objects/primitives/sphere.py | 43 +- .../models/objects/random_object.py | 44 +- .../models/objects/random_primitive.py | 72 ++- .../env_manager/models/robots/__init__.py | 15 +- .../env_manager/models/robots/panda.py | 348 ----------- .../env_manager/models/robots/rbs_arm.py | 322 ++++------ .../env_manager/models/robots/rbs_arm_b.py | 106 ---- .../env_manager/models/robots/robot.py | 52 ++ .../env_manager/models/sensors/camera.py | 101 +++- .../env_manager/models/terrains/ground.py | 20 + .../utils/model_collection_randomizer.py | 559 +++++++++++++++++- .../env_manager/env_manager/scene/scene.py | 476 +++++++++++++-- .../env_manager/scene/scene_randomizer.py | 4 - 25 files changed, 2023 insertions(+), 811 deletions(-) delete mode 100644 env_manager/env_manager/env_manager/models/robots/panda.py delete mode 100644 env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py delete mode 100644 env_manager/env_manager/env_manager/scene/scene_randomizer.py diff --git a/env_manager/env_manager/env_manager/models/configs/camera.py b/env_manager/env_manager/env_manager/models/configs/camera.py index a6038eb..378937a 100644 --- a/env_manager/env_manager/env_manager/models/configs/camera.py +++ b/env_manager/env_manager/env_manager/models/configs/camera.py @@ -4,6 +4,73 @@ import numpy as np @dataclass class CameraData: + """ + CameraData stores the parameters and configuration settings for an RGB-D camera. + + Attributes: + ---------- + name : str + The name of the camera instance. Default is an empty string. + enable : bool + Boolean flag to enable or disable the camera. Default is True. + type : str + Type of the camera. Default is "rgbd_camera". + relative_to : str + The reference frame relative to which the camera is placed. Default is "base_link". + + width : int + The image width (in pixels) captured by the camera. Default is 128. + height : int + The image height (in pixels) captured by the camera. Default is 128. + image_format : str + The image format used (e.g., "R8G8B8"). Default is "R8G8B8". + update_rate : int + The rate (in Hz) at which the camera provides updates. Default is 10 Hz. + horizontal_fov : float + The horizontal field of view (in radians). Default is pi / 3.0. + vertical_fov : float + The vertical field of view (in radians). Default is pi / 3.0. + + clip_color : tuple[float, float] + The near and far clipping planes for the color camera. Default is (0.01, 1000.0). + clip_depth : tuple[float, float] + The near and far clipping planes for the depth camera. Default is (0.05, 10.0). + + noise_mean : float | None + The mean value of the Gaussian noise applied to the camera's data. Default is None (no noise). + noise_stddev : float | None + The standard deviation of the Gaussian noise applied to the camera's data. Default is None (no noise). + + publish_color : bool + Whether or not to publish color data from the camera. Default is False. + publish_depth : bool + Whether or not to publish depth data from the camera. Default is False. + publish_points : bool + Whether or not to publish point cloud data from the camera. Default is False. + + spawn_position : tuple[float, float, float] + The initial spawn position (x, y, z) of the camera relative to the reference frame. Default is (0, 0, 1). + spawn_quat_xyzw : tuple[float, float, float, float] + The initial spawn orientation of the camera in quaternion (x, y, z, w). Default is (0, 0.70710678118, 0, 0.70710678118). + + random_pose_rollouts_num : int + The number of random pose rollouts. Default is 1. + random_pose_mode : str + The mode for random pose generation (e.g., "orbit"). Default is "orbit". + random_pose_orbit_distance : float + The fixed distance from the object in "orbit" mode. Default is 1.0. + random_pose_orbit_height_range : tuple[float, float] + The range of vertical positions (z-axis) in "orbit" mode. Default is (0.1, 0.7). + random_pose_orbit_ignore_arc_behind_robot : float + The arc angle (in radians) behind the robot to ignore when generating random poses. Default is pi / 8. + random_pose_select_position_options : list[tuple[float, float, float]] + A list of preset position options for the camera in random pose mode. Default is an empty list. + random_pose_focal_point_z_offset : float + The offset in the z-direction of the focal point when generating random poses. Default is 0.0. + random_pose_rollout_counter : float + A counter tracking the number of rollouts completed for random poses. Default is 0.0. + """ + name: str = field(default_factory=str) enable: bool = field(default=True) type: str = field(default="rgbd_camera") diff --git a/env_manager/env_manager/env_manager/models/configs/light.py b/env_manager/env_manager/env_manager/models/configs/light.py index 3aac9bc..ea15f52 100644 --- a/env_manager/env_manager/env_manager/models/configs/light.py +++ b/env_manager/env_manager/env_manager/models/configs/light.py @@ -3,6 +3,32 @@ from dataclasses import dataclass, field @dataclass class LightData: + """ + LightData stores the configuration and settings for a light source in a simulation or rendering environment. + + Attributes: + ---------- + type : str + The type of the light source (e.g., "sun", "point", "spot"). Default is "sun". + direction : tuple[float, float, float] + The direction vector of the light source in (x, y, z). This is typically used for directional lights like the sun. + Default is (0.5, -0.25, -0.75). + random_minmax_elevation : tuple[float, float] + The minimum and maximum elevation angles (in radians) for randomizing the light's direction. + Default is (-0.15, -0.65). + color : tuple[float, float, float, float] + The RGBA color of the light source. Each value ranges from 0 to 1. Default is (1.0, 1.0, 1.0, 1.0), which represents white light. + distance : float + The effective distance of the light source. Default is 1000.0 units. + visual : bool + A flag indicating whether the light source is visually represented in the simulation (e.g., whether it casts visible rays). + Default is True. + radius : float + The radius of the light's influence (for point and spot lights). Default is 25.0 units. + model_rollouts_num : int + The number of rollouts for randomizing light configurations in different simulation runs. Default is 1. + """ + type: str = field(default="sun") direction: tuple[float, float, float] = field(default=(0.5, -0.25, -0.75)) random_minmax_elevation: tuple[float, float] = field(default=(-0.15, -0.65)) diff --git a/env_manager/env_manager/env_manager/models/configs/objects.py b/env_manager/env_manager/env_manager/models/configs/objects.py index e970d12..8b5df50 100644 --- a/env_manager/env_manager/env_manager/models/configs/objects.py +++ b/env_manager/env_manager/env_manager/models/configs/objects.py @@ -3,6 +3,31 @@ from dataclasses import dataclass, field @dataclass class ObjectRandomizerData: + """ + ObjectRandomizerData contains parameters for randomizing object properties during simulation. + + Attributes: + ---------- + count : int + The number of objects to randomize. Default is 0. + random_pose : bool + Flag indicating whether to randomize both the position and orientation of objects. Default is False. + random_position : bool + Flag indicating whether to randomize the position of objects. Default is False. + random_orientation : bool + Flag indicating whether to randomize the orientation of objects. Default is False. + random_model : bool + Flag indicating whether to randomize the model of the objects. Default is False. + random_spawn_position_segments : list + List of segments within which the object can be randomly spawned. Default is an empty list. + random_spawn_position_update_workspace_centre : bool + Flag indicating whether to update the workspace center during random spawning. Default is False. + random_spawn_volume : tuple[float, float, float] + The volume within which objects can be randomly spawned, defined by (x, y, z). Default is (0.5, 0.5, 0.5). + models_rollouts_num : int + The number of rollouts for randomizing models. Default is 0. + """ + count: int = field(default=0) random_pose: bool = field(default=False) random_position: bool = field(default=False) @@ -16,6 +41,27 @@ class ObjectRandomizerData: @dataclass class ObjectData: + """ + ObjectData stores the base properties for any object in the simulation environment. + + Attributes: + ---------- + name : str + The name of the object. Default is "object". + type : str + The type of the object (e.g., "sphere", "box"). Default is an empty string. + relative_to : str + The reference frame relative to which the object is positioned. Default is "world". + position : tuple[float, float, float] + The position of the object in (x, y, z) coordinates. Default is (0.0, 0.0, 0.0). + orientation : tuple[float, float, float, float] + The orientation of the object as a quaternion (x, y, z, w). Default is (1.0, 0.0, 0.0, 0.0). + static : bool + Flag indicating if the object is static in the simulation (immovable). Default is False. + randomize : ObjectRandomizerData + Object randomizer settings for generating randomized object properties. Default is an empty ObjectRandomizerData instance. + """ + name: str = field(default="object") type: str = field(default_factory=str) relative_to: str = field(default="world") @@ -27,6 +73,21 @@ class ObjectData: @dataclass class PrimitiveObjectData(ObjectData): + """ + PrimitiveObjectData defines the base properties for primitive objects (e.g., spheres, boxes) in the simulation. + + Attributes: + ---------- + collision : bool + Flag indicating whether the object participates in collision detection. Default is True. + visual : bool + Flag indicating whether the object has a visual representation in the simulation. Default is True. + color : tuple[float, float, float, float] + The color of the object, represented in RGBA format. Default is (0.8, 0.8, 0.8, 1.0). + mass : float + The mass of the object. Default is 0.1. + """ + collision: bool = field(default=True) visual: bool = field(default=True) color: tuple = field(default=(0.8, 0.8, 0.8, 1.0)) @@ -35,12 +96,36 @@ class PrimitiveObjectData(ObjectData): @dataclass class SphereObjectData(PrimitiveObjectData): + """ + SphereObjectData defines the specific properties for a spherical object in the simulation. + + Attributes: + ---------- + radius : float + The radius of the sphere. Default is 0.025. + friction : float + The friction coefficient of the sphere when in contact with surfaces. Default is 1.0. + """ + radius: float = field(default=0.025) friction: float = field(default=1.0) @dataclass class PlaneObjectData(PrimitiveObjectData): + """ + PlaneObjectData defines the specific properties for a planar object in the simulation. + + Attributes: + ---------- + size : tuple[float, float] + The size of the plane, defined by its width and length. Default is (1.0, 1.0). + direction : tuple[float, float, float] + The normal vector representing the direction the plane faces. Default is (0.0, 0.0, 1.0). + friction : float + The friction coefficient of the plane when in contact with other objects. Default is 1.0. + """ + size: tuple = field(default=(1.0, 1.0)) direction: tuple = field(default=(0.0, 0.0, 1.0)) friction: float = field(default=1.0) @@ -48,6 +133,19 @@ class PlaneObjectData(PrimitiveObjectData): @dataclass class CylinderObjectData(PrimitiveObjectData): + """ + CylinderObjectData defines the specific properties for a cylindrical object in the simulation. + + Attributes: + ---------- + radius : float + The radius of the cylinder. Default is 0.025. + length : float + The length of the cylinder. Default is 0.1. + friction : float + The friction coefficient of the cylinder when in contact with surfaces. Default is 1.0. + """ + radius: float = field(default=0.025) length: float = field(default=0.1) friction: float = field(default=1.0) @@ -55,10 +153,30 @@ class CylinderObjectData(PrimitiveObjectData): @dataclass class BoxObjectData(PrimitiveObjectData): + """ + BoxObjectData defines the specific properties for a box-shaped object in the simulation. + + Attributes: + ---------- + size : tuple[float, float, float] + The dimensions of the box in (width, height, depth). Default is (0.05, 0.05, 0.05). + friction : float + The friction coefficient of the box when in contact with surfaces. Default is 1.0. + """ + size: tuple = field(default=(0.05, 0.05, 0.05)) friction: float = field(default=1.0) @dataclass class MeshObjectData(ObjectData): + """ + MeshObjectData defines the specific properties for a mesh-based object in the simulation. + + Attributes: + ---------- + texture : list[float] + A list of texture coordinates or texture properties applied to the mesh. Default is an empty list. + """ + texture: list[float] = field(default_factory=list) diff --git a/env_manager/env_manager/env_manager/models/configs/robot.py b/env_manager/env_manager/env_manager/models/configs/robot.py index cdc8f03..4bdafd7 100644 --- a/env_manager/env_manager/env_manager/models/configs/robot.py +++ b/env_manager/env_manager/env_manager/models/configs/robot.py @@ -3,6 +3,27 @@ from dataclasses import dataclass, field @dataclass class RobotRandomizerData: + """ + RobotRandomizerData stores configuration parameters for randomizing robot properties during simulation. + + Attributes: + ---------- + pose : bool + Flag indicating whether to randomize the robot's pose (position and orientation). Default is False. + spawn_volume : tuple[float, float, float] + The volume within which the robot can be spawned, defined by (x, y, z) dimensions. Default is (1.0, 1.0, 0.0). + joint_positions : bool + Flag indicating whether to randomize the robot's joint positions. Default is False. + joint_positions_std : float + The standard deviation for randomizing the robot's joint positions. Default is 0.1. + joint_positions_above_object_spawn : bool + Flag indicating whether the robot's joint positions should be randomized to place the robot above the object's spawn position. Default is False. + joint_positions_above_object_spawn_elevation : float + The elevation above the object's spawn position when placing the robot's joints. Default is 0.2. + joint_positions_above_object_spawn_xy_randomness : float + The randomness in the x and y coordinates when placing the robot's joints above the object's spawn position. Default is 0.2. + """ + pose: bool = field(default=False) spawn_volume: tuple[float, float, float] = field(default=(1.0, 1.0, 0.0)) joint_positions: bool = field(default=False) @@ -14,6 +35,29 @@ class RobotRandomizerData: @dataclass class RobotData: + """ + RobotData stores the base properties and configurations for a robot in the simulation. + + Attributes: + ---------- + name : str + The name of the robot. Default is an empty string. + urdf_string : str + Optional parameter that can store a URDF. This parameter is overridden by the node parameters if set in the node configuration. + spawn_position : tuple[float, float, float] + The position where the robot will be spawned in (x, y, z) coordinates. Default is (0.0, 0.0, 0.0). + spawn_quat_xyzw : tuple[float, float, float, float] + The orientation of the robot in quaternion format (x, y, z, w) at spawn. Default is (0.0, 0.0, 0.0, 1.0). + joint_positions : list[float] + A list of the robot's joint positions. Default is an empty list. + with_gripper : bool + Flag indicating whether the robot is equipped with a gripper. Default is False. + gripper_joint_positions : list[float] | float + The joint positions for the gripper. Can be a list of floats or a single float. Default is an empty list. + randomizer : RobotRandomizerData + The randomization settings for the robot, allowing various properties to be randomized in simulation. Default is an instance of RobotRandomizerData. + """ + name: str = field(default_factory=str) urdf_string: str = field(default_factory=str) spawn_position: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0)) diff --git a/env_manager/env_manager/env_manager/models/configs/scene.py b/env_manager/env_manager/env_manager/models/configs/scene.py index d7ee46e..61ebef4 100644 --- a/env_manager/env_manager/env_manager/models/configs/scene.py +++ b/env_manager/env_manager/env_manager/models/configs/scene.py @@ -8,6 +8,21 @@ from .camera import CameraData @dataclass class PluginsData: + """ + PluginsData stores the configuration for various plugins used in the simulation environment. + + Attributes: + ---------- + scene_broadcaster : bool + Flag indicating whether the scene broadcaster plugin is enabled. Default is False. + user_commands : bool + Flag indicating whether the user commands plugin is enabled. Default is False. + fts_broadcaster : bool + Flag indicating whether the force torque sensor (FTS) broadcaster plugin is enabled. Default is False. + sensors_render_engine : str + The rendering engine used for sensors. Default is "ogre2". + """ + scene_broadcaster: bool = field(default_factory=bool) user_commands: bool = field(default_factory=bool) fts_broadcaster: bool = field(default_factory=bool) @@ -16,6 +31,31 @@ class PluginsData: @dataclass class SceneData: + """ + SceneData contains the configuration and settings for the simulation scene. + + Attributes: + ---------- + physics_rollouts_num : int + The number of physics rollouts to perform in the simulation. Default is 0. + gravity : tuple[float, float, float] + The gravitational acceleration vector applied in the scene, represented as (x, y, z). Default is (0.0, 0.0, -9.80665). + gravity_std : tuple[float, float, float] + The standard deviation for the gravitational acceleration, represented as (x, y, z). Default is (0.0, 0.0, 0.0232). + robot : RobotData + The configuration data for the robot present in the scene. Default is an instance of RobotData. + terrain : TerrainData + The configuration data for the terrain in the scene. Default is an instance of TerrainData. + light : LightData + The configuration data for the lighting in the scene. Default is an instance of LightData. + objects : list[ObjectData] + A list of objects present in the scene, represented by their ObjectData configurations. Default is an empty list. + camera : list[CameraData] + A list of cameras in the scene, represented by their CameraData configurations. Default is an empty list. + plugins : PluginsData + The configuration data for various plugins utilized in the simulation environment. Default is an instance of PluginsData. + """ + physics_rollouts_num: int = field(default=0) gravity: tuple[float, float, float] = field(default=(0.0, 0.0, -9.80665)) gravity_std: tuple[float, float, float] = field(default=(0.0, 0.0, 0.0232)) diff --git a/env_manager/env_manager/env_manager/models/configs/terrain.py b/env_manager/env_manager/env_manager/models/configs/terrain.py index 50255d5..ea1a89e 100644 --- a/env_manager/env_manager/env_manager/models/configs/terrain.py +++ b/env_manager/env_manager/env_manager/models/configs/terrain.py @@ -3,6 +3,25 @@ from dataclasses import dataclass, field @dataclass class TerrainData: + """ + TerrainData stores the configuration for the terrain in the simulation environment. + + Attributes: + ---------- + name : str + The name of the terrain. Default is "ground". + type : str + The type of terrain (e.g., "flat", "hilly", "uneven"). Default is "flat". + spawn_position : tuple[float, float, float] + The position where the terrain will be spawned in the simulation, represented as (x, y, z). Default is (0, 0, 0). + spawn_quat_xyzw : tuple[float, float, float, float] + The orientation of the terrain at spawn, represented as a quaternion (x, y, z, w). Default is (0, 0, 0, 1). + size : tuple[float, float] + The size of the terrain, represented as (width, length). Default is (1.5, 1.5). + model_rollouts_num : int + The number of rollouts for randomizing terrain models. Default is 1. + """ + name: str = field(default="ground") type: str = field(default="flat") spawn_position: tuple[float, float, float] = field(default=(0, 0, 0)) diff --git a/env_manager/env_manager/env_manager/models/lights/random_sun.py b/env_manager/env_manager/env_manager/models/lights/random_sun.py index 366815c..e91f56c 100644 --- a/env_manager/env_manager/env_manager/models/lights/random_sun.py +++ b/env_manager/env_manager/env_manager/models/lights/random_sun.py @@ -1,33 +1,79 @@ -from typing import Optional, Tuple - import numpy as np from gym_gz.scenario import model_wrapper from gym_gz.utils.scenario import get_unique_model_name from numpy.random import RandomState from scenario import core as scenario +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo class RandomSun(model_wrapper.ModelWrapper): + """ + RandomSun is a class that creates a randomly positioned directional light (like the Sun) in a simulation world. + + Attributes: + ---------- + world : scenario_gazebo.World + The simulation world where the model is inserted. + name : str + The name of the sun model. Default is "sun". + minmax_elevation : tuple[float, float] + Minimum and maximum values for the random elevation angle of the sun. Default is (-0.15, -0.65). + distance : float + Distance from the origin to the sun in the simulation. Default is 800.0. + visual : bool + Flag indicating whether the sun should have a visual sphere in the simulation. Default is True. + radius : float + Radius of the visual sphere representing the sun. Default is 20.0. + color_minmax_r : tuple[float, float] + Range for the red component of the sun's light color. Default is (1.0, 1.0). + color_minmax_g : tuple[float, float] + Range for the green component of the sun's light color. Default is (1.0, 1.0). + color_minmax_b : tuple[float, float] + Range for the blue component of the sun's light color. Default is (1.0, 1.0). + specular : float + The specular factor for the sun's light. Default is 1.0. + attenuation_minmax_range : tuple[float, float] + Range for the light attenuation (falloff) distance. Default is (750.0, 15000.0). + attenuation_minmax_constant : tuple[float, float] + Range for the constant component of the light attenuation. Default is (0.5, 1.0). + attenuation_minmax_linear : tuple[float, float] + Range for the linear component of the light attenuation. Default is (0.001, 0.1). + attenuation_minmax_quadratic : tuple[float, float] + Range for the quadratic component of the light attenuation. Default is (0.0001, 0.01). + np_random : RandomState | None + Random state for generating random values. Default is None, in which case a new random generator is created. + + Raises: + ------- + RuntimeError: + Raised if the sun model fails to be inserted into the world. + + Methods: + -------- + get_sdf(): + Generates the SDF string for the sun model. + """ + def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, name: str = "sun", - minmax_elevation: Tuple[float, float] = (-0.15, -0.65), + minmax_elevation: tuple[float, float] = (-0.15, -0.65), distance: float = 800.0, visual: bool = True, radius: float = 20.0, - color_minmax_r: Tuple[float, float] = (1.0, 1.0), - color_minmax_g: Tuple[float, float] = (1.0, 1.0), - color_minmax_b: Tuple[float, float] = (1.0, 1.0), + color_minmax_r: tuple[float, float] = (1.0, 1.0), + color_minmax_g: tuple[float, float] = (1.0, 1.0), + color_minmax_b: tuple[float, float] = (1.0, 1.0), specular: float = 1.0, - attenuation_minmax_range: Tuple[float, float] = (750.0, 15000.0), - attenuation_minmax_constant: Tuple[float, float] = (0.5, 1.0), - attenuation_minmax_linear: Tuple[float, float] = (0.001, 0.1), - attenuation_minmax_quadratic: Tuple[float, float] = (0.0001, 0.01), - np_random: Optional[RandomState] = None, + attenuation_minmax_range: tuple[float, float] = (750.0, 15000.0), + attenuation_minmax_constant: tuple[float, float] = (0.5, 1.0), + attenuation_minmax_linear: tuple[float, float] = (0.001, 0.1), + attenuation_minmax_quadratic: tuple[float, float] = (0.0001, 0.01), + np_random: RandomState | None = None, **kwargs, ): - if np_random is None: np_random = np.random.default_rng() @@ -48,7 +94,7 @@ class RandomSun(model_wrapper.ModelWrapper): direction = direction / np.linalg.norm(direction) # Initial pose - initial_pose = scenario.Pose( + initial_pose = scenario_core.Pose( ( -direction[0] * distance, -direction[1] * distance, @@ -60,7 +106,7 @@ class RandomSun(model_wrapper.ModelWrapper): # Create SDF string for the model sdf = self.get_sdf( model_name=model_name, - direction=direction, + direction=tuple(direction), visual=visual, radius=radius, color_minmax_r=color_minmax_r, @@ -91,20 +137,56 @@ class RandomSun(model_wrapper.ModelWrapper): def get_sdf( self, model_name: str, - direction: Tuple[float, float, float], + direction: tuple[float, float, float], visual: bool, radius: float, - color_minmax_r: Tuple[float, float], - color_minmax_g: Tuple[float, float], - color_minmax_b: Tuple[float, float], - attenuation_minmax_range: Tuple[float, float], - attenuation_minmax_constant: Tuple[float, float], - attenuation_minmax_linear: Tuple[float, float], - attenuation_minmax_quadratic: Tuple[float, float], + color_minmax_r: tuple[float, float], + color_minmax_g: tuple[float, float], + color_minmax_b: tuple[float, float], + attenuation_minmax_range: tuple[float, float], + attenuation_minmax_constant: tuple[float, float], + attenuation_minmax_linear: tuple[float, float], + attenuation_minmax_quadratic: tuple[float, float], specular: float, np_random: RandomState, ) -> str: + """ + Generates the SDF string for the sun model. + Args: + ----- + model_name : str + The name of the model. + direction : Tuple[float, float, float] + The direction of the sun's light. + visual : bool + If True, a visual representation of the sun will be created. + radius : float + The radius of the visual representation. + color_minmax_r : Tuple[float, float] + Range for the red component of the light color. + color_minmax_g : Tuple[float, float] + Range for the green component of the light color. + color_minmax_b : Tuple[float, float] + Range for the blue component of the light color. + attenuation_minmax_range : Tuple[float, float] + Range for light attenuation distance. + attenuation_minmax_constant : Tuple[float, float] + Range for the constant attenuation factor. + attenuation_minmax_linear : Tuple[float, float] + Range for the linear attenuation factor. + attenuation_minmax_quadratic : Tuple[float, float] + Range for the quadratic attenuation factor. + specular : float + The specular reflection factor for the light. + np_random : RandomState + The random number generator used to sample random values for the parameters. + + Returns: + -------- + str: + The SDF string for the sun model. + """ # Sample random values for parameters color_r = np_random.uniform(color_minmax_r[0], color_minmax_r[1]) color_g = np_random.uniform(color_minmax_g[0], color_minmax_g[1]) diff --git a/env_manager/env_manager/env_manager/models/lights/sun.py b/env_manager/env_manager/env_manager/models/lights/sun.py index ba6c374..a10c565 100644 --- a/env_manager/env_manager/env_manager/models/lights/sun.py +++ b/env_manager/env_manager/env_manager/models/lights/sun.py @@ -6,6 +6,49 @@ from scenario import gazebo as scenario_gazebo class Sun(model_wrapper.ModelWrapper): + """ + Sun is a class that represents a directional light source in the simulation, similar to the Sun. + + It can have both visual and light properties, with customizable parameters such as color, direction, and attenuation. + + Attributes: + ---------- + world : scenario_gazebo.World + The Gazebo world where the sun model will be inserted. + name : str + The name of the sun model. Default is "sun". + direction : tuple[float, float, float] + The direction of the sunlight, normalized. Default is (0.5, -0.25, -0.75). + color : tuple[float, float, float, float] + The RGBA color values for the light's diffuse color. Default is (1.0, 1.0, 1.0, 1.0). + distance : float + The distance from the origin to the sun. Default is 800.0. + visual : bool + If True, a visual representation of the sun will be added. Default is True. + radius : float + The radius of the visual representation of the sun. Default is 20.0. + specular : float + The intensity of the specular reflection. Default is 1.0. + attenuation_range : float + The maximum range for the light attenuation. Default is 10000.0. + attenuation_constant : float + The constant attenuation factor. Default is 0.9. + attenuation_linear : float + The linear attenuation factor. Default is 0.01. + attenuation_quadratic : float + The quadratic attenuation factor. Default is 0.001. + + Raises: + ------- + RuntimeError: + If the sun model fails to be inserted into the Gazebo world. + + Methods: + -------- + get_sdf() -> str: + Generates the SDF string used to describe the sun model in the Gazebo simulation. + """ + def __init__( self, world: scenario_gazebo.World, @@ -80,6 +123,38 @@ class Sun(model_wrapper.ModelWrapper): attenuation_linear: float, attenuation_quadratic: float, ) -> str: + """ + Generates the SDF string for the sun model. + + Args: + ----- + model_name : str + The name of the sun model. + direction : tuple[float, float, float] + The direction vector for the sunlight. + color : tuple[float, float, float, float] + The RGBA color values for the sunlight. + visual : bool + Whether to include a visual representation of the sun (a sphere). + radius : float + The radius of the visual sphere. + specular : float + The specular reflection intensity. + attenuation_range : float + The range of the light attenuation. + attenuation_constant : float + The constant factor for the light attenuation. + attenuation_linear : float + The linear factor for the light attenuation. + attenuation_quadratic : float + The quadratic factor for the light attenuation. + + Returns: + -------- + str: + The SDF string for the sun model. + """ + return f''' true diff --git a/env_manager/env_manager/env_manager/models/objects/model.py b/env_manager/env_manager/env_manager/models/objects/model.py index cb6cd9f..88c903b 100644 --- a/env_manager/env_manager/env_manager/models/objects/model.py +++ b/env_manager/env_manager/env_manager/models/objects/model.py @@ -9,6 +9,27 @@ from rbs_assets_library import get_model_file class Mesh(model_wrapper.ModelWrapper): + """ + Represents a 3D mesh model in the Gazebo simulation environment. + This class is responsible for loading and inserting a mesh model + into the Gazebo world with specified attributes. + + Args: + world (scenario_gazebo.World): The Gazebo world where the mesh model will be inserted. + name (str, optional): The name of the mesh model. Defaults to "object". + position (tuple[float, float, float], optional): The position of the mesh in the world. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float], optional): The orientation of the mesh in quaternion format. Defaults to (1, 0, 0, 0). + gui_only (bool, optional): If True, the visual representation of the mesh will only appear in the GUI. Defaults to False. + **kwargs: Additional keyword arguments. + + Raises: + RuntimeError: If the mesh model fails to be inserted into the Gazebo world. + + Methods: + get_sdf(model_name: str) -> str: + Generates the SDF string used to describe the mesh model in the Gazebo simulation. + """ + def __init__( self, world: scenario_gazebo.World, @@ -18,7 +39,6 @@ class Mesh(model_wrapper.ModelWrapper): gui_only: bool = False, **kwargs, ): - # Get a unique model name model_name = get_unique_model_name(world, name) @@ -31,9 +51,7 @@ class Mesh(model_wrapper.ModelWrapper): ) # Insert the model - ok_model = world.to_gazebo().insert_model( - sdf, initial_pose, model_name - ) + ok_model = world.to_gazebo().insert_model(sdf, initial_pose, model_name) if not ok_model: raise RuntimeError("Failed to insert " + model_name) @@ -48,4 +66,13 @@ class Mesh(model_wrapper.ModelWrapper): cls, model_name: str, ) -> str: + """ + Generates the SDF string for the specified mesh model. + + Args: + model_name (str): The name of the mesh model to generate the SDF for. + + Returns: + str: The SDF string that defines the mesh model in Gazebo. + """ return get_model_file(model_name) diff --git a/env_manager/env_manager/env_manager/models/objects/primitives/box.py b/env_manager/env_manager/env_manager/models/objects/primitives/box.py index 2bf6327..f0bd9c4 100644 --- a/env_manager/env_manager/env_manager/models/objects/primitives/box.py +++ b/env_manager/env_manager/env_manager/models/objects/primitives/box.py @@ -5,6 +5,34 @@ from scenario import gazebo as scenario_gazebo class Box(model_wrapper.ModelWrapper): + """ + The Box class represents a 3D box model in the Gazebo simulation environment. + It includes physical and visual properties such as size, mass, color, and collision properties. + + Attributes: + world (scenario_gazebo.World): The Gazebo world where the box model will be inserted. + name (str): The name of the box model. Default is "box". + position (tuple[float, float, float]): The position of the box in the world. Default is (0, 0, 0). + orientation (tuple[float, float, float, float]): The orientation of the box in quaternion format. Default is (1, 0, 0, 0). + size (tuple[float, float, float]): The size of the box (width, length, height). Default is (0.05, 0.05, 0.05). + mass (float): The mass of the box in kilograms. Default is 0.1. + static (bool): If True, the box will be immovable and static. Default is False. + collision (bool): If True, the box will have collision properties. Default is True. + friction (float): The friction coefficient for the box’s surface. Default is 1.0. + visual (bool): If True, the box will have a visual representation. Default is True. + gui_only (bool): If True, the visual representation of the box will only appear in the GUI, not in the simulation physics. Default is False. + color (tuple[float, float, float, float]): The RGBA color of the box. Default is (0.8, 0.8, 0.8, 1.0). + + Raises: + RuntimeError: + If the box model fails to be inserted into the Gazebo world. + + Methods: + -------- + get_sdf() -> str: + Generates the SDF string used to describe the box model in the Gazebo simulation. + """ + def __init__( self, world: scenario_gazebo.World, @@ -21,7 +49,6 @@ class Box(model_wrapper.ModelWrapper): color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), **kwargs, ): - # Get a unique model name model_name = get_unique_model_name(world, name) @@ -67,6 +94,23 @@ class Box(model_wrapper.ModelWrapper): gui_only: bool, color: tuple[float, float, float, float], ) -> str: + """ + Generates the SDF string for the box model. + + Args: + model_name (str): The name of the box model. + size (tuple[float, float, float]): The dimensions of the box (width, length, height). + mass (float): The mass of the box. + static (bool): If True, the box will be static and immovable. + collision (bool): If True, the box will have collision properties. + friction (float): The friction coefficient for the box. + visual (bool): If True, the box will have a visual representation. + gui_only (bool): If True, the box's visual representation will only appear in the GUI and will not affect physics. + color (tuple[float, float, float, float]): The RGBA color of the box. + + Returns: + The SDF string that defines the box model in Gazebo. + """ return f''' {"true" if static else "false"} diff --git a/env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py b/env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py index 34aa17b..86aa376 100644 --- a/env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py +++ b/env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py @@ -5,6 +5,29 @@ from scenario import gazebo as scenario_gazebo class Cylinder(model_wrapper.ModelWrapper): + """ + The Cylinder class represents a cylindrical model in the Gazebo simulation environment. + + Attributes: + world (scenario_gazebo.World): The Gazebo world where the cylinder model will be inserted. + name (str): The name of the cylinder model. Default is "cylinder". + position (tuple[float, float, float]): The position of the cylinder in the world. Default is (0, 0, 0). + orientation (tuple[float, float, float, float]): The orientation of the cylinder in quaternion format. Default is (1, 0, 0, 0). + radius (float): The radius of the cylinder. Default is 0.025. + length (float): The length/height of the cylinder. Default is 0.1. + mass (float): The mass of the cylinder in kilograms. Default is 0.1. + static (bool): If True, the cylinder will be immovable. Default is False. + collision (bool): If True, the cylinder will have collision properties. Default is True. + friction (float): The friction coefficient for the cylinder's surface. Default is 1.0. + visual (bool): If True, the cylinder will have a visual representation. Default is True. + gui_only (bool): If True, the visual representation of the cylinder will only appear in the GUI, not in the simulation physics. Default is False. + color (tuple[float, float, float, float]): The RGBA color of the cylinder. Default is (0.8, 0.8, 0.8, 1.0). + + Raises: + RuntimeError: If the cylinder model fails to be inserted into the Gazebo world. + + """ + def __init__( self, world: scenario_gazebo.World, @@ -22,14 +45,10 @@ class Cylinder(model_wrapper.ModelWrapper): color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), **kwargs, ): - - # Получаем уникальное имя модели model_name = get_unique_model_name(world, name) - # Инициализируем начальную позу initial_pose = scenario_core.Pose(position, orientation) - # Создаем строку SDF для модели sdf = self.get_sdf( model_name=model_name, radius=radius, @@ -43,17 +62,14 @@ class Cylinder(model_wrapper.ModelWrapper): color=color, ) - # Вставляем модель ok_model = world.to_gazebo().insert_model_from_string( sdf, initial_pose, model_name ) if not ok_model: raise RuntimeError(f"Failed to insert {model_name}") - # Получаем модель model = world.get_model(model_name) - # Вызов конструктора родительского класса super().__init__(model=model) @classmethod @@ -70,7 +86,24 @@ class Cylinder(model_wrapper.ModelWrapper): gui_only: bool, color: tuple[float, float, float, float], ) -> str: - # Инерция одинакова для xx и yy, вычисляем ее один раз + """ + Generates the SDF string for the cylinder model. + + Args: + model_name (str): The name of the model. + radius (float): The radius of the cylinder. + length (float): The length or height of the cylinder. + mass (float): The mass of the cylinder in kilograms. + static (bool): If True, the cylinder will remain immovable in the simulation. + collision (bool): If True, adds collision properties to the cylinder. + friction (float): The friction coefficient for the cylinder's surface. + visual (bool): If True, a visual representation of the cylinder will be created. + gui_only (bool): If True, the visual representation will only appear in the GUI, without impacting the simulation's physics. + color (tuple[float, float, float, float]): The RGBA color of the cylinder, where each value is between 0 and 1. + + Returns: + str: The SDF string representing the cylinder model + """ inertia_xx_yy = (3 * radius**2 + length**2) * mass / 12 return f''' diff --git a/env_manager/env_manager/env_manager/models/objects/primitives/plane.py b/env_manager/env_manager/env_manager/models/objects/primitives/plane.py index 8f6db76..142e320 100644 --- a/env_manager/env_manager/env_manager/models/objects/primitives/plane.py +++ b/env_manager/env_manager/env_manager/models/objects/primitives/plane.py @@ -5,6 +5,24 @@ from scenario import gazebo as scenario_gazebo class Plane(model_wrapper.ModelWrapper): + """ + The Plane class represents a plane model in the Gazebo simulation environment. + It allows for defining a flat surface with collision and visual properties, as well as its orientation and friction settings. + + Attributes: + world (scenario_gazebo.World): The Gazebo world where the plane model will be inserted. + name (str): The name of the plane model. Default is "plane". + position (tuple[float, float, float]): The position of the plane in the world. Default is (0, 0, 0). + orientation (tuple[float, float, float, float]): The orientation of the plane in quaternion format. Default is (1, 0, 0, 0). + size (tuple[float, float]): The size of the plane along the x and y axes. Default is (1.0, 1.0). + direction (tuple[float, float, float]): The normal vector representing the plane's direction. Default is (0.0, 0.0, 1.0), representing a horizontal plane. + collision (bool): If True, the plane will have collision properties. Default is True. + friction (float): The friction coefficient for the plane's surface. Default is 1.0. + visual (bool): If True, the plane will have a visual representation. Default is True. + + Raises: + RuntimeError: If the plane model fails to be inserted into the Gazebo world. + """ def __init__( self, world: scenario_gazebo.World, @@ -18,13 +36,11 @@ class Plane(model_wrapper.ModelWrapper): visual: bool = True, **kwargs, ): - # Получаем уникальное имя модели + model_name = get_unique_model_name(world, name) - # Инициализируем начальную позу initial_pose = scenario_core.Pose(position, orientation) - # Генерация строки SDF для модели sdf = f''' true @@ -73,15 +89,12 @@ class Plane(model_wrapper.ModelWrapper): ''' - # Вставка модели ok_model = world.to_gazebo().insert_model_from_string( sdf, initial_pose, model_name ) if not ok_model: raise RuntimeError(f"Failed to insert {model_name}") - # Получение модели model = world.get_model(model_name) - # Вызов конструктора родительского класса super().__init__(model=model) diff --git a/env_manager/env_manager/env_manager/models/objects/primitives/sphere.py b/env_manager/env_manager/env_manager/models/objects/primitives/sphere.py index 959ddd8..15c4ff8 100644 --- a/env_manager/env_manager/env_manager/models/objects/primitives/sphere.py +++ b/env_manager/env_manager/env_manager/models/objects/primitives/sphere.py @@ -5,6 +5,31 @@ from scenario import gazebo as scenario_gazebo class Sphere(model_wrapper.ModelWrapper): + """ + A Sphere model for Gazebo simulation. + + This class represents a spherical object that can be inserted into a Gazebo world. + The sphere can be customized by specifying its physical properties, such as radius, mass, + collision parameters, friction, and visual attributes. + + Attributes: + world (scenario_gazebo.World): The Gazebo world where the sphere model will be inserted. + name (str): The name of the sphere model. Defaults to "sphere". + position (tuple[float, float, float]): The position of the sphere in the world. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float]): The orientation of the sphere in quaternion format. Defaults to (1, 0, 0, 0). + radius (float): The radius of the sphere. Defaults to 0.025. + mass (float): The mass of the sphere. Defaults to 0.1. + static (bool): If True, the sphere will be static in the simulation. Defaults to False. + collision (bool): If True, the sphere will have collision properties. Defaults to True. + friction (float): The friction coefficient of the sphere's surface. Defaults to 1.0. + visual (bool): If True, the sphere will have a visual representation. Defaults to True. + gui_only (bool): If True, the visual will only appear in the GUI. Defaults to False. + color (tuple[float, float, float, float]): The RGBA color of the sphere. Defaults to (0.8, 0.8, 0.8, 1.0). + + Raises: + RuntimeError: If the sphere fails to be inserted into the Gazebo world. + """ + def __init__( self, world: scenario_gazebo.World, @@ -21,7 +46,6 @@ class Sphere(model_wrapper.ModelWrapper): color: tuple[float, float, float, float] = (0.8, 0.8, 0.8, 1.0), **kwargs, ): - # Get a unique model name model_name = get_unique_model_name(world, name) @@ -67,6 +91,23 @@ class Sphere(model_wrapper.ModelWrapper): gui_only: bool, color: tuple[float, float, float, float], ) -> str: + """ + Generates the SDF (Simulation Description Format) string for the sphere model. + + Args: + model_name (str): The name of the model. + radius (float): The radius of the sphere. + mass (float): The mass of the sphere. + static (bool): Whether the sphere is static in the simulation. + collision (bool): Whether the sphere should have collision properties. + friction (float): The friction coefficient for the sphere. + visual (bool): Whether the sphere should have a visual representation. + gui_only (bool): Whether the visual representation is only visible in the GUI. + color (tuple[float, float, float, float]): The RGBA color of the sphere. + + Returns: + str: The SDF string representing the sphere. + """ # Inertia is identical for all axes inertia_xx_yy_zz = (mass * radius**2) * 2 / 5 diff --git a/env_manager/env_manager/env_manager/models/objects/random_object.py b/env_manager/env_manager/env_manager/models/objects/random_object.py index 244b1eb..dd8c80c 100644 --- a/env_manager/env_manager/env_manager/models/objects/random_object.py +++ b/env_manager/env_manager/env_manager/models/objects/random_object.py @@ -1,36 +1,60 @@ -from typing import List, Optional - from gym_gz.scenario import model_wrapper from gym_gz.utils.scenario import get_unique_model_name from numpy.random import RandomState -from scenario import core as scenario +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo + from env_manager.models.utils import ModelCollectionRandomizer class RandomObject(model_wrapper.ModelWrapper): + """ + Represents a randomly selected 3D object model in the Gazebo simulation environment. + This class allows for the insertion of various models based on a collection of model paths, + utilizing a randomization strategy for the chosen model. + + Args: + world (scenario_gazebo.World): The Gazebo world where the random object model will be inserted. + name (str, optional): The name of the random object model. Defaults to "object". + position (tuple[float, float, float], optional): The position of the object in the world. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float], optional): The orientation of the object in quaternion format. Defaults to (1, 0, 0, 0). + model_paths (str | None, optional): Paths to the model files. Must be set; raises an error if None. + owner (str, optional): The owner of the model collection. Defaults to "GoogleResearch". + collection (str, optional): The collection of models to choose from. Defaults to "Google Scanned Objects". + server (str, optional): The server URL for the model collection. Defaults to "https://fuel.ignitionrobotics.org". + server_version (str, optional): The version of the server to use. Defaults to "1.0". + unique_cache (bool, optional): If True, enables caching of unique models. Defaults to False. + reset_collection (bool, optional): If True, resets the model collection for new selections. Defaults to False. + np_random (RandomState | None, optional): An instance of RandomState for random number generation. Defaults to None. + **kwargs: Additional keyword arguments. + + Raises: + RuntimeError: If the model path is not set or if the random object model fails to be inserted into the Gazebo world. + """ def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, name: str = "object", - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - model_paths: str = None, + position: tuple[float, float, float] = (0, 0, 0), + orientation: tuple[float, float, float, float] = (1, 0, 0, 0), + model_paths: str | None = None, owner: str = "GoogleResearch", collection: str = "Google Scanned Objects", server: str = "https://fuel.ignitionrobotics.org", server_version: str = "1.0", unique_cache: bool = False, reset_collection: bool = False, - np_random: Optional[RandomState] = None, + np_random: RandomState | None = None, **kwargs, ): - # Get a unique model name + if model_paths is None: + raise RuntimeError("Set model path for continue") model_name = get_unique_model_name(world, name) # Initial pose - initial_pose = scenario.Pose(position, orientation) + initial_pose = scenario_core.Pose(position, orientation) model_collection_randomizer = ModelCollectionRandomizer( model_paths=model_paths, diff --git a/env_manager/env_manager/env_manager/models/objects/random_primitive.py b/env_manager/env_manager/env_manager/models/objects/random_primitive.py index 1a199a2..58510b1 100644 --- a/env_manager/env_manager/env_manager/models/objects/random_primitive.py +++ b/env_manager/env_manager/env_manager/models/objects/random_primitive.py @@ -1,31 +1,50 @@ -from typing import List, Optional, Union - import numpy as np from gym_gz.scenario import model_wrapper -from gym_gz.utils import misc from gym_gz.utils.scenario import get_unique_model_name from numpy.random import RandomState -from scenario import core as scenario +from scenario import core as scenario_core +from scenario import gazebo as scenario_gazebo from . import Box, Cylinder, Sphere class RandomPrimitive(model_wrapper.ModelWrapper): + """ + Represents a randomly generated primitive shape (box, cylinder, or sphere) in the Gazebo simulation environment. + This class allows for the insertion of various primitive models based on the user's specifications or randomly chosen. + + Args: + world (scenario_gazebo.World): The Gazebo world where the primitive model will be inserted. + name (str, optional): The name of the primitive model. Defaults to "primitive". + use_specific_primitive ((str | None), optional): If specified, the exact type of primitive to create ('box', 'cylinder', or 'sphere'). Defaults to None, which will randomly select a primitive type. + position (tuple[float, float, float], optional): The position of the primitive in the world. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float], optional): The orientation of the primitive in quaternion format. Defaults to (1, 0, 0, 0). + static (bool, optional): If True, the primitive will be static and immovable. Defaults to False. + collision (bool, optional): If True, the primitive will have collision properties. Defaults to True. + visual (bool, optional): If True, the primitive will have a visual representation. Defaults to True. + gui_only (bool, optional): If True, the visual representation will only appear in the GUI and not in the simulation physics. Defaults to False. + np_random (RandomState | None, optional): An instance of RandomState for random number generation. If None, a default random generator will be used. Defaults to None. + **kwargs: Additional keyword arguments. + + Raises: + RuntimeError: If the primitive model fails to be inserted into the Gazebo world. + TypeError: If the specified primitive type is not supported. + """ + def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, name: str = "primitive", - use_specific_primitive: Union[str, None] = None, - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), + use_specific_primitive: (str | None) = None, + position: tuple[float, float, float] = (0, 0, 0), + orientation: tuple[float, float, float, float] = (1, 0, 0, 0), static: bool = False, collision: bool = True, visual: bool = True, gui_only: bool = False, - np_random: Optional[RandomState] = None, + np_random: RandomState | None = None, **kwargs, ): - if np_random is None: np_random = np.random.default_rng() @@ -33,7 +52,7 @@ class RandomPrimitive(model_wrapper.ModelWrapper): model_name = get_unique_model_name(world, name) # Initial pose - initial_pose = scenario.Pose(position, orientation) + initial_pose = scenario_core.Pose(position, orientation) # Create SDF string for the model sdf = self.get_sdf( @@ -61,16 +80,37 @@ class RandomPrimitive(model_wrapper.ModelWrapper): @classmethod def get_sdf( - self, + cls, model_name: str, - use_specific_primitive: Union[str, None], + use_specific_primitive: (str | None), static: bool, collision: bool, visual: bool, gui_only: bool, np_random: RandomState, ) -> str: + """ + Generates the SDF (Simulation Description Format) string for the specified primitive model. + This method can create the SDF representation for a box, cylinder, or sphere based on the provided parameters. + If a specific primitive type is not provided, one will be randomly selected. + + Args: + model_name (str): The name of the model being generated. + use_specific_primitive ((str | None)): The specific type of primitive to create ('box', 'cylinder', or 'sphere'). If None, a random primitive will be chosen. + static (bool): If True, the primitive will be static and immovable. + collision (bool): If True, the primitive will have collision properties. + visual (bool): If True, the primitive will have a visual representation. + gui_only (bool): If True, the visual representation will only appear in the GUI and will not affect physics. + np_random (RandomState): An instance of RandomState for random number generation. + + Returns: + str: The SDF string that defines the specified primitive model, including its physical and visual properties. + + Raises: + TypeError: If the specified primitive type is not supported. + + """ if use_specific_primitive is not None: primitive = use_specific_primitive else: @@ -78,13 +118,13 @@ class RandomPrimitive(model_wrapper.ModelWrapper): mass = np_random.uniform(0.05, 0.25) friction = np_random.uniform(0.75, 1.5) - color = list(np_random.uniform(0.0, 1.0, (3,))) - color.append(1.0) + color = tuple(np_random.uniform(0.0, 1.0, (3,))) + color: tuple[float, float, float, float] = (color[0], color[1], color[2], 1.0) if "box" == primitive: return Box.get_sdf( model_name=model_name, - size=list(np_random.uniform(0.04, 0.06, (3,))), + size=tuple(np_random.uniform(0.04, 0.06, (3,))), mass=mass, static=static, collision=collision, diff --git a/env_manager/env_manager/env_manager/models/robots/__init__.py b/env_manager/env_manager/env_manager/models/robots/__init__.py index 8518940..c690e8c 100644 --- a/env_manager/env_manager/env_manager/models/robots/__init__.py +++ b/env_manager/env_manager/env_manager/models/robots/__init__.py @@ -1,21 +1,16 @@ -from .robot import RobotWrapper -# from .rbs_arm import RbsArm -from .rbs_arm_b import RbsArm as RbsArmB - -# from .panda import Panda from enum import Enum +from .rbs_arm import RbsArm +from .robot import RobotWrapper + class RobotEnum(Enum): RBS_ARM = "rbs_arm" - # PANDA = "panda" # Uncomment when Panda is implemented def get_robot_model_class(robot_model: str) -> type[RobotWrapper]: - # Используем enum для проверки модели робота model_mapping = { - RobotEnum.RBS_ARM.value: RbsArmB, - # RobotModel.PANDA.value: Panda, # Uncomment when Panda is implemented + RobotEnum.RBS_ARM.value: RbsArm, } - return model_mapping.get(robot_model, RbsArmB) + return model_mapping.get(robot_model, RbsArm) diff --git a/env_manager/env_manager/env_manager/models/robots/panda.py b/env_manager/env_manager/env_manager/models/robots/panda.py deleted file mode 100644 index 0a36b72..0000000 --- a/env_manager/env_manager/env_manager/models/robots/panda.py +++ /dev/null @@ -1,348 +0,0 @@ -from os import path -from typing import Dict, List, Optional, Tuple - -from ament_index_python.packages import get_package_share_directory -from gym_gz.scenario import model_with_file, model_wrapper -from gym_gz.utils.scenario import get_unique_model_name -from scenario import core as scenario -from scenario import gazebo as scenario_gazebo - -from env_manager.models.utils import xacro2sdf - - -class Panda(model_wrapper.ModelWrapper, model_with_file.ModelWithFile): - - ROBOT_MODEL_NAME: str = "panda" - DEFAULT_PREFIX: str = "panda_" - - __DESCRIPTION_PACKAGE = ROBOT_MODEL_NAME + "_description" - __DEFAULT_XACRO_FILE = path.join( - get_package_share_directory(__DESCRIPTION_PACKAGE), - "urdf", - ROBOT_MODEL_NAME + ".urdf.xacro", - ) - __DEFAULT_XACRO_MAPPINGS: Dict[str, any] = { - "name": ROBOT_MODEL_NAME, - "gripper": True, - "collision_arm": False, - "collision_gripper": True, - "ros2_control": True, - "ros2_control_plugin": "ign", - "ros2_control_command_interface": "effort", - "gazebo_preserve_fixed_joint": True, - } - __XACRO_MODEL_PATH_REMAP: Tuple[str, str] = ( - __DESCRIPTION_PACKAGE, - ROBOT_MODEL_NAME, - ) - - DEFAULT_ARM_JOINT_POSITIONS: List[float] = ( - 0.0, - -0.7853981633974483, - 0.0, - -2.356194490192345, - 0.0, - 1.5707963267948966, - 0.7853981633974483, - ) - OPEN_GRIPPER_JOINT_POSITIONS: List[float] = ( - 0.04, - 0.04, - ) - CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = ( - 0.0, - 0.0, - ) - DEFAULT_GRIPPER_JOINT_POSITIONS: List[float] = OPEN_GRIPPER_JOINT_POSITIONS - - BASE_LINK_Z_OFFSET: float = 0.0 - - def __init__( - self, - world: scenario.World, - name: str = ROBOT_MODEL_NAME, - position: List[float] = (0, 0, 0), - orientation: List[float] = (1, 0, 0, 0), - model_file: str = None, - use_fuel: bool = False, - use_xacro: bool = True, - xacro_file: str = __DEFAULT_XACRO_FILE, - xacro_mappings: Dict[str, any] = __DEFAULT_XACRO_MAPPINGS, - initial_arm_joint_positions: List[float] = DEFAULT_ARM_JOINT_POSITIONS, - initial_gripper_joint_positions: List[float] = OPEN_GRIPPER_JOINT_POSITIONS, - **kwargs, - ): - - # Store params that are needed internally - self.__prefix = f"{name}_" - self.__initial_arm_joint_positions = initial_arm_joint_positions - self.__initial_gripper_joint_positions = initial_gripper_joint_positions - - # Allow passing of custom model file as an argument - if model_file is None: - if use_xacro: - # Generate SDF from xacro - mappings = self.__DEFAULT_XACRO_MAPPINGS - mappings.update(kwargs) - mappings.update(xacro_mappings) - model_file = xacro2sdf( - input_file_path=xacro_file, - mappings=mappings, - model_path_remap=self.__XACRO_MODEL_PATH_REMAP, - ) - else: - # Otherwise, use the default SDF file (local or fuel) - model_file = self.get_model_file(fuel=use_fuel) - - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Setup initial pose - initial_pose = scenario.Pose(position, orientation) - - # Determine whether to insert from string or file - if use_xacro: - insert_fn = scenario_gazebo.World.insert_model_from_string - else: - insert_fn = scenario_gazebo.World.insert_model_from_file - - # Insert the model - ok_model = insert_fn(world.to_gazebo(), model_file, initial_pose, model_name) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Set initial joint configuration - self.set_initial_joint_positions(model) - - # Initialize base class - super().__init__(model=model) - - def set_initial_joint_positions(self, model): - - model = model.to_gazebo() - if not model.reset_joint_positions( - self.initial_arm_joint_positions, self.arm_joint_names - ): - raise RuntimeError("Failed to set initial positions of arm's joints") - if not model.reset_joint_positions( - self.initial_gripper_joint_positions, self.gripper_joint_names - ): - raise RuntimeError("Failed to set initial positions of gripper's joints") - - @classmethod - def get_model_file(cls, fuel: bool = False) -> str: - - if fuel: - raise NotImplementedError - return scenario_gazebo.get_model_file_from_fuel( - "https://fuel.ignitionrobotics.org/1.0/AndrejOrsula/models/" - + cls.ROBOT_MODEL_NAME - ) - else: - return cls.ROBOT_MODEL_NAME - - # Meta information # - @property - def is_mobile(self) -> bool: - - return False - - # Prefix # - @property - def prefix(self) -> str: - - return self.__prefix - - # Joints # - @property - def joint_names(self) -> List[str]: - - return self.move_base_joint_names + self.manipulator_joint_names - - @property - def move_base_joint_names(self) -> List[str]: - - return [] - - @property - def manipulator_joint_names(self) -> List[str]: - - return self.arm_joint_names + self.gripper_joint_names - - @classmethod - def get_arm_joint_names(cls, prefix: str = "") -> List[str]: - - return [ - prefix + "joint1", - prefix + "joint2", - prefix + "joint3", - prefix + "joint4", - prefix + "joint5", - prefix + "joint6", - prefix + "joint7", - ] - - @property - def arm_joint_names(self) -> List[str]: - - return self.get_arm_joint_names(self.prefix) - - @classmethod - def get_gripper_joint_names(cls, prefix: str = "") -> List[str]: - - return [ - prefix + "finger_joint1", - prefix + "finger_joint2", - ] - - @property - def gripper_joint_names(self) -> List[str]: - - return self.get_gripper_joint_names(self.prefix) - - @property - def move_base_joint_limits(self) -> Optional[List[Tuple[float, float]]]: - - return None - - @property - def arm_joint_limits(self) -> Optional[List[Tuple[float, float]]]: - - return [ - (-2.897246558310587, 2.897246558310587), - (-1.762782544514273, 1.762782544514273), - (-2.897246558310587, 2.897246558310587), - (-3.07177948351002, -0.06981317007977318), - (-2.897246558310587, 2.897246558310587), - (-0.0174532925199433, 3.752457891787809), - (-2.897246558310587, 2.897246558310587), - ] - - @property - def gripper_joint_limits(self) -> Optional[List[Tuple[float, float]]]: - - return [ - (0.0, 0.04), - (0.0, 0.04), - ] - - @property - def gripper_joints_close_towards_positive(self) -> bool: - - return ( - self.OPEN_GRIPPER_JOINT_POSITIONS[0] - < self.CLOSED_GRIPPER_JOINT_POSITIONS[0] - ) - - @property - def initial_arm_joint_positions(self) -> List[float]: - - return self.__initial_arm_joint_positions - - @property - def initial_gripper_joint_positions(self) -> List[float]: - - return self.__initial_gripper_joint_positions - - # Passive joints # - @property - def passive_joint_names(self) -> List[str]: - - return self.manipulator_passive_joint_names + self.move_base_passive_joint_names - - @property - def move_base_passive_joint_names(self) -> List[str]: - - return [] - - @property - def manipulator_passive_joint_names(self) -> List[str]: - - return self.arm_passive_joint_names + self.gripper_passive_joint_names - - @property - def arm_passive_joint_names(self) -> List[str]: - - return [] - - @property - def gripper_passive_joint_names(self) -> List[str]: - - return [] - - # Links # - @classmethod - def get_robot_base_link_name(cls, prefix: str = "") -> str: - - return cls.get_arm_base_link_name(prefix) - - @property - def robot_base_link_name(self) -> str: - - return self.get_robot_base_link_name(self.prefix) - - @classmethod - def get_arm_base_link_name(cls, prefix: str = "") -> str: - - # Same as `self.arm_link_names[0]`` - return prefix + "link0" - - @property - def arm_base_link_name(self) -> str: - - return self.get_arm_base_link_name(self.prefix) - - @classmethod - def get_ee_link_name(cls, prefix: str = "") -> str: - - return prefix + "hand_tcp" - - @property - def ee_link_name(self) -> str: - - return self.get_ee_link_name(self.prefix) - - @classmethod - def get_wheel_link_names(cls, prefix: str = "") -> List[str]: - - return [] - - @property - def wheel_link_names(self) -> List[str]: - - return self.get_wheel_link_names(self.prefix) - - @classmethod - def get_arm_link_names(cls, prefix: str = "") -> List[str]: - - return [ - prefix + "link0", - prefix + "link1", - prefix + "link2", - prefix + "link3", - prefix + "link4", - prefix + "link5", - prefix + "link6", - prefix + "link7", - ] - - @property - def arm_link_names(self) -> List[str]: - - return self.get_arm_link_names(self.prefix) - - @classmethod - def get_gripper_link_names(cls, prefix: str = "") -> List[str]: - - return [ - prefix + "leftfinger", - prefix + "rightfinger", - ] - - @property - def gripper_link_names(self) -> List[str]: - - return self.get_gripper_link_names(self.prefix) diff --git a/env_manager/env_manager/env_manager/models/robots/rbs_arm.py b/env_manager/env_manager/env_manager/models/robots/rbs_arm.py index 45bf565..75556fd 100644 --- a/env_manager/env_manager/env_manager/models/robots/rbs_arm.py +++ b/env_manager/env_manager/env_manager/models/robots/rbs_arm.py @@ -1,267 +1,153 @@ -from typing import Dict, List, Optional, Tuple, Any - -from gym_gz.scenario import model_with_file, model_wrapper from gym_gz.utils.scenario import get_unique_model_name -from scenario import gazebo as sgaz -import numpy as np +from rclpy.node import Node +from robot_builder.parser.urdf import URDF_parser +from robot_builder.elements.robot import Robot -from scenario import core as scenario +from scenario import core as scenario_core from scenario import gazebo as scenario_gazebo from .robot import RobotWrapper -# from robot_builder.elements.robot import Robot -# from robot_builder.writer.urdf import URDF_writer -# from robot_builder.parser.urdf import URDF_parser class RbsArm(RobotWrapper): + """ + A class representing a robotic arm built using the `robot_builder` library. - DEFAULT_ARM_JOINT_POSITIONS: List[float] = ( - 0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0, - ) + This class is responsible for creating a robotic arm model within a Gazebo simulation environment. + It allows for the manipulation of joint positions for both the arm and the gripper. - OPEN_GRIPPER_JOINT_POSITIONS: List[float] = ( - 0.064, - 0.064, - ) + Attributes: + DEFAULT_ARM_JOINT_POSITIONS (list[float]): The default joint positions for the arm. + OPEN_GRIPPER_JOINT_POSITIONS (list[float]): The joint positions for the gripper in the open state. + CLOSED_GRIPPER_JOINT_POSITIONS (list[float]): The joint positions for the gripper in the closed state. + DEFAULT_GRIPPER_JOINT_POSITIONS (list[float]): The default joint positions for the gripper. + + Args: + world (scenario_gazebo.World): The Gazebo world where the robot model will be inserted. + node (Node): The ROS2 node associated with the robotic arm. + urdf_string (str): The URDF string defining the robot. + name (str, optional): The name of the robotic arm. Defaults to "rbs_arm". + position (tuple[float, float, float], optional): The initial position of the robot in the world. Defaults to (0.0, 0.0, 0.0). + orientation (tuple[float, float, float, float], optional): The initial orientation of the robot in quaternion format. Defaults to (1.0, 0, 0, 0). + initial_arm_joint_positions (list[float] | float, optional): The initial joint positions for the arm. Defaults to `DEFAULT_ARM_JOINT_POSITIONS`. + initial_gripper_joint_positions (list[float] | float, optional): The initial joint positions for the gripper. Defaults to `DEFAULT_GRIPPER_JOINT_POSITIONS`. - CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = ( + Raises: + RuntimeError: If the robot model fails to be inserted into the Gazebo world. + """ + + DEFAULT_ARM_JOINT_POSITIONS: list[float] = [ 0.0, + 0.5, + 3.14159, + 1.5, 0.0, - ) + 1.4, + 0.0, + ] - DEFAULT_GRIPPER_JOINT_POSITIONS: List[float] = OPEN_GRIPPER_JOINT_POSITIONS + OPEN_GRIPPER_JOINT_POSITIONS: list[float] = [ + 0.064, + ] - BASE_LINK_Z_OFFSET: float = 0.0 - DEFAULT_PREFIX: str = "" - ROBOT_MODEL_NAME: str = "rbs_arm" + CLOSED_GRIPPER_JOINT_POSITIONS: list[float] = [ + 0.0, + ] + + DEFAULT_GRIPPER_JOINT_POSITIONS: list[float] = CLOSED_GRIPPER_JOINT_POSITIONS def __init__( self, - world: scenario.World, + world: scenario_gazebo.World, + node: Node, + urdf_string: str, name: str = "rbs_arm", position: tuple[float, float, float] = (0.0, 0.0, 0.0), orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0), - model_file: str | None = None, - use_fuel: bool = False, - use_xacro: bool = False, - xacro_file: str = "", - xacro_mappings: Dict[str, Any] = {}, - initial_arm_joint_positions: List[float] = DEFAULT_ARM_JOINT_POSITIONS, - initial_gripper_joint_positions: List[float] = DEFAULT_GRIPPER_JOINT_POSITIONS, - **kwargs + initial_arm_joint_positions: list[float] | float = DEFAULT_ARM_JOINT_POSITIONS, + initial_gripper_joint_positions: list[float] + | float = DEFAULT_GRIPPER_JOINT_POSITIONS, ): - - self.__prefix = f"{name}_" - self.__initial_arm_joint_positions = initial_arm_joint_positions - self.__initial_gripper_joint_positions = initial_gripper_joint_positions - - model_file = self.get_model_file() - # Get a unique model name model_name = get_unique_model_name(world, name) # Setup initial pose - initial_pose = scenario.Pose(position, orientation) - - # Determine whether to insert from string or file - if use_xacro: - insert_fn = scenario_gazebo.World.insert_model_from_string - else: - insert_fn = scenario_gazebo.World.insert_model_from_file + initial_pose = scenario_core.Pose(position, orientation) # Insert the model - ok_model = insert_fn(world.to_gazebo(), model_file, initial_pose, model_name) + ok_model = world.insert_model_from_string(urdf_string, initial_pose, model_name) if not ok_model: raise RuntimeError("Failed to insert " + model_name) # Get the model model = world.get_model(model_name) - # Set initial joint configuration - self.set_initial_joint_positions(model) + # Parse robot to get metadata + self._robot: Robot = URDF_parser.load_string(urdf_string) - # Initialize base class - super().__init__(model=model) - - def set_initial_joint_positions(self, model): - - model = model.to_gazebo() - if not model.reset_joint_positions( - self.initial_arm_joint_positions, self.arm_joint_names - ): - raise RuntimeError("Failed to set initial positions of arm's joints") - if not model.reset_joint_positions( - self.initial_gripper_joint_positions, self.gripper_joint_names - ): - raise RuntimeError("Failed to set initial positions of gripper's joints") - - # Meta information # - @property - def is_mobile(self) -> bool: - return False - - # Prefix # - @property - def prefix(self) -> str: - return self.__prefix - - # Joints # - @property - def joint_names(self) -> List[str]: - return self.move_base_joint_names + self.manipulator_joint_names - - @property - def move_base_joint_names(self) -> List[str]: - return [] - - @property - def manipulator_joint_names(self) -> List[str]: - return self.arm_joint_names + self.gripper_joint_names - - @classmethod - def get_arm_joint_names(cls, prefix: str) -> List[str]: - return [ - "fork0_link_joint", - "main0_link_joint", - "fork1_link_joint", - "main1_link_joint", - "fork2_link_joint", - "ee_link_joint", - "rbs_gripper_rot_base_joint", - ] - - @property - def arm_joint_names(self) -> List[str]: - return self.get_arm_joint_names(self.prefix) - - @classmethod - def get_gripper_joint_names(cls, prefix: str) -> List[str]: - return [ - "rbs_gripper_r_finger_joint", - "rbs_gripper_l_finger_joint" - ] - - @property - def gripper_joint_names(self) -> List[str]: - return self.get_gripper_joint_names(self.prefix) - - @property - def arm_joint_limits(self) -> Optional[List[Tuple[float, float]]]: - return [ - (-3.14159, 3.14159), - (-1.5708, 3.14159), - (-3.14159, 3.14159), - (-1.5708, 3.14159), - (-3.14159, 3.14159), - (-1.5708, 3.14159), - (-3.14159, 3.14159), - ] - - @property - def gripper_joint_limits(self) -> Optional[List[Tuple[float, float]]]: - return [ - (0.0, 0.064), - (0.0, 0.064), - ] - - @property - def gripper_joints_close_towards_positive(self) -> bool: - return ( - self.OPEN_GRIPPER_JOINT_POSITIONS[0] - < self.CLOSED_GRIPPER_JOINT_POSITIONS[0] + self.__initial_gripper_joint_positions = ( + [float(initial_gripper_joint_positions)] + * len(self._robot.gripper_actuated_joint_names) + if isinstance(initial_gripper_joint_positions, (int, float)) + else initial_gripper_joint_positions ) + self.__initial_arm_joint_positions = ( + [float(initial_arm_joint_positions)] * len(self._robot.actuated_joint_names) + if isinstance(initial_arm_joint_positions, (int, float)) + else initial_arm_joint_positions + ) + + # Set initial joint configuration + self.set_initial_joint_positions(model) + super().__init__(model=model) + @property - def initial_arm_joint_positions(self) -> List[float]: + def robot(self) -> Robot: + """Returns the robot metadata parsed from the URDF string. + + Returns: + Robot: The robot instance containing metadata. + """ + return self._robot + + @property + def initial_arm_joint_positions(self) -> list[float]: + """Returns the initial joint positions for the arm. + + Returns: + list[float]: The initial joint positions for the arm. + """ return self.__initial_arm_joint_positions @property - def initial_gripper_joint_positions(self) -> List[float]: + def initial_gripper_joint_positions(self) -> list[float]: + """Returns the initial joint positions for the gripper. + + Returns: + list[float]: The initial joint positions for the gripper. + """ return self.__initial_gripper_joint_positions - # Passive joints # - @property - def passive_joint_names(self) -> List[str]: - return self.manipulator_passive_joint_names + self.move_base_passive_joint_names + def set_initial_joint_positions(self, model): + """Sets the initial positions for the robot's joints. - @property - def move_base_passive_joint_names(self) -> List[str]: - return [] + This method resets the joint positions of both the arm and gripper to their specified initial values. - @property - def manipulator_passive_joint_names(self) -> List[str]: - return self.arm_passive_joint_names + self.gripper_passive_joint_names + Args: + model: The model representation of the robot within the Gazebo environment. - @property - def arm_passive_joint_names(self) -> List[str]: - return [] + Raises: + RuntimeError: If resetting the joint positions fails for any of the joints. + """ + model = model.to_gazebo() - @property - def gripper_passive_joint_names(self) -> List[str]: - return [] - - # Links # - @classmethod - def get_robot_base_link_name(cls, prefix: str = "") -> str: - return cls.get_arm_base_link_name(prefix) - - @property - def robot_base_link_name(self) -> str: - return self.get_robot_base_link_name(self.prefix) - - @classmethod - def get_arm_base_link_name(cls, prefix: str = "") -> str: - # Same as `self.arm_link_names[0]`` - return "base_link" - - @property - def arm_base_link_name(self) -> str: - return self.get_arm_base_link_name(self.prefix) - - @classmethod - def get_ee_link_name(cls, prefix: str = "") -> str: - return "gripper_grasp_point" - - @property - def ee_link_name(self) -> str: - return self.get_ee_link_name(self.prefix) - - @classmethod - def get_wheel_link_names(cls, prefix: str = "") -> List[str]: - return [] - - @property - def wheel_link_names(self) -> List[str]: - return self.get_wheel_link_names(self.prefix) - - @classmethod - def get_arm_link_names(cls, prefix: str) -> List[str]: - return ["fork0_link", - "main0_link", - "fork1_link", - "main1_link", - "fork2_link", - "tool0", - "ee_link", - "rbs_gripper_rot_base_link"] - - @property - def arm_link_names(self) -> List[str]: - return self.get_arm_link_names(self.prefix) - - @classmethod - def get_gripper_link_names(cls, prefix: str) -> List[str]: - return [ - "rbs_gripper_l_finger_link", - "rbs_gripper_r_finger_link" + joint_position_data = [ + (self.__initial_arm_joint_positions, self._robot.actuated_joint_names), + (self.__initial_gripper_joint_positions, self._robot.gripper_actuated_joint_names), ] - @property - def gripper_link_names(self) -> List[str]: - return self.get_gripper_link_names(self.prefix) - - @classmethod - def get_model_file(cls) -> str: - return "/home/narmak/rbs_ws/current.urdf" - + for positions, joint_names in joint_position_data: + print(f"Setting joint positions for {joint_names}: {positions}") + if not model.reset_joint_positions(positions, joint_names): + raise RuntimeError(f"Failed to set initial positions of {joint_names[0].split('_')[0]}'s joints") diff --git a/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py b/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py deleted file mode 100644 index 0eb8b46..0000000 --- a/env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py +++ /dev/null @@ -1,106 +0,0 @@ -from gym_gz.utils.scenario import get_unique_model_name -from rclpy.node import Node -from robot_builder.parser.urdf import URDF_parser -from robot_builder.elements.robot import Robot - -from scenario import core as scenario_core -from scenario import gazebo as scenario_gazebo - -from .robot import RobotWrapper - - -class RbsArm(RobotWrapper): - """ - Experimental class with `robot_builder` - """ - - DEFAULT_ARM_JOINT_POSITIONS: list[float] = [ - 0.0, - 0.5, - 3.14159, - 1.5, - 0.0, - 1.4, - 0.0, - ] - - OPEN_GRIPPER_JOINT_POSITIONS: list[float] = [ - 0.064, - ] - - CLOSED_GRIPPER_JOINT_POSITIONS: list[float] = [ - 0.0, - ] - - DEFAULT_GRIPPER_JOINT_POSITIONS: list[float] = CLOSED_GRIPPER_JOINT_POSITIONS - - def __init__( - self, - world: scenario_gazebo.World, - node: Node, - urdf_string: str, - name: str = "rbs_arm", - position: tuple[float, float, float] = (0.0, 0.0, 0.0), - orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0), - initial_arm_joint_positions: list[float] | float = DEFAULT_ARM_JOINT_POSITIONS, - initial_gripper_joint_positions: list[float] - | float = DEFAULT_GRIPPER_JOINT_POSITIONS, - ): - # Get a unique model name - model_name = get_unique_model_name(world, name) - - # Setup initial pose - initial_pose = scenario_core.Pose(position, orientation) - - # Insert the model - ok_model = world.insert_model_from_string(urdf_string, initial_pose, model_name) - if not ok_model: - raise RuntimeError("Failed to insert " + model_name) - - # Get the model - model = world.get_model(model_name) - - # Parse robot to get metadata - self._robot: Robot = URDF_parser.load_string(urdf_string) - - self.__initial_gripper_joint_positions = ( - [float(initial_gripper_joint_positions)] - * len(self._robot.gripper_actuated_joint_names) - if isinstance(initial_gripper_joint_positions, (int, float)) - else initial_gripper_joint_positions - ) - - self.__initial_arm_joint_positions = ( - [float(initial_arm_joint_positions)] * len(self._robot.actuated_joint_names) - if isinstance(initial_arm_joint_positions, (int, float)) - else initial_arm_joint_positions - ) - - # Set initial joint configuration - self.set_initial_joint_positions(model) - super().__init__(model=model) - - @property - def robot(self) -> Robot: - return self._robot - - @property - def initial_arm_joint_positions(self) -> list[float]: - return self.__initial_arm_joint_positions - - @property - def initial_gripper_joint_positions(self) -> list[float]: - return self.__initial_gripper_joint_positions - - def set_initial_joint_positions(self, model): - model = model.to_gazebo() - - joint_position_data = [ - (self.__initial_arm_joint_positions, self._robot.actuated_joint_names), - (self.__initial_gripper_joint_positions, self._robot.gripper_actuated_joint_names), - ] - - for positions, joint_names in joint_position_data: - print(f"Setting joint positions for {joint_names}: {positions}") - if not model.reset_joint_positions(positions, joint_names): - raise RuntimeError(f"Failed to set initial positions of {joint_names[0].split('_')[0]}'s joints") diff --git a/env_manager/env_manager/env_manager/models/robots/robot.py b/env_manager/env_manager/env_manager/models/robots/robot.py index eacaa8f..fdfa062 100644 --- a/env_manager/env_manager/env_manager/models/robots/robot.py +++ b/env_manager/env_manager/env_manager/models/robots/robot.py @@ -5,6 +5,26 @@ from abc import abstractmethod class RobotWrapper(model_wrapper.ModelWrapper): + """ + An abstract base class for robot models in a Gazebo simulation. + + This class extends the ModelWrapper from gym_gz and provides a structure for creating + robot models with specific configurations, including joint positions for arms and grippers. + + Args: + world (scenario_gazebo.World, optional): The Gazebo world where the robot model will be inserted. + urdf_string (str, optional): The URDF (Unified Robot Description Format) string defining the robot. + name (str, optional): The name of the robot model. Defaults to None. + position (tuple[float, float, float], optional): The initial position of the robot in the world. Defaults to None. + orientation (tuple[float, float, float, float], optional): The initial orientation of the robot in quaternion format. Defaults to None. + initial_arm_joint_positions (list[float] | float, optional): The initial joint positions for the arm. Defaults to an empty list. + initial_gripper_joint_positions (list[float] | float, optional): The initial joint positions for the gripper. Defaults to an empty list. + model (model_wrapper.ModelWrapper, optional): An existing model instance to initialize the wrapper. Must be provided. + **kwargs: Additional keyword arguments. + + Raises: + ValueError: If the model parameter is not provided. + """ def __init__( self, world: scenario_gazebo.World | None = None, @@ -25,19 +45,51 @@ class RobotWrapper(model_wrapper.ModelWrapper): @property @abstractmethod def robot(self) -> Robot: + """Returns the robot instance containing metadata. + + This property must be implemented by subclasses to return the specific robot metadata. + + Returns: + Robot: The robot instance. + """ pass @property @abstractmethod def initial_gripper_joint_positions(self) -> list[float]: + """Returns the initial joint positions for the gripper. + + This property must be implemented by subclasses to provide the initial positions of the gripper joints. + + Returns: + list[float]: The initial joint positions for the gripper. + """ pass @property @abstractmethod def initial_arm_joint_positions(self) -> list[float]: + """Returns the initial joint positions for the arm. + + This property must be implemented by subclasses to provide the initial positions of the arm joints. + + Returns: + list[float]: The initial joint positions for the arm. + """ pass @abstractmethod def set_initial_joint_positions(self, model): + """Sets the initial positions for the robot's joints. + + This method must be implemented by subclasses to reset the joint positions of the robot + to their specified initial values. + + Args: + model: The model representation of the robot within the Gazebo environment. + + Raises: + RuntimeError: If resetting the joint positions fails. + """ pass diff --git a/env_manager/env_manager/env_manager/models/sensors/camera.py b/env_manager/env_manager/env_manager/models/sensors/camera.py index fa0b58b..9808bd6 100644 --- a/env_manager/env_manager/env_manager/models/sensors/camera.py +++ b/env_manager/env_manager/env_manager/models/sensors/camera.py @@ -4,15 +4,49 @@ from threading import Thread from gym_gz.scenario import model_wrapper from gym_gz.utils.scenario import get_unique_model_name from scenario import core as scenario_core -from scenario import gazebo as scenario_gz +from scenario import gazebo as scenario_gazebo from env_manager.models.utils import ModelCollectionRandomizer class Camera(model_wrapper.ModelWrapper): + """ + Represents a camera model in a Gazebo simulation. + + This class extends the ModelWrapper from gym_gz to define a camera model that can be inserted + into the Gazebo world. It supports different types of cameras and can bridge ROS2 topics for + camera data. + + Args: + world (scenario_gazebo.World): The Gazebo world where the camera will be inserted. + name (str, optional): The name of the camera model. If None, a unique name is generated. + position (tuple[float, float, float], optional): The initial position of the camera. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float], optional): The initial orientation of the camera in quaternion. Defaults to (1, 0, 0, 0). + static (bool, optional): If True, the camera is a static model. Defaults to True. + camera_type (str, optional): The type of camera to create. Defaults to "rgbd_camera". + width (int, optional): The width of the camera image. Defaults to 212. + height (int, optional): The height of the camera image. Defaults to 120. + image_format (str, optional): The format of the camera image. Defaults to "R8G8B8". + update_rate (int, optional): The update rate for the camera sensor. Defaults to 15. + horizontal_fov (float, optional): The horizontal field of view for the camera. Defaults to 1.567821. + vertical_fov (float, optional): The vertical field of view for the camera. Defaults to 1.022238. + clip_color (tuple[float, float], optional): The near and far clipping distances for color. Defaults to (0.02, 1000.0). + clip_depth (tuple[float, float], optional): The near and far clipping distances for depth. Defaults to (0.02, 10.0). + noise_mean (float, optional): The mean of the noise added to the camera images. Defaults to None. + noise_stddev (float, optional): The standard deviation of the noise added to the camera images. Defaults to None. + ros2_bridge_color (bool, optional): If True, a ROS2 bridge for color images is created. Defaults to False. + ros2_bridge_depth (bool, optional): If True, a ROS2 bridge for depth images is created. Defaults to False. + ros2_bridge_points (bool, optional): If True, a ROS2 bridge for point cloud data is created. Defaults to False. + visibility_mask (int, optional): The visibility mask for the camera sensor. Defaults to 0. + visual (str, optional): The type of visual representation for the camera. Defaults to None. + + Raises: + ValueError: If the visual mesh or textures cannot be found. + RuntimeError: If the camera model fails to insert into the Gazebo world. + """ def __init__( self, - world: scenario_gz.World, + world: scenario_gazebo.World, name: str | None = None, position: tuple[float, float, float] = (0, 0, 0), orientation: tuple[float, float, float, float] = (1, 0, 0, 0), @@ -33,7 +67,6 @@ class Camera(model_wrapper.ModelWrapper): ros2_bridge_points: bool = False, visibility_mask: int = 0, visual: str | None = None, - # visual: Optional[str] = "intel_realsense_d435", ): # Get a unique model name if name is not None: @@ -281,12 +314,21 @@ class Camera(model_wrapper.ModelWrapper): thread.start() def __del__(self): + """Cleans up threads when the Camera object is deleted.""" if hasattr(self, "__threads"): for thread in self.__threads: thread.join() @classmethod def construct_ros2_bridge(cls, topic: str, ros_msg: str, ign_msg: str): + """ + Constructs and runs a ROS2 bridge command for a given topic. + + Args: + topic (str): The topic to bridge. + ros_msg (str): The ROS2 message type to use. + ign_msg (str): The Ignition message type to use. + """ node_name = "parameter_bridge" + topic.replace("/", "_") command = ( f"ros2 run ros_gz_bridge parameter_bridge {topic}@{ros_msg}[{ign_msg} " @@ -296,46 +338,99 @@ class Camera(model_wrapper.ModelWrapper): @classmethod def get_frame_id(cls, model_name: str) -> str: + """ + Gets the frame ID for the camera model. + + Args: + model_name (str): The name of the camera model. + + Returns: + str: The frame ID. + """ return f"{model_name}/{model_name}_link/camera" @property def frame_id(self) -> str: + """Returns the frame ID of the camera.""" return self.get_frame_id(self._model_name) @classmethod def get_color_topic(cls, model_name: str, camera_type: str) -> str: + """ + Gets the color topic for the camera. + + Args: + model_name (str): The name of the camera model. + camera_type (str): The type of the camera. + + Returns: + str: The color topic. + """ return f"/{model_name}/image" if "rgbd" in camera_type else f"/{model_name}" @property def color_topic(self) -> str: + """Returns the color topic for the camera.""" return self.get_color_topic(self._model_name, self._camera_type) @classmethod def get_depth_topic(cls, model_name: str, camera_type: str) -> str: + """ + Gets the depth topic for the camera. + + Args: + model_name (str): The name of the camera model. + camera_type (str): The type of the camera. + + Returns: + str: The depth topic. + """ return ( f"/{model_name}/depth_image" if "rgbd" in camera_type else f"/{model_name}" ) @property def depth_topic(self) -> str: + """Returns the depth topic for the camera.""" return self.get_depth_topic(self._model_name, self._camera_type) @classmethod def get_points_topic(cls, model_name: str) -> str: + """ + Gets the points topic for the camera. + + Args: + model_name (str): The name of the camera model. + + Returns: + str: /{model_name}/points. + """ return f"/{model_name}/points" @property def points_topic(self) -> str: + """Returns the points topic for the camera.""" return self.get_points_topic(self._model_name) @property def info_topic(self): + """Returns the camera info topic.""" return f"/{self._model_name}/camera_info" @classmethod def get_link_name(cls, model_name: str) -> str: + """ + Gets the link name for the camera model. + + Args: + model_name (str): The name of the camera model. + + Returns: + str: {model_name}_link. + """ return f"{model_name}_link" @property def link_name(self) -> str: + """Returns the link name for the camera.""" return self.get_link_name(self._model_name) diff --git a/env_manager/env_manager/env_manager/models/terrains/ground.py b/env_manager/env_manager/env_manager/models/terrains/ground.py index 712e011..e5bb238 100644 --- a/env_manager/env_manager/env_manager/models/terrains/ground.py +++ b/env_manager/env_manager/env_manager/models/terrains/ground.py @@ -7,6 +7,26 @@ from scenario import core as scenario_core from scenario import gazebo as scenario_gazebo class Ground(model_wrapper.ModelWrapper): + """ + Represents a ground model in a Gazebo simulation. + + This class extends the ModelWrapper from gym_gz to define a ground model that can be + inserted into the Gazebo world. The ground is defined by its size, position, orientation, + and friction properties. + + Args: + world (scenario_gazebo.World): The Gazebo world where the ground will be inserted. + name (str, optional): The name of the ground model. Defaults to "ground". + position (tuple[float, float, float], optional): The initial position of the ground. Defaults to (0, 0, 0). + orientation (tuple[float, float, float, float], optional): The initial orientation of the ground in quaternion. Defaults to (1, 0, 0, 0). + size (tuple[float, float], optional): The size of the ground plane. Defaults to (1.5, 1.5). + collision_thickness (float, optional): The thickness of the collision surface. Defaults to 0.05. + friction (float, optional): The friction coefficient for the ground surface. Defaults to 5.0. + **kwargs: Additional keyword arguments for future extensions. + + Raises: + RuntimeError: If the ground model fails to insert into the Gazebo world. + """ def __init__( self, world: scenario_gazebo.World, diff --git a/env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py b/env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py index bbdd4f5..428a7a5 100644 --- a/env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py +++ b/env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py @@ -1,6 +1,5 @@ import glob import os -from typing import List, Optional, Tuple import numpy as np import trimesh @@ -14,6 +13,25 @@ from scenario import gazebo as scenario_gazebo class ModelCollectionRandomizer: + """ + A class to randomize and manage paths for model collections in simulation environments. + + This class allows for the selection and management of model paths, enabling features + like caching, blacklisting, and random sampling from a specified collection. + + Attributes: + _class_model_paths (list[str] | None): Class-level cache for model paths. + __sdf_base_name (str): The base name for the SDF model file. + __configured_sdf_base_name (str): The base name for the modified SDF model file. + __blacklisted_base_name (str): The base name used for blacklisted models. + __collision_mesh_dir (str): Directory where collision meshes are stored. + __collision_mesh_file_type (str): The file type of the collision meshes. + __original_scale_base_name (str): The base name for original scale metadata files. + _unique_cache (bool): Whether to use a unique cache for model paths. + _enable_blacklisting (bool): Whether to enable blacklisting of unusable models. + _model_paths (list[str] | None): Instance-specific model paths if unique cache is enabled. + np_random (RandomState): Numpy random state for random sampling. + """ _class_model_paths = None __sdf_base_name = "model.sdf" @@ -33,8 +51,25 @@ class ModelCollectionRandomizer: unique_cache=False, reset_collection=False, enable_blacklisting=True, - np_random: Optional[RandomState] = None, + np_random: RandomState | None = None, ): + """ + Initializes the ModelCollectionRandomizer. + + Args: + model_paths (list[str] | None): List of model paths to initialize with. + owner (str): The owner of the model collection (default is "GoogleResearch"). + collection (str): The name of the model collection (default is "Google Scanned Objects"). + server (str): The server URL for the model collection (default is "https://fuel.ignitionrobotics.org"). + server_version (str): The server version to use (default is "1.0"). + unique_cache (bool): Whether to use a unique cache for the instance (default is False). + reset_collection (bool): Whether to reset the class-level model paths (default is False). + enable_blacklisting (bool): Whether to enable blacklisting of models (default is True). + np_random (RandomState | None): Numpy random state for random operations (default is None). + + Raises: + ValueError: If an invalid model path is provided. + """ # If enabled, the newly created objects of this class will use its own individual cache # for model paths and must discover/download them on its own @@ -89,7 +124,27 @@ class ModelCollectionRandomizer: server="https://fuel.ignitionrobotics.org", server_version="1.0", model_name: str = "", - ) -> List[str]: + ) -> list[str]: + """ + Retrieves model paths from the local cache or downloads them from a specified server. + + The method first checks the local cache for models belonging to the specified owner. + If no models are found, it attempts to download the models from the specified Fuel server. + + Args: + cls: The class reference. + owner (str): The owner of the model collection (default is "GoogleResearch"). + collection (str): The name of the model collection (default is "Google Scanned Objects"). + server (str): The server URL for the model collection (default is "https://fuel.ignitionrobotics.org"). + server_version (str): The server version to use (default is "1.0"). + model_name (str): The name of the specific model to fetch (default is an empty string). + + Returns: + list[str]: A list of paths to the retrieved models. + + Raises: + RuntimeError: If the download command fails or if no models are found after the download. + """ # First check the local cache (for performance) # Note: This unfortunately does not check if models belong to the specified collection @@ -153,13 +208,42 @@ class ModelCollectionRandomizer: skip_blacklisted=True, return_sdf_path=True, ) -> str: + """ + Selects and configures a random model from the collection. + + The method attempts to find a valid model, applying randomization + and returning the path to the configured SDF file or the model directory. + + Args: + min_scale (float): Minimum scale for the model. + max_scale (float): Maximum scale for the model. + min_mass (float): Minimum mass for the model. + max_mass (float): Maximum mass for the model. + min_friction (float): Minimum friction coefficient. + max_friction (float): Maximum friction coefficient. + decimation_fraction_of_visual (float): Fraction of visual decimation. + decimation_min_faces (int): Minimum number of faces for decimation. + decimation_max_faces (int): Maximum number of faces for decimation. + max_faces (int): Maximum faces for the model. + max_vertices (int, optional): Maximum vertices for the model. + component_min_faces_fraction (float): Minimum face fraction for components. + component_max_volume_fraction (float): Maximum volume fraction for components. + fix_mtl_texture_paths (bool): Whether to fix MTL texture paths. + skip_blacklisted (bool): Whether to skip blacklisted models. + return_sdf_path (bool): If True, return the configured SDF file path. + + Returns: + str: Path to the configured SDF file or model directory. + + Raises: + RuntimeError: If a valid model cannot be found after multiple attempts. + """ # Loop until a model is found, checked for validity, configured and returned # If any of these steps fail, sample another model and try again # Note: Due to this behaviour, the function could stall if all models are invalid # TODO: Add a simple timeout to random sampling of valid model (# of attempts or time-based) while True: - # Get path to a random model from the collection model_path = self.get_random_model_path() @@ -215,6 +299,39 @@ class ModelCollectionRandomizer: component_max_volume_fraction=0.35, fix_mtl_texture_paths=True, ): + """ + Processes all models in the collection, applying configuration and decimation. + + This method iterates over each model in the collection, applying visual decimation + and checking for validity. If a model cannot be processed successfully, it is + blacklisted. The method tracks and prints the number of processed models and + the count of blacklisted models at the end of execution. + + Args: + decimation_fraction_of_visual (float): Fraction of visual decimation to apply. + Default is 0.025. + decimation_min_faces (int): Minimum number of faces for decimation. Default is 8. + decimation_max_faces (int): Maximum number of faces for decimation. Default is 400. + max_faces (int): Maximum faces allowed for the model. Default is 40000. + max_vertices (int, optional): Maximum vertices allowed for the model. Default is None. + component_min_faces_fraction (float): Minimum face fraction for components during + processing. Default is 0.1. + component_max_volume_fraction (float): Maximum volume fraction for components during + processing. Default is 0.35. + fix_mtl_texture_paths (bool): Whether to fix MTL texture paths for the models. + Default is True. + + Returns: + None: This method modifies the models in place and does not return a value. + + Prints: + - The status of each processed model along with its index. + - The total number of blacklisted models after processing all models. + + Raises: + None: The method does not raise exceptions but may log issues if models cannot be + processed. + """ if self._unique_cache: model_paths = self._model_paths else: @@ -251,16 +368,54 @@ class ModelCollectionRandomizer: component_max_volume_fraction=0.35, fix_mtl_texture_paths=True, ) -> bool: + """ + Processes a specified model to configure it for simulation, applying geometry + decimation and checking for various validity criteria. + + This method parses the SDF (Simulation Description Format) of the specified + model, processes its components, checks for geometry validity, and updates + inertial properties as needed. The processed model is then saved back to + an SDF file. + + Args: + model_path (str): Path to the model to be processed. + decimation_fraction_of_visual (float): Fraction of visual geometry to retain + during decimation. Default is 0.25. + decimation_min_faces (int): Minimum number of faces allowed in the decimated + visual geometry. Default is 40. + decimation_max_faces (int): Maximum number of faces allowed in the decimated + visual geometry. Default is 200. + max_faces (int): Maximum number of faces for the entire model. Default is 40000. + max_vertices (Optional[int]): Maximum number of vertices for the model. Default is None. + component_min_faces_fraction (float): Minimum face fraction for components to + be considered valid during processing. Default is 0.1. + component_max_volume_fraction (float): Maximum volume fraction for components + to be considered valid during processing. Default is 0.35. + fix_mtl_texture_paths (bool): Whether to fix texture paths in MTL files for + mesh formats. Default is True. + + Returns: + bool: Returns True if the model was processed successfully; otherwise, + returns False if any validity checks fail or if processing issues are encountered. + + Raises: + None: The method does not raise exceptions but may return False if processing + fails due to model invalidity. + + Notes: + - The model is blacklisted if it fails to meet the specified criteria for + geometry and inertial properties. + - The method updates the SDF of the model with processed geometries and + inertial properties. + """ # Parse the SDF of the model sdf = parse_sdf(self.get_sdf_path(model_path)) # Process the model(s) contained in the SDF for model in sdf.models: - # Process the link(s) of each model for link in model.links: - # Get rid of the existing collisions prior to simplifying it link.collisions.clear() @@ -272,7 +427,6 @@ class ModelCollectionRandomizer: # Go through the visuals and process them for visual in link.visuals: - # Get path to the mesh of the link's visual mesh_path = self.get_mesh_path(model_path, visual) @@ -348,8 +502,42 @@ class ModelCollectionRandomizer: max_faces=750, friction=1.0, ): + """ + Adds collision geometry to a specified link in a model, using a simplified + version of the provided mesh. - # Determine name of path to the collistion geometry + This method creates a collision mesh by decimating the visual geometry of + the model, generating an SDF (Simulation Description Format) element for + the collision, and adding it to the specified link. + + Args: + mesh (trimesh.Trimesh): The original mesh geometry from which the + collision geometry will be created. + link (Link): The link to which the collision geometry will be added. + model_path (str): The path to the model where the collision mesh will + be stored. + fraction_of_visual (float): The fraction of the visual mesh to retain + for the collision mesh. Default is 0.05. + min_faces (int): The minimum number of faces allowed in the collision + mesh after decimation. Default is 8. + max_faces (int): The maximum number of faces allowed in the collision + mesh after decimation. Default is 750. + friction (float): The friction coefficient to be applied to the collision + surface. Default is 1.0. + + Returns: + None: This method does not return a value. + + Notes: + - The collision mesh is generated through quadratic decimation of the + original visual mesh based on the specified parameters. + - The created collision mesh is stored in the same directory as the + model, and its path is relative to the model path. + - The method updates the link's SDF with the collision geometry and + surface properties. + """ + + # Determine name of path to the collision geometry collision_name = ( link.attributes["name"] + "_collision_" + str(len(link.collisions)) ) @@ -391,8 +579,7 @@ class ModelCollectionRandomizer: def sum_inertial_properties( self, mesh, total_mass, total_inertia, common_centre_of_mass, density=1.0 - ) -> Tuple[float, float, float]: - + ) -> tuple[float, float, float]: # Arbitrary density is used here # The mass will be randomized once it is fully computed for a link mesh.density = density @@ -423,6 +610,38 @@ class ModelCollectionRandomizer: min_friction=0.75, max_friction=1.5, ): + """ + Randomizes the scale, mass, and friction properties of a configured model + in the specified SDF file. + + This method modifies the properties of each link in the model, applying + random values within specified ranges for scale, mass, and friction. + The randomized values overwrite the original settings in the configured + SDF file. + + Args: + model_path (str): The path to the model directory containing the + configured SDF file. + min_scale (float): The minimum scale factor for randomization. + Default is 0.05. + max_scale (float): The maximum scale factor for randomization. + Default is 0.25. + min_mass (float): The minimum mass for randomization. Default is 0.1. + max_mass (float): The maximum mass for randomization. Default is 3.0. + min_friction (float): The minimum friction coefficient for randomization. + Default is 0.75. + max_friction (float): The maximum friction coefficient for randomization. + Default is 1.5. + + Returns: + None: This method does not return a value. + + Notes: + - The configured SDF file is updated in place, meaning the original + properties are overwritten with the new randomized values. + - The randomization is applied to each link in the model, ensuring + a diverse range of physical properties. + """ # Get path to the configured SDF file configured_sdf_path = self.get_configured_sdf_path(model_path) @@ -432,10 +651,8 @@ class ModelCollectionRandomizer: # Process the model(s) contained in the SDF for model in sdf.models: - # Process the link(s) of each model for link in model.links: - # Randomize scale of the link self.randomize_scale( model_path, link, min_scale=min_scale, max_scale=max_scale @@ -453,6 +670,38 @@ class ModelCollectionRandomizer: sdf.export_xml(configured_sdf_path) def randomize_scale(self, model_path, link, min_scale=0.05, max_scale=0.25): + """ + Randomizes the scale of a link's geometry within a specified range. + + This method modifies the scale of the visual and collision geometry + of a link in the model. The scale is randomized using a uniform + distribution within the given minimum and maximum scale values. + Additionally, the link's inertial properties (mass and inertia) + are recalculated based on the new scale. + + Args: + model_path (str): The path to the model directory, used to + read the original scale of the mesh. + link (Link): The link object containing the visual and collision + geometries to be scaled. + min_scale (float): The minimum scale factor for randomization. + Default is 0.05. + max_scale (float): The maximum scale factor for randomization. + Default is 0.25. + + Returns: + bool: Returns `False` if the link has more than one visual, + indicating that scaling is not supported. Returns `None` + if the scaling is successful. + + Notes: + - This method currently supports only links that contain a + single mesh geometry. + - The mass of the link is scaled by the cube of the scale factor, + while the inertial properties are scaled by the fifth power of + the scale factor. + - The scale values are applied uniformly to all dimensions (x, y, z). + """ # Note: This function currently supports only scaling of links with single mesh geometry if len(link.visuals) > 1: @@ -492,7 +741,28 @@ class ModelCollectionRandomizer: def randomize_inertial( self, link, min_mass=0.1, max_mass=3.0 - ) -> Tuple[float, float]: + ) -> tuple[float, float]: + """ + Randomizes the mass and updates the inertial properties of a link. + + This method assigns a random mass to the link within the specified + range and recalculates its inertial properties based on the new mass. + + Args: + link (Link): The link object whose mass and inertial properties + will be randomized. + min_mass (float): The minimum mass for randomization. Default is 0.1. + max_mass (float): The maximum mass for randomization. Default is 3.0. + + Returns: + tuple[float, float]: A tuple containing the new mass and the + mass scale factor used to update the inertial properties. + + Notes: + - The method modifies the link's mass and updates its + inertial properties (ixx, iyy, izz, ixy, ixz, iyz) based on the + ratio of the new mass to the original mass. + """ random_mass = self.np_random.uniform(min_mass, max_mass) mass_scale_factor = random_mass / link.mass.value @@ -506,6 +776,25 @@ class ModelCollectionRandomizer: link.inertia.iyz = link.inertia.iyz.value * mass_scale_factor def randomize_friction(self, link, min_friction=0.75, max_friction=1.5): + """ + Randomizes the friction coefficients of a link's collision surfaces. + + This method assigns random friction values to each collision surface + of the link within the specified range. + + Args: + link (Link): The link object whose collision surfaces' friction + coefficients will be randomized. + min_friction (float): The minimum friction coefficient for + randomization. Default is 0.75. + max_friction (float): The maximum friction coefficient for + randomization. Default is 1.5. + + Notes: + - The friction coefficients are applied to both the + 'mu' and 'mu2' attributes of the collision surface's friction + properties. + """ for collision in link.collisions: random_friction = self.np_random.uniform(min_friction, max_friction) @@ -514,6 +803,24 @@ class ModelCollectionRandomizer: collision.surface.friction.ode.mu2 = random_friction def write_inertial_properties(self, link, mass, inertia, centre_of_mass): + """ + Writes the specified mass and inertial properties to a link. + + This method updates the link's mass, inertia tensor, and + centre of mass based on the provided values. + + Args: + link (Link): The link object to be updated with new inertial properties. + mass (float): The mass to set for the link. + inertia (list[list[float]]): A 3x3 list representing the + inertia tensor of the link. + centre_of_mass (list[float]): A list containing the x, y, z + coordinates of the link's centre of mass. + + Notes: + - The method directly modifies the link's mass and inertia properties, + as well as its inertial pose, which is set to the specified centre of mass. + """ link.mass = mass @@ -534,12 +841,47 @@ class ModelCollectionRandomizer: ] def write_original_scale(self, mesh, model_path): + """ + Writes the original scale of the mesh to a file. + + This method records the scale of the provided mesh into a text file, + which can be used later for reference during randomization or scaling + operations. + + Args: + mesh (Mesh): The mesh object whose original scale is to be recorded. + model_path (str): The path to the model directory where the + original scale file will be saved. + + Notes: + - The scale is written as a string representation of the mesh's + scale property. + - The file is created or overwritten at the location returned + by the `get_original_scale_path` method. + """ file = open(self.get_original_scale_path(model_path), "w") file.write(str(mesh.scale)) file.close() def read_original_scale(self, model_path) -> float: + """ + Reads the original scale of a model from a file. + + This method retrieves the original scale value that was previously + saved for a specific model. The scale is expected to be stored as + a string in a file, and this method converts it back to a float. + + Args: + model_path (str): The path to the model directory from which + to read the original scale. + + Returns: + float: The original scale of the model. + + Raises: + ValueError: If the contents of the scale file cannot be converted to a float. + """ file = open(self.get_original_scale_path(model_path), "r") original_scale = file.read() @@ -550,6 +892,24 @@ class ModelCollectionRandomizer: def check_excessive_geometry( self, mesh, model_path, max_faces=40000, max_vertices=None ) -> bool: + """ + Checks if the mesh exceeds the specified geometry limits. + + This method evaluates the number of faces and vertices in the + given mesh. If the mesh exceeds the defined limits for faces + or vertices, it blacklists the model and returns False. + + Args: + mesh (Mesh): The mesh object to be checked for excessive geometry. + model_path (str): The path to the model directory for logging purposes. + max_faces (int, optional): The maximum allowed number of faces. + Defaults to 40000. + max_vertices (int, optional): The maximum allowed number of vertices. + Defaults to None. + + Returns: + bool: True if the mesh does not exceed the limits; otherwise, False. + """ if max_faces is not None: num_faces = len(mesh.faces) @@ -576,8 +936,28 @@ class ModelCollectionRandomizer: component_min_faces_fraction=0.05, component_max_volume_fraction=0.1, ) -> bool: + """ + Checks for disconnected components within the mesh. - # Get a list of all connected componends inside the mesh + This method identifies connected components within the mesh. If + there are multiple components, it evaluates their relative volumes. + If any component exceeds the specified volume fraction threshold, + the model is blacklisted. + + Args: + mesh (Mesh): The mesh object to be evaluated for disconnected components. + model_path (str): The path to the model directory for logging purposes. + component_min_faces_fraction (float, optional): The minimum fraction + of faces required for a component to be considered. Defaults to 0.05. + component_max_volume_fraction (float, optional): The maximum allowed + volume fraction for a component. Defaults to 0.1. + + Returns: + bool: True if the mesh has no significant disconnected components; + otherwise, False. + """ + + # Get a list of all connected components inside the mesh # Consider components only with `component_min_faces_fraction` percent faces min_faces = round(component_min_faces_fraction * len(mesh.faces)) connected_components = trimesh.graph.connected_components( @@ -610,7 +990,21 @@ class ModelCollectionRandomizer: return True def check_inertial_properties(self, model_path, mass, inertia) -> bool: + """ + Checks the validity of inertial properties for a model. + This method evaluates whether the mass and the principal moments of + inertia are valid. If any of these values are below a specified threshold, + the model is blacklisted. + + Args: + model_path (str): The path to the model directory for logging purposes. + mass (float): The mass of the model to be checked. + inertia (list of list of float): A 2D list representing the inertia matrix. + + Returns: + bool: True if the inertial properties are valid; otherwise, False. + """ if ( mass < 1e-10 or inertia[0][0] < 1e-10 @@ -623,14 +1017,34 @@ class ModelCollectionRandomizer: return True def get_random_model_path(self) -> str: + """ + Retrieves a random model path from the available models. + This method selects and returns a random model path. The selection + depends on whether a unique cache is being used or not. + + Returns: + str: A randomly selected model path. + """ if self._unique_cache: return self.np_random.choice(self._model_paths) else: return self.np_random.choice(self._class_model_paths) def get_collision_mesh_path(self, model_path, collision_name) -> str: + """ + Constructs the path for the collision mesh file. + This method generates and returns the file path for a collision + mesh based on the model path and the name of the collision. + + Args: + model_path (str): The path to the model directory. + collision_name (str): The name of the collision geometry. + + Returns: + str: The full path to the collision mesh file. + """ return os.path.join( model_path, self.__collision_mesh_dir, @@ -638,29 +1052,99 @@ class ModelCollectionRandomizer: ) def get_sdf_path(self, model_path) -> str: + """ + Constructs the path for the SDF (Simulation Description Format) file. + This method generates and returns the file path for the SDF file + corresponding to the given model path. + + Args: + model_path (str): The path to the model directory. + + Returns: + str: The full path to the SDF file. + """ return os.path.join(model_path, self.__sdf_base_name) def get_configured_sdf_path(self, model_path) -> str: + """ + Constructs the path for the configured SDF file. + This method generates and returns the file path for the configured + SDF file that includes any modifications made to the original model. + + Args: + model_path (str): The path to the model directory. + + Returns: + str: The full path to the configured SDF file. + """ return os.path.join(model_path, self.__configured_sdf_base_name) def get_blacklisted_path(self, model_path) -> str: + """ + Constructs the path for the blacklisted model file. + This method generates and returns the file path where information + about blacklisted models is stored. + + Args: + model_path (str): The path to the model directory. + + Returns: + str: The full path to the blacklisted model file. + """ return os.path.join(model_path, self.__blacklisted_base_name) def get_mesh_path(self, model_path, visual_or_collision) -> str: + """ + Constructs the path for the mesh associated with a visual or collision. + This method retrieves the URI from the specified visual or collision + object and constructs the full path to the corresponding mesh file. + + Args: + model_path (str): The path to the model directory. + visual_or_collision (object): An object containing the geometry + information for either visual or collision. + + Returns: + str: The full path to the mesh file. + + Notes: + This method may require adjustments for specific collections or models. + """ # TODO: This might need fixing for certain collections/models mesh_uri = visual_or_collision.geometry.mesh.uri.value return os.path.join(model_path, mesh_uri) def get_original_scale_path(self, model_path) -> str: + """ + Constructs the path for the original scale file. + This method generates and returns the file path for the file + that stores the original scale of the model. + + Args: + model_path (str): The path to the model directory. + + Returns: + str: The full path to the original scale file. + """ return os.path.join(model_path, self.__original_scale_base_name) def blacklist_model(self, model_path, reason="Unknown"): + """ + Blacklists a model by writing its path and the reason to a file. + If blacklisting is enabled, this method writes the specified reason + for blacklisting the model to a designated file. It also logs the + blacklisting action. + + Args: + model_path (str): The path to the model directory. + reason (str): The reason for blacklisting the model. Default is "Unknown". + """ if self._enable_blacklisting: bl_file = open(self.get_blacklisted_path(model_path), "w") bl_file.write(reason) @@ -675,18 +1159,58 @@ class ModelCollectionRandomizer: ) def is_blacklisted(self, model_path) -> bool: + """ + Checks if a model is blacklisted. + This method checks if the blacklisted file for a given model exists, + indicating that the model has been blacklisted. + + Args: + model_path (str): The path to the model directory. + + Returns: + bool: True if the model is blacklisted; otherwise, False. + """ return os.path.isfile(self.get_blacklisted_path(model_path)) def is_configured(self, model_path) -> bool: + """ + Checks if a model is configured. + This method checks if the configured SDF file for a given model + exists, indicating that the model has been processed and configured. + + Args: + model_path (str): The path to the model directory. + + Returns: + bool: True if the model is configured; otherwise, False. + """ return os.path.isfile(self.get_configured_sdf_path(model_path)) def fix_mtl_texture_paths(self, model_path, mesh_path, model_name): + """ + Fixes the texture paths in the associated MTL file of an OBJ model. + This method modifies the texture paths in the MTL file associated + with a given OBJ file to ensure they are correctly linked to + the model's textures. It also ensures that the texture files are + uniquely named by prepending the model's name to avoid conflicts. + + Args: + model_path (str): The path to the model directory where textures are located. + mesh_path (str): The path to the OBJ file for which the MTL file needs fixing. + model_name (str): The name of the model, used to make texture file names unique. + + Notes: + - This method scans the model directory for texture files, identifies + the relevant MTL file, and updates any texture paths in that MTL file. + - If a texture file's name does not include the model's name, the method + renames it to include the model's name. + - The method only processes MTL files linked to OBJ files. + """ # The `.obj` files use mtl if mesh_path.endswith(".obj"): - # Find all textures located in the model path, used later to relative linking texture_files = glob.glob(os.path.join(model_path, "**", "textures", "*.*")) @@ -714,10 +1238,7 @@ class ModelCollectionRandomizer: texture_file ) == map_file or os.path.basename( texture_file - ) == os.path.basename( - map_file - ): - + ) == os.path.basename(map_file): # Make the file unique to the model (unless it already is) if model_name in texture_file: new_texture_file_name = texture_file diff --git a/env_manager/env_manager/env_manager/scene/scene.py b/env_manager/env_manager/env_manager/scene/scene.py index 1816988..74a1056 100644 --- a/env_manager/env_manager/env_manager/scene/scene.py +++ b/env_manager/env_manager/env_manager/scene/scene.py @@ -23,7 +23,21 @@ import asyncio # TODO: Split scene randomizer and scene class Scene: """ - Manager scene during runtime, including randomization. + Manages the simulation scene during runtime, including object and environment randomization. + + The Scene class initializes and manages various components in a Gazebo simulation + environment, such as robots, cameras, lights, and terrain. It also provides methods + for scene setup, parameter retrieval, and scene initialization. + + Attributes: + gazebo (GazeboSimulator): An instance of the Gazebo simulator. + robot (RobotData): The robot data configuration. + cameras (list[CameraData]): List of camera configurations. + objects (list[ObjectData]): List of object configurations in the scene. + node (Node): The ROS 2 node for communication. + light (LightData): The light configuration for the scene. + terrain (TerrainData): The terrain configuration for the scene. + world (World): The Gazebo world instance. """ def __init__( @@ -33,6 +47,15 @@ class Scene: scene: SceneData, robot_urdf_string: str, ) -> None: + """ + Initializes the Scene object with the necessary components and parameters. + + Args: + node (Node): The ROS 2 node for managing communication and parameters. + gazebo (GazeboSimulator): An instance of the Gazebo simulator to manage the simulation. + scene (SceneData): A data object containing configuration for the scene. + robot_urdf_string (str): The URDF string of the robot model. + """ self.gazebo = gazebo self.robot = scene.robot self.cameras = scene.camera @@ -47,10 +70,6 @@ class Scene: self.__plugint_sensor_render_engine = "ogre2" self.world: World = gazebo.get_world() - # if not node.has_parameter("robot_description"): - # node.declare_parameter("robot_description") - - print(node._parameters) self.__objects_unique_names = {} self.__object_positions: dict[str, Point] = {} self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0) @@ -60,7 +79,11 @@ class Scene: GetParameters, "/robot_state_publisher/get_parameters" ) asyncio.run(self.get_parameter_async()) - scene.robot.urdf_string = node.get_parameter("robot_description").get_parameter_value().string_value + scene.robot.urdf_string = ( + node.get_parameter("robot_description") + .get_parameter_value() + .string_value + ) else: scene.robot.urdf_string = robot_urdf_string @@ -85,6 +108,17 @@ class Scene: ) async def get_parameter_async(self): + """ + Asynchronously retrieves the robot_description parameter from the robot state publisher. + + This method waits for the parameter service to become available and then sends + a request to retrieve the robot_description parameter. If the parameter is found, + it updates the local node parameter. + + Raises: + RuntimeError: If the robot_description parameter is not found or if the + response type is unexpected. + """ while not self.param_client.wait_for_service(timeout_sec=1.0): self.node.get_logger().info( "Waiting for parameter service [robot_state_publisher]..." @@ -97,7 +131,9 @@ class Scene: match response: case None: - raise RuntimeError("robot_description parameter not found in robot_state_publisher") + raise RuntimeError( + "robot_description parameter not found in robot_state_publisher" + ) case GetParameters.Response(values=[value]) if value.string_value: self.node.get_logger().info(f"Got parameter: {value.string_value}") param = Parameter( @@ -105,12 +141,30 @@ class Scene: ) self.node.set_parameters([param]) case _: - raise RuntimeError(f"I don't know what but the type of robot_description parameter is {type(response)}") + raise RuntimeError( + f"I don't know what but the type of robot_description parameter is {type(response)}" + ) def create_get_parameters_request(self) -> GetParameters.Request: + """ + Creates a request to get the parameters from the robot state publisher. + + Returns: + GetParameters.Request: A request object containing the parameter name to retrieve. + """ return GetParameters.Request(names=["robot_description"]) def init_scene(self): + """ + Initializes the simulation scene by adding the terrain, lights, robots, and objects. + + This method pauses the Gazebo simulation, sets up the physics engine, and + configures the environment and components as defined in the SceneData. + + Note: + This method must be called after the Scene object has been initialized + and before running the simulation. + """ self.gazebo_paused_run() self.init_world_plugins() @@ -128,6 +182,21 @@ class Scene: self.__scene_initialized = True def reset_scene(self): + """ + Resets the scene to its initial state by resetting the robot joint positions + and randomizing the positions of objects and cameras. + + This method performs the following actions: + - Resets the joint positions of the robot. + - Clears previously stored object positions. + - For each object in the scene, if randomization is enabled, + randomizes its position; otherwise, resets it to its original pose. + - Randomizes the pose of enabled cameras if their pose has expired. + + Note: + This method assumes that the objects and cameras have been properly initialized + and that their properties (like randomization and pose expiration) are set correctly. + """ self.reset_robot_joint_positions() object_randomized = set() @@ -146,6 +215,22 @@ class Scene: self.randomize_camera_pose(camera=camera) def init_world_plugins(self): + """ + Initializes and inserts world plugins into the Gazebo simulator for various functionalities. + + This method configures the following plugins based on the internal settings: + - **SceneBroadcaster**: Inserts the scene broadcaster plugin to enable GUI clients to receive + updates about the scene. This is only done if the scene broadcaster is enabled. + - **UserCommands**: Inserts the user commands plugin to allow user interactions with the scene. + - **Sensors**: Inserts the sensors plugin if any cameras are enabled. The rendering engine for + sensors can be specified. + - **ForceTorque**: Inserts the Force Torque Sensor plugin if enabled. + + The method pauses the Gazebo simulation after inserting plugins to ensure that changes take effect. + + Note: + If plugins are already active, this method does not reinsert them. + """ # SceneBroadcaster if self.__plugin_scene_broadcaster: if not self.gazebo.scene_broadcaster_active(self.world.to_gazebo().name()): @@ -199,7 +284,24 @@ class Scene: def add_robot(self): """ - Configure and insert robot into the simulation + Configure and insert the robot into the simulation. + + This method performs the following actions: + - Retrieves the robot model class based on the robot's name. + - Instantiates the robot model with specified parameters including position, orientation, + URDF string, and initial joint positions for both the arm and gripper. + - Ensures that the instantiated robot's name is unique and returns the actual name used. + - Enables contact detection for all gripper links (fingers) of the robot. + - Pauses the Gazebo simulation to apply the changes. + - Resets the robot's joints to their default positions. + + Note: + The method assumes that the robot name is valid and that the associated model class + can be retrieved. If the robot's URDF string or joint positions are not correctly specified, + this may lead to errors during robot instantiation. + + Raises: + RuntimeError: If the robot model class cannot be retrieved for the specified robot name. """ robot_model_class = get_robot_model_class(self.robot.name) @@ -222,21 +324,8 @@ class Scene: # Enable contact detection for all gripper links (fingers) robot_gazebo = self.robot_wrapper.to_gazebo() - # for gripper_link_name in self.robot.gripper_link_names: - # finger = robot_gazebo.get_link(link_name=gripper_link_name) - # finger.enable_contact_detection(True) robot_gazebo.enable_contacts() - # for arm_link_name in self.robot_wrapper.arm_link_names: - # link = robot_gazebo.get_link(link_name=arm_link_name) - # link.enable_contact_detection(True) - - # # If mobile, enable contact detection also for the wheels - # if self.robot.is_mobile: - # for wheel_link_name in self.robot.wheel_link_names: - # wheel = robot_gazebo.get_link(link_name=wheel_link_name) - # wheel.enable_contact_detection(True) - self.gazebo_paused_run() # Reset robot joints to the defaults @@ -244,7 +333,28 @@ class Scene: def add_camera(self, camera: CameraData): """ - Configure and insert camera into the simulation. Camera is places with respect to the robot + Configure and insert a camera into the simulation, placing it with respect to the robot. + + The method performs the following steps: + - Determines the appropriate position and orientation of the camera based on whether it is + relative to the world or to the robot. + - Creates an instance of the `Camera` class with the specified parameters, including + position, orientation, type, dimensions, image formatting, update rate, field of view, + clipping properties, noise characteristics, and ROS 2 bridge publishing options. + - Pauses the Gazebo simulation to apply changes. + - Attaches the camera to the robot if the camera's reference is not the world. + - Broadcasts the transformation (TF) of the camera relative to the robot. + + Parameters: + camera (CameraData): The camera data containing configuration options including: + + Raises: + Exception: If the camera cannot be attached to the robot. + + Notes: + The method expects the camera data to be correctly configured. If the camera's + `relative_to` field does not match any existing model in the world, it will raise an + exception when attempting to attach the camera to the robot. """ if self.world.to_gazebo().name() == camera.relative_to: @@ -308,6 +418,28 @@ class Scene: ) def add_object(self, obj: ObjectData): + """ + Configure and insert an object into the simulation. + + This method creates an instance of the specified object type (e.g., box, plane, sphere, + cylinder, mesh) and initializes it in the Gazebo simulation environment. The method + also handles unique naming for objects and enables contact detection for their links. + + Parameters: + obj (ObjectData): The object data containing configuration options including: + + Raises: + NotImplementedError: If the specified type is not supported. + + Notes: + - The method uses the `asdict` function to convert the object data to a dictionary format, + which is then passed as keyword arguments to the corresponding object class constructor. + - If the object type is not specified, the method attempts to match the object against + specific object data classes (e.g., `BoxObjectData`, `PlaneObjectData`) to determine + the appropriate class to instantiate. + - Contact detection is enabled for all links of the inserted object. + - Any exceptions raised during the insertion process will be logged as warnings. + """ obj_dict = asdict(obj) obj_dict["world"] = self.world try: @@ -366,7 +498,23 @@ class Scene: def add_light(self): """ - Configure and insert default light into the simulation + Configure and insert a default light source into the simulation. + + This method initializes and adds a light source to the Gazebo simulation based on the + specified light type. Currently, it supports a "sun" type light. If an unsupported + light type is specified, it raises an error. + + Raises: + RuntimeError: If the specified light type is unsupported, including: + - "random_sun": Currently not implemented. + - Any other unrecognized light type. + + Notes: + - The method constructs the light using the properties defined in the `self.light` object, + which should include direction, color, distance, visual properties, and radius, + as applicable for the specified light type. + - After adding the light to the simulation, the method pauses the Gazebo simulation + to ensure the scene is updated. """ # Create light @@ -391,7 +539,23 @@ class Scene: def add_terrain(self): """ - Configure and insert default terrain into the simulation + Configure and insert default terrain into the simulation. + + This method initializes and adds a terrain element to the Gazebo simulation using + the properties specified in the `self.terrain` object. Currently, it creates a + ground plane, but there are plans to support additional terrain types in the future. + + Raises: + NotImplementedError: If there are attempts to add terrain types other than + the default ground. + + Notes: + - The method utilizes properties such as name, spawn position, spawn quaternion + (for orientation), and size defined in `self.terrain` to create the terrain. + - After the terrain is added, contact detection is enabled for all links associated + with the terrain object. + - The method pauses the Gazebo simulation after adding the terrain to ensure + changes are reflected in the environment. """ # TODO: add another terrains @@ -411,6 +575,28 @@ class Scene: self.gazebo_paused_run() def randomize_object_position(self, object: ObjectData): + """ + Randomize the position and/or orientation of a specified object in the simulation. + + This method retrieves the names of the object instances from the scene and updates + their positions and orientations based on the randomization settings defined in + the `object` parameter. The method applies random transformations to the object's + pose according to the specified flags in the `ObjectData` instance. + + Args: + object (ObjectData): An instance of `ObjectData` containing the properties + of the object to be randomized. + + Raises: + KeyError: If the object name is not found in the unique object names dictionary. + + Notes: + - If `random_pose` is `True`, both position and orientation are randomized. + - If `random_orientation` is `True`, only the orientation is randomized, while + the position remains unchanged. + - If `random_position` is `True`, only the position is randomized, while the + orientation remains unchanged. + """ task_object_names = self.__objects_unique_names[object.name] for object_name in task_object_names: position, quat_random = self.get_random_object_pose(object) @@ -427,6 +613,30 @@ class Scene: self.gazebo_paused_run() def randomize_camera_pose(self, camera: CameraData): + """ + Randomize the pose of a specified camera in the simulation. + + This method updates the camera's position and orientation based on the selected + randomization mode. The new pose is computed relative to either the world frame or + a specified target model (e.g., the robot). The camera can be detached from the robot + during the pose update and reattached afterward if needed. + + Args: + camera (CameraData): An instance of `CameraData` containing properties of the + camera to be randomized. + + Raises: + TypeError: If the randomization mode specified in `camera.random_pose_mode` is + invalid. + Exception: If the camera cannot be detached from or attached to the robot. + + Notes: + - Supported randomization modes for camera pose: + - `"orbit"`: The camera is positioned in an orbital path around the object. + - `"select_random"`: A random camera pose is selected from predefined options. + - `"select_nearest"`: The camera is positioned based on the nearest predefined + camera pose. + """ # Get random camera pose, centered at object position (or center of object spawn box) if "orbit" == camera.random_pose_mode: camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit( @@ -502,6 +712,28 @@ class Scene: ) def get_random_camera_pose_orbit(self, camera: CameraData) -> Pose: + """ + Generate a random camera pose in an orbital path around a focal point. + + This method computes a random position for the camera based on an orbital distance + and height range. The camera's orientation is calculated to focus on a specified + focal point. The generated position ensures that it does not fall within a specified + arc behind the robot. + + Args: + camera (CameraData): An instance of `CameraData` containing properties of the + camera, including the orbital distance, height range, + focal point offset, and arc behind the robot to ignore. + + Returns: + Pose: A tuple containing the randomly generated position and orientation + (as a quaternion) of the camera. + + Notes: + - The method utilizes a random number generator to create a uniformly + distributed position within specified bounds. + - The computed camera orientation is represented in the quaternion format. + """ rng = np.random.default_rng() while True: @@ -537,6 +769,20 @@ class Scene: return tuple(position), tuple(quat_xyzw) def get_random_camera_pose_sample_random(self, camera: CameraData) -> Pose: + """ + Select a random position from the predefined camera position options. + + This method randomly selects one of the available camera position options + specified in the `camera` object and processes it to generate the camera's pose. + + Args: + camera (CameraData): An instance of `CameraData` containing the available + position options for random selection. + + Returns: + Pose: A tuple containing the selected camera position and its orientation + (as a quaternion). + """ # Select a random entry from the options camera.random_pose_select_position_options @@ -547,6 +793,20 @@ class Scene: return self.get_random_camera_pose_sample_process(camera, selection) def get_random_camera_pose_sample_nearest(self, camera: CameraData) -> Pose: + """ + Select the nearest entry from the predefined camera position options. + + This method calculates the squared distances of the camera position options + from a central point and selects the nearest one to generate the camera's pose. + + Args: + camera (CameraData): An instance of `CameraData` containing the position options + to choose from. + + Returns: + Pose: A tuple containing the nearest camera position and its orientation + (as a quaternion). + """ # Select the nearest entry dist_sqr = np.sum( ( @@ -563,6 +823,25 @@ class Scene: def get_random_camera_pose_sample_process( self, camera: CameraData, position: Point ) -> Pose: + """ + Process the selected camera position to determine its orientation. + + This method calculates the camera's orientation based on the selected position, + ensuring that the camera faces a focal point adjusted by a specified offset. + + Args: + camera (CameraData): An instance of `CameraData` that contains properties + used for orientation calculations. + position (Point): A 3D point representing the camera's position. + + Returns: + Pose: A tuple containing the camera's position and its orientation + (as a quaternion). + + Notes: + - The orientation is computed using roll, pitch, and yaw angles based on + the position relative to the workspace center. + """ # Determine orientation such that camera faces the center rpy = [ 0.0, @@ -587,6 +866,25 @@ class Scene: return camera.spawn_position, tuple(quat_xyzw) def reset_robot_pose(self): + """ + Reset the robot's pose to a randomized position or its default spawn position. + + If the robot's randomizer is enabled for pose randomization, this method will + generate a new random position within the specified spawn volume and assign + a random orientation. Otherwise, it will reset the robot's pose to its default + spawn position and orientation. + + The new position is calculated by adding a random offset within the spawn + volume to the robot's initial spawn position. The orientation is generated + as a random rotation around the z-axis. + + Raises: + Exception: If there are issues resetting the robot pose in the Gazebo simulation. + + Notes: + - The position is a 3D vector represented as [x, y, z]. + - The orientation is represented as a quaternion in the form (x, y, z, w). + """ if self.robot.randomizer.pose: position = [ self.robot.spawn_position[0] @@ -619,6 +917,26 @@ class Scene: self.gazebo_paused_run() def reset_robot_joint_positions(self): + """ + Reset the robot's joint positions and velocities to their initial values. + + This method retrieves the robot's initial arm joint positions and applies + them to the robot in the Gazebo simulation. If joint position randomization + is enabled, it adds Gaussian noise to the joint positions based on the + specified standard deviation. + + Additionally, the method resets the velocities of both the arm and gripper + joints to zero. After updating the joint states, it executes an unpaused step + in Gazebo to process the modifications and update the joint states. + + Raises: + RuntimeError: If there are issues resetting joint positions or velocities + in the Gazebo simulation, or if the Gazebo step fails. + + Notes: + - This method assumes the robot has a gripper only if `self.robot.with_gripper` is true. + - Joint positions for the gripper are reset only if gripper actuated joint names are available. + """ gazebo_robot = self.robot_wrapper.to_gazebo() arm_joint_positions = self.robot_wrapper.initial_arm_joint_positions @@ -671,6 +989,26 @@ class Scene: # self.gripper.open() def reset_objects_pose(self, object: ObjectData): + """ + Reset the pose of specified objects in the simulation. + + This method updates the base position and orientation of all instances of + the specified object type in the Gazebo simulation to their defined + positions and orientations. + + Args: + object (ObjectData): The object data containing the name and the desired + position and orientation for the objects to be reset. + + Raises: + KeyError: If the specified object's name is not found in the unique names + registry. + + Notes: + - The method retrieves the list of unique names for the specified object + type and resets the pose of each instance to the provided position and + orientation from the `object` data. + """ task_object_names = self.__objects_unique_names[object.name] for object_name in task_object_names: obj = self.world.to_gazebo().get_model(object_name).to_gazebo() @@ -685,6 +1023,35 @@ class Scene: min_distance_to_other_objects: float = 0.2, min_distance_decay_factor: float = 0.95, ) -> Pose: + """ + Generate a random pose for an object, ensuring it is sufficiently + distanced from other objects. + + This method computes a random position within a specified spawn volume for + the given object. It checks that the new position does not violate minimum + distance constraints from existing objects in the simulation. + + Args: + obj (ObjectData): The object data containing the base position, + randomization parameters, and relative positioning info. + name (str, optional): The name of the object being positioned. Defaults to an empty string. + Mic_distance_to_other_objects (float, optional): The minimum required distance + from other objects. Defaults to 0.2. + min_distance_decay_factor (float, optional): Factor by which the minimum distance + will decay if the new position is + too close to existing objects. Defaults to 0.95. + + Returns: + Pose: A tuple containing the randomly generated position (x, y, z) and a + randomly generated orientation (quaternion). + + Notes: + - The method uses a uniform distribution to select random coordinates + within the object's defined spawn volume. + - The quaternion is generated to provide a random orientation for the object. + - The method iteratively checks for proximity to other objects and adjusts the + minimum distance if necessary. + """ is_too_close = True # The default value for randomization object_position = obj.position @@ -740,10 +1107,21 @@ class Scene: def is_camera_pose_expired(self, camera: CameraData) -> bool: """ - Checks if camera pose needs to be randomized. + Check if the camera pose has expired and needs to be randomized. - Return: - True if expired, false otherwise + This method evaluates whether the camera's current pose has reached its + limit for randomization based on the specified rollout count. If the + camera has exceeded the allowed number of rollouts, it will reset the + counter and indicate that the pose should be randomized. + + Args: + camera (CameraData): The camera data containing the current rollout + counter and the maximum number of rollouts + allowed before a new pose is required. + + Returns: + bool: True if the camera pose needs to be randomized (i.e., it has + expired), False otherwise. """ if camera.random_pose_rollouts_num == 0: @@ -758,6 +1136,15 @@ class Scene: return False def gazebo_paused_run(self): + """ + Execute a run in Gazebo while paused. + + This method attempts to run the Gazebo simulation in a paused state. + If the operation fails, it raises a RuntimeError to indicate the failure. + + Raises: + RuntimeError: If the Gazebo run operation fails. + """ if not self.gazebo.run(paused=True): raise RuntimeError("Failed to execute a paused Gazebo run") @@ -766,7 +1153,18 @@ class Scene: object_position: Point, ) -> bool: """ - Returns true if the object is outside the workspace (parallelepiped defined by center and dimensions). + Check if an object is outside the defined workspace. + + This method evaluates whether the given object position lies outside the + specified workspace boundaries, which are defined by a parallelepiped + determined by minimum and maximum bounds. + + Args: + object_position (Point): A tuple or list representing the 3D position + of the object in the format (x, y, z). + + Returns: + bool: True if the object is outside the workspace, False otherwise. """ return ( @@ -785,11 +1183,21 @@ class Scene: terrain_allowed_penetration_depth: float = 0.002, ) -> bool: """ - Go through all objects and make sure that none of them are overlapping. - If an object is overlapping, reset its position. - Positions are reset also if object is in collision with robot right after reset. - Collisions/overlaps with terrain are ignored. - Returns True if all objects are okay, false if they had to be reset + Check for overlapping objects in the simulation and reset their positions if necessary. + + This method iterates through all objects and ensures that none of them are overlapping. + If an object is found to be overlapping with another object (or the robot), it resets + its position to a random valid pose. Collisions or overlaps with terrain are ignored. + + Args: + object (ObjectData): The object data to check for overlaps. + allowed_penetration_depth (float, optional): The maximum allowed penetration depth + between objects. Defaults to 0.001. + terrain_allowed_penetration_depth (float, optional): The maximum allowed penetration + depth when in contact with terrain. Defaults to 0.002. + + Returns: + bool: True if all objects are in valid positions, False if any had to be reset. """ # Update object positions diff --git a/env_manager/env_manager/env_manager/scene/scene_randomizer.py b/env_manager/env_manager/env_manager/scene/scene_randomizer.py deleted file mode 100644 index b74ce54..0000000 --- a/env_manager/env_manager/env_manager/scene/scene_randomizer.py +++ /dev/null @@ -1,4 +0,0 @@ - -class SceneRandomizer: - def __init__(self) -> None: - pass