robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

docs(env_manager): Update docstrings and refactor code

Browse source 
- Added detailed docstrings to the env_manager package for improved documentation and clarity.
- Refactored code in multiple files to enhance readability and maintainability.
This commit is contained in:
Ilya Uraev 2024-09-30 23:30:04 +03:00
parent 860f7d6566
commit 31d2afe6e2
25 changed files with 2023 additions and 811 deletions
Download patch file Download diff file Expand all files Collapse all files

67
env_manager/env_manager/env_manager/models/configs/camera.py
View file

@ -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")

26
env_manager/env_manager/env_manager/models/configs/light.py
View file

@ -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))

118
env_manager/env_manager/env_manager/models/configs/objects.py
View file

@ -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)

44
env_manager/env_manager/env_manager/models/configs/robot.py
View file

@ -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))

40
env_manager/env_manager/env_manager/models/configs/scene.py
View file

@ -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))

19
env_manager/env_manager/env_manager/models/configs/terrain.py
View file

@ -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))

128
env_manager/env_manager/env_manager/models/lights/random_sun.py
View file

@ -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])

75
env_manager/env_manager/env_manager/models/lights/sun.py
View file

@ -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'''<sdf version="1.9">
<model name="{model_name}">
<static>true</static>

35
env_manager/env_manager/env_manager/models/objects/model.py
View file

@ -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)

46
env_manager/env_manager/env_manager/models/objects/primitives/box.py
View file

@ -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'''<sdf version="1.7">
<model name="{model_name}">
<static>{"true" if static else "false"}</static>

49
env_manager/env_manager/env_manager/models/objects/primitives/cylinder.py
View file

@ -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'''<sdf version="1.7">

25
env_manager/env_manager/env_manager/models/objects/primitives/plane.py
View file

@ -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'''<sdf version="1.7">
<model name="{model_name}">
<static>true</static>
@ -73,15 +89,12 @@ class Plane(model_wrapper.ModelWrapper):
</model>
</sdf>'''
# Вставка модели
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)

43
env_manager/env_manager/env_manager/models/objects/primitives/sphere.py
View file

@ -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

44
env_manager/env_manager/env_manager/models/objects/random_object.py
View file

@ -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,

72
env_manager/env_manager/env_manager/models/objects/random_primitive.py
View file

@ -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,

15
env_manager/env_manager/env_manager/models/robots/__init__.py
View file

@ -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)

348
env_manager/env_manager/env_manager/models/robots/panda.py
View file

@ -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)

322
env_manager/env_manager/env_manager/models/robots/rbs_arm.py
View file

@ -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")

106
env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py
View file

@ -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")

52
env_manager/env_manager/env_manager/models/robots/robot.py
View file

@ -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

101
env_manager/env_manager/env_manager/models/sensors/camera.py
View file

@ -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)

20
env_manager/env_manager/env_manager/models/terrains/ground.py
View file

@ -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,

559
env_manager/env_manager/env_manager/models/utils/model_collection_randomizer.py
View file

@ -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

476
env_manager/env_manager/env_manager/scene/scene.py
View file

@ -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

4
env_manager/env_manager/env_manager/scene/scene_randomizer.py
View file

@ -1,4 +0,0 @@
class SceneRandomizer:
def __init__(self) -> None:
pass