Source code for skbot.ignition.sdformat.generic_sdf.world

from ..exceptions import ParseError
from typing import Dict, List, Any, Tuple
from itertools import chain
import warnings

from .base import (
    ElementBase,
    NamedPoseBearing,
    Pose,
    vector3,
    PoseBearing,
    StringElement,
    FloatElement,
    should_warn_unsupported,
)
from .model import Model
from .physics import Physics
from .light import Light
from .frame import Frame
from .scene import Scene
from .actor import Actor
from .joint import Joint
from .plugin import Plugin
from .state import State
from .include import Include
from .population import Population
from .atmosphere import Atmosphere
from .gui import Gui
from .... import transform as tf


[docs]class World(ElementBase): """A simulation world The world element is a container for all information related to a single simulation. Parameters ---------- name : str The name of the world. It must be unique among all worlds in the same :class:`Sdf` container. .. versionadded:: SDFormat v1.2 audio: World.Audio The simulations global audio properties. .. versionadded:: SDFormat v1.4 wind : World.Wind The direction and strength of the wind. Currently only constant wind is supported. .. versionadded:: SDFormat v1.6 includes : List[Include] References to other SDF files that contain fragments (:class:`Models <Model>`, :class:`Lights <Light>`, :class:`Actors <Actor>`) to include in the simulation. .. versionadded:: SDFormat v1.4 gravity : str The gravity in m/s^2, expressed in a coordinate frame defined by the spherical_coordinates attribute. The default is "0 0 -9.8". .. versionadded:: SDFormat v1.6 magnetic_field : str The magnetic field vector in Tesla, expressed in a coordinate frame defined by the spherical_coordinates attribute. The default is "5.5645e-6 22.8758e-6 -42.3884e-6". .. versionadded:: SDFormat v1.6 atmosphere : World.Atmosphere The atmosphere model to use. .. versionadded:: SDFormat v1.6 gui : World.Gui Layout of the Ignition Gazebo GUI. (Applications other than Gazebo will likely not need this.) physics_engine : Physics Configuration parameters of the dynamics engine. .. deprecated:: SDFormat v1.6 Use `physics_engines` instead. physics_engines : List[Physics] Configuration parameters of the dynamics engine. .. versionadded:: SDFormat v1.6 scene : Scene Ambience configuration of the world (skybox, ambient light, etc.) lights : List[Light] A list of light sources of the world. frames : List[Frame] A list of reference frames used to ease the positioning of objects. .. versionadded:: SDFormat v1.7 models : List[Model] A list of models used in this simulation. actors : List[Actor] A list of actors used in this simulation. plugins : List[Plugin] A list of plugins used to customize the runtime behavior of the simulation. joints : List[Joint] A list of connections between two :class:`Links <Link>` that constrains their relative movement. .. deprecated:: SDFormat v1.7 To attach models to the world, use the canonical_link kwarg instead. All other usage of `joints` has no replacement. road : List[World.Road] A list of roads used in this simulation. spherical_coordinates : World.SphericalCoordinates Simulator root frame location. .. versionadded:: SDFormat v1.4 state : List[State] The current state of this simulation. population : List["Population"] A group of models that is procedually inserted into the simulation. .. versionadded:: SDFormat v1.5 sdf_version : str The SDFormat version to use when constructing this element. Attributes ---------- name : str The name of the world. audio: World.Audio The simulations global audio properties. wind : World.Wind The direction and strength of the wind. gravity : np.ndarray The gravity in m/s^2, expressed in a coordinate frame defined by the spherical_coordinates attribute. magnetic_field : np.ndarray The magnetic field vector in Tesla, expressed in a coordinate frame defined by the spherical_coordinates attribute. atmosphere : World.Atmosphere The atmosphere model to use. gui : World.Gui Layout of the Ignition Gazebo GUI. physics_engines : List[Physics] Configuration parameters of the dynamics engine(s). scene : Scene Ambience configuration of the world (skybox, ambient light, etc.) lights : List[Light] A list of light sources of the world. frames : List[Frame] A list of reference frames used to ease the positioning of objects. models : List[Model] A list of models used in this simulation. actors : List[Actor] A list of actors used in this simulation. plugins : List[Plugin] A list of plugins used to customize the runtime behavior of the simulation. road : List[World.Road] A list of roads used in this simulation. spherical_coordinates : World.SphericalCoordinates Simulator root frame location. state : List[State] The current state of this simulation. physics_engine : Physics .. deprecated:: SDFormat v1.6 Use `physics_engines` instead. joints : List[Joint] .. deprecated:: SDFormat v1.7 Use `Model.joints` instead and set `Model.joints.parent` to `world`. Notes ----- The `includes` kwarg is resolved upon construction and the included fragmens are appended to `World.models`, `Worlds.actors`, or `Worlds.lights` respectively. The `populations` kwarg is resolved upon construction and generated models are appended to the list of `Worlds.models`. """
[docs] def __init__( self, *, name: str = "", audio: "World.Audio" = None, wind: "World.Wind" = None, includes: List[Include], gravity: str = "0 0 -9.8", magnetic_field: str = "5.5645e-6 22.8758e-6 -42.3884e-6", atmosphere: "Atmosphere" = None, gui: "Gui" = None, physics_engine: Physics = None, physics_engines: List[Physics] = None, scene: Scene, lights: List[Light], frames: List[Frame], models: List[Model], actors: List[Actor], plugins: List[Plugin], joints: List[Joint] = None, roads: List["World.Road"], spherical_coordinates: "World.SphericalCoordinates" = None, states: List[State], populations: List["World.WorldPopulation"], sdf_version: str = "1.8", ) -> None: super().__init__(sdf_version=sdf_version) self.name = name self.audio = World.Audio(sdf_version=sdf_version) if audio is None else audio self.wind = World.Wind(sdf_version=sdf_version) if wind is None else wind self.gravity = vector3(gravity) self.magnetic_field = vector3(magnetic_field) self.atmosphere = ( Atmosphere(sdf_version=sdf_version) if atmosphere is None else atmosphere ) self.gui = Gui(sdf_version=sdf_version) if gui is None else gui if sdf_version in ["1.0", "1.2", "1.3", "1.4", "1.5"]: if physics_engines is not None: raise ValueError( "`World.physics_engines` requires an SDF version newer than 1.5." ) self.physics_engines = [] if physics_engine is None else [physics_engine] else: if physics_engine is not None: raise ValueError( "`World.physics_engine` is deprecated. Use `physics_engines` instead." ) self.physics_engines = [] if physics_engines is None else physics_engines self.scene = scene self.lights = lights self.frames = frames self.models = models self.actors = actors self.plugins = plugins self.populations = populations if sdf_version in ["1.0", "1.2", "1.3", "1.4", "1.5", "1.6"]: self._joints = [] if joints is None else joints elif joints is not None: raise ValueError( "`World.joints` is deprecated." "To connect a model to the world use `Model.canonical_link` instead." ) else: self._joints = list() self.road = roads self.spherical_coordinates = ( World.SphericalCoordinates() if spherical_coordinates is None else spherical_coordinates ) self.state = states for include in includes: fragment = include.resolve() if isinstance(fragment, Model): self.models.append(fragment) elif isinstance(fragment, Actor): self.actors.append(fragment) else: self.lights.append(fragment) for frame in self.frames: if frame.attached_to is None: frame.attached_to = "world" el: PoseBearing pose_bearing: List[PoseBearing] = [ # lights, self.frames, self.models, self._joints, ] for el in chain(*pose_bearing): if el.pose.relative_to is None: el.pose.relative_to = "world"
@property def joints(self): warnings.warn( "`World.joints` has been deprecated. Use `Model.joints` with" " `Model.joints.parent = 'world'` instead.", DeprecationWarning, ) return self._joints @classmethod def from_specific(cls, specific: Any, *, version: str) -> "World": world_args = { "includes": [ Include.from_specific(x, version=version) for x in specific.include ], "scene": Scene.from_specific(specific.scene, version=version), } elements_with_default: Dict[str, ElementBase] = { "name": StringElement, "gravity": StringElement, "magnetic_field": StringElement, "audio": World.Audio, "wind": World.Wind, "atmosphere": Atmosphere, "gui": Gui, "spherical_coordinates": World.SphericalCoordinates, } list_elements: Dict[str, Tuple[str, ElementBase]] = { "include": ("includes", Include), "light": ("lights", Light), "frame": ("frames", Frame), "model": ("models", Model), "actor": ("actors", Actor), "plugin": ("plugins", Plugin), "road": ("roads", World.Road), "state": ("states", State), "population": ("populations", World.WorldPopulation), } args = cls._prepare_standard_args( specific, elements_with_default, list_elements, version=version ) world_args.update(args) if version in ["1.0", "1.1", "1.2", "1.3", "1.4", "1.5"]: world_args["physics_engine"] = Physics.from_specific( specific.physics, version=version ) else: world_args["physics_engines"] = [ Physics.from_specific(x, version=version) for x in specific.physics ] if version in ["1.0", "1.1", "1.2", "1.3", "1.4", "1.5", "1.6"]: world_args["joints"] = [ Joint.from_specific(x, version=version) for x in specific.joint ] return World(**world_args, sdf_version=version) def declared_frames(self) -> Dict[str, tf.Frame]: world_frame = tf.Frame(3, name=self.name) declared_frames = {"world": world_frame} for el in self.models: model_frames = el.declared_frames() declared_frames[el.name] = model_frames["__model__"] for name, frame in model_frames.items(): declared_frames[f"{el.name}::{name}"] = frame for el in chain(self.frames, self._joints): declared_frames.update(el.declared_frames()) return declared_frames def to_static_graph( self, declared_frames: Dict[str, tf.Frame], *, seed: int = None, shape: Tuple[int] = ..., axis: int = -1, ) -> tf.Frame: for model in self.models: scope = { name.split("::", 1)[1]: frame for name, frame in declared_frames.items() if name.startswith(f"{model.name}::") } scope["world"] = declared_frames["world"] model.to_static_graph(scope, seed=seed, shape=shape, axis=axis) for model in self.models: link = model.pose.to_tf_link() parent_name = model.pose.relative_to child_name = f"{model.name}::{model.placement_frame}" parent = declared_frames[parent_name] child = declared_frames[child_name] link(child, parent) for el in chain(self.frames, self._joints): link: tf.Link = el.pose.to_tf_link() parent_name = el.pose.relative_to child_name = el.name parent = declared_frames[parent_name] child = declared_frames[child_name] link(child, parent) el.to_static_graph(declared_frames, seed=seed, shape=shape, axis=axis) return declared_frames["world"] def to_dynamic_graph( self, declared_frames: Dict[str, tf.Frame], *, seed: int = None, shape: Tuple[int] = (3,), axis: int = -1, apply_state: bool = True, _scaffolding: Dict[str, tf.Frame] = None, ) -> tf.Frame: if _scaffolding is None: _scaffolding = self.declared_frames() self.to_static_graph(_scaffolding, seed=seed, shape=shape, axis=axis) for model in self.models: scaffold_scope = { name.split("::", 1)[1]: frame for name, frame in _scaffolding.items() if name.startswith(f"{model.name}::") } scaffold_scope["world"] = _scaffolding["world"] scope = { name.split("::", 1)[1]: frame for name, frame in declared_frames.items() if name.startswith(f"{model.name}::") } scope["world"] = declared_frames["world"] model.to_dynamic_graph( scope, seed=seed, shape=shape, axis=axis, apply_state=apply_state, _scaffolding=scaffold_scope, ) for el in chain(self.frames, self._joints): el.to_dynamic_graph( declared_frames, seed=seed, shape=shape, axis=axis, apply_state=apply_state, _scaffolding=_scaffolding, ) return declared_frames["world"] class Audio(ElementBase): """Global audio properties. Parameters ---------- uri : str Device to use for audio playback. If None it will be set to "default". Attributes ---------- uri : str The audio device to use. """ def __init__(self, *, device: str = "default", sdf_version: str) -> None: super().__init__(sdf_version=sdf_version) self.uri = device @classmethod def from_specific(cls, specific: Any, *, version: str) -> "ElementBase": if specific is None: return World.Audio(sdf_version=version) return World.Audio(device=specific.device, sdf_version=version) class Wind(ElementBase): """The type and properties of the wind in the world. Parameters ---------- linear_velocity : str The direction and strength of the wind. Default is "0 0 0", meaning no wind. Attributes ---------- linear_velocity : np.ndarray The direction and strength of the wind's velocity. """ def __init__(self, *, linear_velocity: str = "0 0 0", sdf_version: str) -> None: self.linear_velocity = vector3(linear_velocity) @classmethod def from_specific(cls, specific: Any, *, version: str) -> "ElementBase": if specific is None: return World.Wind(sdf_version=version) return World.Wind( linear_velocity=specific.linear_velocity, sdf_version=version ) class Road(ElementBase): def __init__(self, *, sdf_version: str) -> None: if should_warn_unsupported(): warnings.warn("`Road` has not been implemented yet.") super().__init__(sdf_version=sdf_version) class SphericalCoordinates(ElementBase): """Simulator root frame location. SphericalCoordnates are used to specify the location of the simulation's root frame on Earth using a chosen surface model. Parameters ---------- surface_model : str Name of planetary surface model, used to determine the surface altitude at a given latitude and longitude. The default is an ellipsoid model of the earth based on the WGS-84 standard. It is used in Gazebo's GPS sensor implementation. world_frame_orientation : str This field identifies how Gazebo world frame is aligned in Geographical sense. The final Gazebo world frame orientation is obtained by rotating a frame aligned with following notation by the field heading_deg (Note that heading_deg corresponds to positive yaw rotation in the NED frame, so it's inverse specifies positive Z-rotation in ENU or NWU). Options are: - ENU (East-North-Up; default) - NED (North-East-Down) - NWU (North-West-Up) For example, world frame specified by setting world_orientation="ENU" and heading_deg=-90° is effectively equivalent to NWU with heading of 0°. latitude_deg : float Geodetic latitude at origin of gazebo reference frame, specified in units of degrees. The default is 0.0. longitude_deg : float Longitude at origin of gazebo reference frame, specified in units of degrees. The default is 0.0. elevation : float Elevation of origin of gazebo reference frame, specified in meters. The default is 0.0. heading_deg : float Heading offset of gazebo reference frame, measured as angle between East and gazebo x axis, or equivalently, the angle between North and gazebo y axis. The angle is specified in degrees. The default is 0.0. Attributes ---------- surface_model : str The name of planetary surface model. world_frame_orientation : str Reference orientation for frame alignment. latitude_deg : float Geodetic latitude at origin of the root frame. longitude_deg : float Longitude at origin of the root frame. elevation : float Elevation of origin of the root frame. heading_deg : float Angle (in degrees) between the root's y-axis and magnetic north. """ def __init__( self, *, surface_model: str = "EARTH_WGS84", world_frame_orientation: str = "ENU", latitude_deg: float = 0.0, longitude_deg: float = 0.0, elevation: float = 0.0, heading_deg: float = 0.0, sdf_version: str, ) -> None: super().__init__(sdf_version=sdf_version) self.surface_model = surface_model self.world_frame_orientation = world_frame_orientation self.latitude_deg = latitude_deg self.longitude_deg = longitude_deg self.elevation = elevation self.heading_deg = heading_deg @classmethod def from_specific(cls, specific: Any, *, version: str) -> "World": elements_with_default: Dict[str, ElementBase] = { "surface_model": StringElement, "world_frame_orientation": StringElement, "latitude_deg": FloatElement, "longitude_deg": FloatElement, "elevation": FloatElement, "heading_deg": FloatElement, } generic_args = cls._prepare_standard_args( specific, elements_with_default, version=version ) return cls(**generic_args, sdf_version=version) class WorldPopulation(Population): # def __init__( # self, # *, # name: str, # pose: Pose = None, # model_count: int = 1, # distribution: "Distribution", # box: "Box" = None, # cylinder: "Cylinder" = None, # model: Model, # frames: List[Frame] = None, # ) -> None: # super().__init__(name=name, pose=pose) # self.name=name # self.model_count = model_count # self.distribution = distribution # self.box = box # self.cylinder = cylinder # self.model = model # self.frames = frames # if frames is None: # self.frames = list() class Distribution(ElementBase): def __init__( self, *, kind: str = "random", rows: int = 1, cols: int = 1, step: Tuple[float] = (0.5, 0.5, 0), ): self.type = kind self.rows = rows self.cols = cols self.step = step class Box(ElementBase): def __init__(self, *, size: Tuple[float] = (1, 1, 1)) -> None: self.size = size class Cylinder(ElementBase): def __init__(self, *, radius: float = 1, length: float = 1) -> None: self.radius = radius self.length = length