From 7b0b014865d8ebd7914c2ba5190c3787e2851248 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Apr 2020 13:11:59 +0200 Subject: [PATCH 01/35] Revert "Disable gym_ignition package" This reverts commit 8e23abc724268a10a8bce89268c156420949ee64. --- gym_ignition/__init__.py | 197 +++++++++++++++++++-------------------- 1 file changed, 97 insertions(+), 100 deletions(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 17e07d794..bb63a647a 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -2,133 +2,130 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import scenario_bindings - - # Import SWIG bindings # See https://github.com/robotology/gym-ignition/issues/7 # https://stackoverflow.com/a/45473441/12150968 -# import sys -# if sys.platform.startswith('linux') or sys.platform.startswith('darwin'): -# import os -# dlopen_flags = sys.getdlopenflags() -# if "gympp_bindings" not in sys.modules: -# sys.setdlopenflags(dlopen_flags | os.RTLD_GLOBAL) -# else: -# sys.setdlopenflags(dlopen_flags | os.RTLD_LAZY | os.RTLD_NOLOAD | os.RTLD_GLOBAL) -# -# import gympp_bindings -# -# # Restore the flags -# sys.setdlopenflags(dlopen_flags) -# else: -# import gympp_bindings +import sys +if sys.platform.startswith('linux') or sys.platform.startswith('darwin'): + import os + dlopen_flags = sys.getdlopenflags() + if "gympp_bindings" not in sys.modules: + sys.setdlopenflags(dlopen_flags | os.RTLD_GLOBAL) + else: + sys.setdlopenflags(dlopen_flags | os.RTLD_LAZY | os.RTLD_NOLOAD | os.RTLD_GLOBAL) + + import gympp_bindings + + # Restore the flags + sys.setdlopenflags(dlopen_flags) +else: + import gympp_bindings # Configure OS environment variables -# from gym_ignition.utils import gazebo_env_vars, resource_finder -# gazebo_env_vars.setup_gazebo_env_vars() -# resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") +from gym_ignition.utils import gazebo_env_vars, resource_finder +gazebo_env_vars.setup_gazebo_env_vars() +resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") # ========================= # REGISTER THE ENVIRONMENTS # ========================= -# from gym.envs.registration import register -# from gym_ignition.utils import resource_finder +from gym.envs.registration import register +from gym_ignition.utils import resource_finder # Import the robots -# from gym_ignition.robots import rt -# from gym_ignition.robots import sim +from gym_ignition.robots import rt +from gym_ignition.robots import sim # Import the tasks -# from gym_ignition.tasks import pendulum_swingup -# from gym_ignition.tasks import cartpole_discrete -# from gym_ignition.tasks import cartpole_continuous +from gym_ignition.tasks import pendulum_swingup +from gym_ignition.tasks import cartpole_discrete +from gym_ignition.tasks import cartpole_continuous # ====================== # GYMPP C++ ENVIRONMENTS # ====================== -# import numpy -# max_float = float(numpy.finfo(numpy.float32).max) -# -# register( -# id='CartPoleDiscrete-Gympp-v0', -# max_episode_steps=5000, -# entry_point='gym_ignition.gympp.cartpole:CartPoleDiscrete') +import numpy +max_float = float(numpy.finfo(numpy.float32).max) + +register( + id='CartPoleDiscrete-Gympp-v0', + max_episode_steps=5000, + entry_point='gym_ignition.gympp.cartpole:CartPoleDiscrete') # ============================ # IGNITION GAZEBO ENVIRONMENTS # ============================ -# register( -# id='Pendulum-Gazebo-v0', -# entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', -# max_episode_steps=5000, -# kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, -# 'robot_cls': sim.gazebo.pendulum.PendulumGazeboRobot, -# 'model': "Pendulum/Pendulum.urdf", -# 'world': "DefaultEmptyWorld.world", -# 'rtf': max_float, -# 'agent_rate': 1000, -# 'physics_rate': 1000, -# }) -# -# register( -# id='CartPoleDiscrete-Gazebo-v0', -# entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', -# max_episode_steps=5000, -# kwargs={'task_cls': cartpole_discrete.CartPoleDiscrete, -# 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, -# 'model': "CartPole/CartPole.urdf", -# 'world': "DefaultEmptyWorld.world", -# 'rtf': max_float, -# 'agent_rate': 1000, -# 'physics_rate': 1000, -# }) -# -# register( -# id='CartPoleContinuous-Gazebo-v0', -# entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', -# max_episode_steps=5000, -# kwargs={'task_cls': cartpole_continuous.CartPoleContinuous, -# 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, -# 'model': "CartPole/CartPole.urdf", -# 'world': "DefaultEmptyWorld.world", -# 'rtf': max_float, -# 'agent_rate': 1000, -# 'physics_rate': 1000, -# }) +register( + id='Pendulum-Gazebo-v0', + entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', + max_episode_steps=5000, + kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, + 'robot_cls': sim.gazebo.pendulum.PendulumGazeboRobot, + 'model': "Pendulum/Pendulum.urdf", + 'world': "DefaultEmptyWorld.world", + 'rtf': max_float, + 'agent_rate': 1000, + 'physics_rate': 1000, + }) + +register( + id='CartPoleDiscrete-Gazebo-v0', + entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', + max_episode_steps=5000, + kwargs={'task_cls': cartpole_discrete.CartPoleDiscrete, + 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, + 'model': "CartPole/CartPole.urdf", + 'world': "DefaultEmptyWorld.world", + 'rtf': max_float, + 'agent_rate': 1000, + 'physics_rate': 1000, + }) + +register( + id='CartPoleContinuous-Gazebo-v0', + entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', + max_episode_steps=5000, + kwargs={'task_cls': cartpole_continuous.CartPoleContinuous, + 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, + 'model': "CartPole/CartPole.urdf", + 'world': "DefaultEmptyWorld.world", + 'rtf': max_float, + 'agent_rate': 1000, + 'physics_rate': 1000, + }) # ===================== # PYBULLET ENVIRONMENTS # ===================== -# register( -# id='Pendulum-PyBullet-v0', -# entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', -# max_episode_steps=5000, -# kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, -# 'robot_cls': sim.pybullet.pendulum.PendulumPyBulletRobot, -# 'model': "Pendulum/Pendulum.urdf", -# 'world': "plane_implicit.urdf", -# 'rtf': max_float, -# 'agent_rate': 1000, -# 'physics_rate': 1000, -# }) -# -# register( -# id='CartPoleDiscrete-PyBullet-v0', -# entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', -# max_episode_steps=5000, -# kwargs={ -# # PyBulletRuntime -# 'task_cls': cartpole_discrete.CartPoleDiscrete, -# 'robot_cls': sim.pybullet.cartpole.CartPolePyBulletRobot, -# 'model': "CartPole/CartPole.urdf", -# 'world': "plane_implicit.urdf", -# 'rtf': max_float, -# 'agent_rate': 1000, -# 'physics_rate': 1000, -# }) +register( + id='Pendulum-PyBullet-v0', + entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', + max_episode_steps=5000, + kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, + 'robot_cls': sim.pybullet.pendulum.PendulumPyBulletRobot, + 'model': "Pendulum/Pendulum.urdf", + 'world': "plane_implicit.urdf", + 'rtf': max_float, + 'agent_rate': 1000, + 'physics_rate': 1000, + }) + +register( + id='CartPoleDiscrete-PyBullet-v0', + entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', + max_episode_steps=5000, + kwargs={ + # PyBulletRuntime + 'task_cls': cartpole_discrete.CartPoleDiscrete, + 'robot_cls': sim.pybullet.cartpole.CartPolePyBulletRobot, + 'model': "CartPole/CartPole.urdf", + 'world': "plane_implicit.urdf", + 'rtf': max_float, + 'agent_rate': 1000, + 'physics_rate': 1000, + }) From aaf3395a84a299aaf7a15014c957d3bc35b9820c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 12 Mar 2020 15:12:37 +0100 Subject: [PATCH 02/35] Update Python files with new GazeboSimulator class --- gym_ignition/base/gympp_env.py | 2 +- gym_ignition/robots/base/gazebo_robot.py | 4 ++-- gym_ignition/runtimes/gazebo_runtime.py | 6 +++--- gym_ignition/utils/logger.py | 8 ++++---- tests/python/test_bindings.py | 2 +- tests/python/test_robot_controller.py | 4 ++-- tests/python/utils.py | 2 +- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/gym_ignition/base/gympp_env.py b/gym_ignition/base/gympp_env.py index a8beb18af..2fe1e2f9a 100644 --- a/gym_ignition/base/gympp_env.py +++ b/gym_ignition/base/gympp_env.py @@ -69,7 +69,7 @@ def gympp_env(self) -> bindings.GazeboEnvironment: return self._env @property - def gazebo(self) -> bindings.GazeboWrapper: + def gazebo(self) -> bindings.GazeboSimulator: return bindings.envToGazeboWrapper(self.gympp_env) @property diff --git a/gym_ignition/robots/base/gazebo_robot.py b/gym_ignition/robots/base/gazebo_robot.py index d60d655a4..3c26cce77 100644 --- a/gym_ignition/robots/base/gazebo_robot.py +++ b/gym_ignition/robots/base/gazebo_robot.py @@ -21,7 +21,7 @@ class GazeboRobot(robot_abc.RobotABC, def __init__(self, model_file: str, - gazebo: bindings.GazeboWrapper, + gazebo: bindings.GazeboSimulator, controller_rate: float = None) -> None: # Find the model file @@ -94,7 +94,7 @@ def gympp_robot(self) -> bindings.Robot: sdf_string = stream.read() # Get the model name - original_name = bindings.GazeboWrapper.getModelNameFromSDF(sdf_string) + original_name = bindings.GazeboSimulator.getModelNameFromSDF(sdf_string) assert original_name, f"Failed to get model name from file {self.model_file}" # Create a unique robot name diff --git a/gym_ignition/runtimes/gazebo_runtime.py b/gym_ignition/runtimes/gazebo_runtime.py index e57fba537..3d00b04bd 100644 --- a/gym_ignition/runtimes/gazebo_runtime.py +++ b/gym_ignition/runtimes/gazebo_runtime.py @@ -67,7 +67,7 @@ def __init__(self, # ========== @property - def gazebo(self) -> bindings.GazeboWrapper: + def gazebo(self) -> bindings.GazeboSimulator: if self._gazebo_wrapper: assert self._gazebo_wrapper.getPhysicsData().rtf == self._rtf, \ "The RTF of gazebo does not match the configuration" @@ -101,8 +101,8 @@ def gazebo(self) -> bindings.GazeboWrapper: logger.debug("Setting {} iteration per simulator step" .format(int(num_of_iterations_per_gazebo_step))) - # Create the GazeboWrapper object - self._gazebo_wrapper = bindings.GazeboWrapper( + # Create the GazeboSimulator object + self._gazebo_wrapper = bindings.GazeboSimulator( int(num_of_iterations_per_gazebo_step), self._rtf, self._physics_rate) diff --git a/gym_ignition/utils/logger.py b/gym_ignition/utils/logger.py index ddbf99971..d46df4bb2 100644 --- a/gym_ignition/utils/logger.py +++ b/gym_ignition/utils/logger.py @@ -41,12 +41,12 @@ def set_level(level: int) -> None: # Set the gympp verbosity if logger.MIN_LEVEL <= logger.DEBUG: - bindings.GazeboWrapper.setVerbosity(4) + bindings.GazeboSimulator.setVerbosity(4) elif logger.MIN_LEVEL <= logger.INFO: - bindings.GazeboWrapper.setVerbosity(3) + bindings.GazeboSimulator.setVerbosity(3) elif logger.MIN_LEVEL <= logger.WARN: - bindings.GazeboWrapper.setVerbosity(2) + bindings.GazeboSimulator.setVerbosity(2) elif logger.MIN_LEVEL <= logger.ERROR: - bindings.GazeboWrapper.setVerbosity(1) + bindings.GazeboSimulator.setVerbosity(1) else: raise Exception("Verbosity level not recognized") diff --git a/tests/python/test_bindings.py b/tests/python/test_bindings.py index 11dd7da79..b175fcc05 100644 --- a/tests/python/test_bindings.py +++ b/tests/python/test_bindings.py @@ -221,7 +221,7 @@ def test_gymfactory(): assert ign_env, "Failed to get the ignition environment" # Set verbosity - bindings.GazeboWrapper.setVerbosity(4) + bindings.GazeboSimulator.setVerbosity(4) # Use the environment env.reset() diff --git a/tests/python/test_robot_controller.py b/tests/python/test_robot_controller.py index 158c02c7d..3e798edbf 100644 --- a/tests/python/test_robot_controller.py +++ b/tests/python/test_robot_controller.py @@ -31,7 +31,7 @@ def test_joint_controller(): model_data.sdfString = model_sdf_string # Get the model name - model_name = bindings.GazeboWrapper.getModelNameFromSDF(model_sdf_string) + model_name = bindings.GazeboSimulator.getModelNameFromSDF(model_sdf_string) robot_name = model_name # ============= @@ -41,7 +41,7 @@ def test_joint_controller(): # Create the gazebo wrapper num_of_iterations = int(physics_rate / agent_rate) desired_rtf = float(np.finfo(np.float32).max) - gazebo = bindings.GazeboWrapper(num_of_iterations, desired_rtf, physics_rate) + gazebo = bindings.GazeboSimulator(num_of_iterations, desired_rtf, physics_rate) assert gazebo, "Failed to get the gazebo wrapper" # Set the verbosity diff --git a/tests/python/utils.py b/tests/python/utils.py index f34dcc950..6b2e3325b 100644 --- a/tests/python/utils.py +++ b/tests/python/utils.py @@ -34,7 +34,7 @@ def __init__(self, physics_rate: float, iterations: int = int(1), rtf=float(np.finfo(np.float32).max)): - self.simulator = bindings.GazeboWrapper(iterations, rtf, physics_rate) + self.simulator = bindings.GazeboSimulator(iterations, rtf, physics_rate) assert self.simulator self.simulator.setVerbosity(4) From ceccd7f46e745c02c6cb08fbe380a434ae255519 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:21:02 +0200 Subject: [PATCH 03/35] Remove robot interfaces from Python --- gym_ignition/base/__init__.py | 1 - gym_ignition/base/robot/__init__.py | 13 - gym_ignition/base/robot/robot_abc.py | 21 - gym_ignition/base/robot/robot_baseframe.py | 142 ------- gym_ignition/base/robot/robot_contacts.py | 57 --- gym_ignition/base/robot/robot_features.py | 61 --- gym_ignition/base/robot/robot_initialstate.py | 127 ------ gym_ignition/base/robot/robot_joints.py | 365 ------------------ gym_ignition/base/robot/robot_links.py | 111 ------ 9 files changed, 898 deletions(-) delete mode 100644 gym_ignition/base/robot/__init__.py delete mode 100644 gym_ignition/base/robot/robot_abc.py delete mode 100644 gym_ignition/base/robot/robot_baseframe.py delete mode 100644 gym_ignition/base/robot/robot_contacts.py delete mode 100644 gym_ignition/base/robot/robot_features.py delete mode 100644 gym_ignition/base/robot/robot_initialstate.py delete mode 100644 gym_ignition/base/robot/robot_joints.py delete mode 100644 gym_ignition/base/robot/robot_links.py diff --git a/gym_ignition/base/__init__.py b/gym_ignition/base/__init__.py index c4ebc2049..a1842f008 100644 --- a/gym_ignition/base/__init__.py +++ b/gym_ignition/base/__init__.py @@ -4,7 +4,6 @@ # Abstract classes from . import task -from . import robot from . import runtime # Base C++ environment diff --git a/gym_ignition/base/robot/__init__.py b/gym_ignition/base/robot/__init__.py deleted file mode 100644 index b0f368ac4..000000000 --- a/gym_ignition/base/robot/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -from . import robot_abc -from . import robot_links -from . import robot_joints -from . import robot_contacts -from . import robot_baseframe -from . import robot_initialstate - -from .robot_joints import PID -from .robot_features import feature_detector, RobotFeatures diff --git a/gym_ignition/base/robot/robot_abc.py b/gym_ignition/base/robot/robot_abc.py deleted file mode 100644 index e7cff4851..000000000 --- a/gym_ignition/base/robot/robot_abc.py +++ /dev/null @@ -1,21 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import abc - - -class RobotABC(abc.ABC): - """ - The base interface of all robot objects - """ - - def __init__(self, model_file: str = None) -> None: - # Optional model file associated with this object - self.model_file = model_file - - @abc.abstractmethod - def name(self) -> str: ... - - @abc.abstractmethod - def valid(self) -> bool: ... diff --git a/gym_ignition/base/robot/robot_baseframe.py b/gym_ignition/base/robot/robot_baseframe.py deleted file mode 100644 index 7fdbfba4a..000000000 --- a/gym_ignition/base/robot/robot_baseframe.py +++ /dev/null @@ -1,142 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -from typing import Tuple -from abc import ABC, abstractmethod - - -class RobotBaseFrame(ABC): - """ - Robot base frame interface. - - This interface provides methods to get and set quantities related to the robot base. - """ - - def __init__(self) -> None: - self._is_floating_base = True - - @abstractmethod - def set_as_floating_base(self, floating: bool) -> bool: - """ - Specify if the robot is floating or fixed base. - - Args: - floating: True for floating base robots, False for fixed based robots. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def is_floating_base(self) -> bool: - """ - Check the base type of the robot. - - Returns: - True if the robot is floating base, False is the robot is fixed base. - """ - - @abstractmethod - def set_base_frame(self, frame_name: str) -> bool: - """ - Set the base frame of the robot. If this method is not called, the detection of - the base frame depends on the implementation. - - Args: - frame_name: The name of the base frame. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def base_frame(self) -> str: - """ - Get the name of the base frame. - - Returns: - The name of the base frame. - """ - - @abstractmethod - def base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the pose of the robot base. - - The pose is composed of a position and a quaternion associated to the base frame, - both expressed in the the world inertial frame. - - Returns: - A Tuple containing the position and orientation of the base: - - - position: 3D array in the [x, y, z] form. - - orientation: a 4D array containing a quaternion in the [w, x, y, z] form. - """ - - @abstractmethod - def base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the velocity of the robot base. - - The velocity is composed of linear and angular velocities of the base wrt the - world inertial frame, both expressed in the world inertial frame. - - Returns: - A Tuple containing the linear and angular velocity of the base: - - - linear velocity: a 3D array in the [vx, vy, vz] form. - - angular velocity: a 3D array in the [wx, wy, wz] form. - """ - - @abstractmethod - def reset_base_pose(self, - position: np.ndarray, - orientation: np.ndarray) -> bool: - """ - Reset the pose of the robot base. - - The pose is composed of a position and a quaternion associated to the base frame, - both expressed in the the world inertial frame. - - This method should be used only on simulated robots, and its effects bypass - the physics. - - Args: - position: 3D array in the [x, y, z] form - orientation: a 4D array containing a quaternion in the [w, x, y, z] form. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def reset_base_velocity(self, - linear_velocity: np.ndarray, - angular_velocity: np.ndarray) -> bool: - """ - Reset the velocity of the robot base. - - The velocity is composed of linear and angular velocities of the base wrt the - world inertial frame, both expressed in the world inertial frame. - - This method should be used only on simulated robots, and its effects bypass - the physics. - - Args: - linear_velocity: a 3D array in the [vx, vy, vz] form. - angular_velocity: a 3D array in the [wx, wy, wz] form. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def base_wrench(self) -> np.ndarray: - """ - Get the wrench applied on the robot base (only for fixed-based setup) - - Returns: - The 6D array of the base wrench expressed in the base frame. - """ diff --git a/gym_ignition/base/robot/robot_contacts.py b/gym_ignition/base/robot/robot_contacts.py deleted file mode 100644 index 5d303adac..000000000 --- a/gym_ignition/base/robot/robot_contacts.py +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -from typing import List, NamedTuple -from abc import ABC, abstractmethod - - -class ContactData(NamedTuple): - bodyA: str - bodyB: str - position: np.ndarray - depth: np.ndarray = None - normal: np.ndarray = None - wrench: np.ndarray = None - - -class RobotContacts(ABC): - """ - Robot contacts interface. - - This interface provides methods to get and set contact-related quantities. - """ - - @abstractmethod - def links_in_contact(self) -> List[str]: - """ - Return a list of link names with active contacts. - - Returns: - The names of the link in contact. - """ - - @abstractmethod - def contact_data(self, contact_link_name: str) -> List[ContactData]: - """ - Return contact data of a link. It might contain multiple contacts. - - Args: - contact_link_name: The name of the link. - - Returns: - A list of NameTuple containing the contact data. - """ - - @abstractmethod - def total_contact_wrench_on_link(self, contact_link_name: str) -> np.ndarray: - """ - Return the total wrench applied to the link expressed in the link frame. - - Args: - contact_link_name: The name of the link. - - Returns: - The wrench applied to the link as a 6D array. - """ diff --git a/gym_ignition/base/robot/robot_features.py b/gym_ignition/base/robot/robot_features.py deleted file mode 100644 index a1d31c5c8..000000000 --- a/gym_ignition/base/robot/robot_features.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import abc -from typing import Type -from gym_ignition.base import robot - - -class RobotFeatures(robot.robot_abc.RobotABC, - robot.robot_baseframe.RobotBaseFrame, - robot.robot_joints.RobotJoints, - robot.robot_links.RobotLinks, - robot.robot_contacts.RobotContacts, - robot.robot_initialstate.RobotInitialState, - abc.ABC): - pass - - -def feature_detector(cls_to_patch): - """ - Feature class decorator for feature detection. - - Usage: - - ``` - @feature_detector - class RobotFeatures(robot.robot_base.RobotBase, robot.robot_joints.RobotJoints): - pass - - class MyRobot(robot.robot_base.RobotBase) - [...] - - my_robot = MyRobot() - - if not RobotFeatures.has_all_feature(my_robot): - raise Exception - ``` - - Args: - cls_to_patch: The feature class - - Returns: - The feature class patched with an additional `has_all_features` method. - """ - # Define the function that will become a class method - def has_all_features(cls: Type, obj: object) -> None: - import inspect - # Get the list of the wanted features to check. - # Remove the first entry since it is the 'cls' class. - wanted_features = inspect.getmro(cls)[1:] - - # Check that all the features are present - for feature in wanted_features: - if not isinstance(obj, feature): - raise Exception("Missing feature: '{}'".format(feature)) - - if not hasattr(cls_to_patch, 'has_all_features'): - setattr(cls_to_patch, 'has_all_features', classmethod(has_all_features)) - - return cls_to_patch diff --git a/gym_ignition/base/robot/robot_initialstate.py b/gym_ignition/base/robot/robot_initialstate.py deleted file mode 100644 index 6acafb34a..000000000 --- a/gym_ignition/base/robot/robot_initialstate.py +++ /dev/null @@ -1,127 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -from typing import Tuple -from abc import ABC, abstractmethod - - -class RobotInitialState(ABC): - """ - Robot initial state interface. - - This interface provides methods to get and set the initial state of the joints and - the base. - """ - - def __init__(self) -> None: - self._initial_joint_positions = None - self._initial_joint_velocities = None - self._initial_base_pose = (np.array([0., 0., 0.]), np.array([1., 0., 0., 0.])) - self._initial_base_velocity = (np.array([0., 0., 0.]), np.array([0., 0., 0.])) - - @abstractmethod - def initial_joint_positions(self) -> np.ndarray: - """ - Return the initial joint positions. - - Returns: - The initial joint positions. - """ - - @abstractmethod - def set_initial_joint_positions(self, positions: np.ndarray) -> bool: - """ - Set the initial joint positions. - - Note that this does not actuate the robot, it only stores the initial joint - positions values. - - Args: - positions: The initial joint positions. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def initial_joint_velocities(self) -> np.ndarray: - """ - Return the initial joint velocities. - - Returns: - The initial joint velocities. - """ - - @abstractmethod - def set_initial_joint_velocities(self, velocities: np.ndarray) -> bool: - """ - Set the initial joint velocities. - - Note that this does not actuate the robot, it only stores the initial joint - velocities values. - - Args: - velocities: The initial joint velocities. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def initial_base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the initial base pose. - - Returns: - A tuple containing the the initial position and orientation of the base: - - - position: 3D array in the [x, y, z] form. - - orientation: a 4D array containing a quaternion in the [w, x, y, z] form. - """ - - @abstractmethod - def set_initial_base_pose(self, position: np.ndarray, - orientation: np.ndarray) -> bool: - """ - Set the initial base pose. - - Note that this does not actuate the robot, it only stores the initial base - pose values. - - Args: - position: The initial base position expressed as a 3D array. - orientation: The initial base orientation expressed as a wxyz quaternion. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def initial_base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the initial base velocity. - - Returns: - A tuple containing the the linear and angular initial velocity of the base: - - - linear: 3D array in the [vx, vy, vz] form. - - angular: a 3D array in the [wx, wy, wz] form. - """ - - @abstractmethod - def set_initial_base_velocity(self, linear: np.ndarray, angular: np.ndarray) -> bool: - """ - Set the initial base velocity. - - Note that this does not actuate the robot, it only stores the initial base - pose values. - - Args: - linear: The linear initial velocity expressed as a 3D array. - angular: The angular initial velocity expressed as a 3D array. - - Returns: - True if successful, False otherwise. - """ diff --git a/gym_ignition/base/robot/robot_joints.py b/gym_ignition/base/robot/robot_joints.py deleted file mode 100644 index ebf42d1cb..000000000 --- a/gym_ignition/base/robot/robot_joints.py +++ /dev/null @@ -1,365 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -from enum import Enum, auto -from abc import ABC, abstractmethod -from typing import List, Tuple, Union - - -class JointType(Enum): - INVALID = auto() - FIXED = auto() - REVOLUTE = auto() - PRISMATIC = auto() - -class JointControlMode(Enum): - POSITION = auto() - POSITION_INTERPOLATED = auto() - VELOCITY = auto() - TORQUE = auto() - - -class PID: - def __init__(self, p: float, i: float, d: float) -> None: - self.p = p - self.i = i - self.d = d - - -class RobotJoints(ABC): - """ - Robot joints interface. - - This interface provides methods to get and set joint-related quantities. - """ - - @abstractmethod - def dofs(self) -> int: - """ - Return the number of degrees of freedom of the robot. - - Returns: - The number of degrees of freedom. - """ - - @abstractmethod - def joint_names(self) -> List[str]: - """ - Return a list of joint names. The order matches joints indices. - - Returns: - The joint names. - """ - - @abstractmethod - def joint_type(self, joint_name: str) -> JointType: - """ - Return the type of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - The joint type. - """ - - @abstractmethod - def joint_control_mode(self, joint_name: str) -> JointControlMode: - """ - Return the control mode of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - The joint control mode. - """ - - @abstractmethod - def set_joint_control_mode(self, joint_name: str, mode: JointControlMode) -> bool: - """ - Set the control mode of the specified joint. - - Args: - joint_name: The name of the joint. - mode: The joint control mode. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def joint_position(self, joint_name: str) -> float: - """ - Return the generalized position of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - The joint generalized position. - - """ - - @abstractmethod - def joint_velocity(self, joint_name: str) -> float: - """ - Return the generalized velocity of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - The joint generalized velocity. - - """ - - @abstractmethod - def joint_force(self, joint_name: str) -> float: - """ - Return the joint force applied to the specified joint in the last physics step. - - The returned value is the effort or torque of the joint, since currently only - 1 DoF joints are supported. - - Note that the returned value depends on the rate of the enabled controllers. - For example, the JointControlMode.POSITION allows to specify a controller rate - different than the physics rate. In this case, this method will return only the - last applied force reference. If the PIDs are running faster than the code - that calls this `joint_force` method, the PIDs are generating forces that - are not returned. - - Args: - joint_name: The name of the joint. - - Returns: - The generalized joint force reference applied in the last physics step. - """ - - @abstractmethod - def joint_positions(self) -> np.ndarray: - """ - Return the generalized positions of the joints. - - The order matches :meth:`joint_names`. - - Returns: - The joint generalized positions. - """ - - @abstractmethod - def joint_velocities(self) -> np.ndarray: - """ - Return the generalized velocities of the joints. - - The order matches :meth:`joint_names`. - - Returns: - The joint generalized velocities. - """ - - @abstractmethod - def joint_pid(self, joint_name: str) -> Union[PID, None]: - """ - Return the PID associated to the specified joint. - - Args: - joint_name: The joint name. - - Returns: - The low-level joint PID or None if no PID was configured. - """ - - @abstractmethod - def dt(self) -> float: - """ - Returns the step_size of the low-level PID controllers. - - Returns: - The PIDs step_size. - """ - - @abstractmethod - def set_dt(self, step_size: float) -> bool: - """ - Set the step size used by the low-level PIDs. - - Args: - step_size: The PIDs step size. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def set_joint_force(self, joint_name: str, force: float, clip: bool = False) -> bool: - """ - Set the generalized force applied to the specified joint. - - If the control mode of the joint is not JointControlMode.TORQUE, this method - returns `False`. - - Args: - joint_name: The name of the joint. - force: The joint generalize force. - clip: Clip joint forces reading effort limits. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def set_joint_position(self, joint_name: str, position: float) -> bool: - """ - Set the desired position of the specified joint. - - This method sends a new position reference to the joint PID controller. - - If the control mode of the joint is not JointControlMode.POSITION, this method - returns `False`. - - Args: - joint_name: The name of the joint. - position: The position reference of the joint. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def set_joint_velocity(self, joint_name: str, velocity: float) -> bool: - """ - Set the desired velocity of the specified joint. - - This method sends a new velocity reference to the joint PID controller. - - If the control mode of the joint is not JointControlMode.VELOCITY, this method - returns `False`. - - Args: - joint_name: The name of the joint. - velocity: The velocity reference of the joint. - - Returns: - True if successful, False otherwise. - """ - - def set_joint_interpolated_position(self, joint_name: str, position: float): - """ - Set the position reference of the joint trajectory generator. - - This method sends a new position reference to the trajectory generator, - which smooths the transient with an interpolation strategy. Refer to the - documentation of the implementation for details about the trajectory generator. - - If the control mode of the joint is not JointControlMode.POSITION_INTERPOLATED, - this method returns `False`. - - Args: - joint_name: The name of the joint. - position: The position reference of the joint trajectory generator. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def set_joint_pid(self, joint_name: str, pid: PID) -> bool: - """ - Set the low-level PID associated to the specified joint. - - It can be either a position or a velocity PID. This method should be called - after setting the joint control mode. - - If the control mode of the joint is not either `POSITION` or `VELOCITY` this - method returns `False`. - - Args: - joint_name: The name of the joint. - pid: The joint PID. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def reset_joint(self, - joint_name: str, - position: float = None, - velocity: float = None) -> bool: - """ - Reset the position and velocity of a joint. - - On simulated robots this method bypasses the physics. - On real robots the usage of this method is undefined and depends on the - implementation. Refer to the documentation of the implementation for more - details. - - Args: - joint_name: The name of the joint. - position: The position of the joint. - velocity: The velocity of the joint. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def update(self, current_time: float) -> bool: - """ - Update the low-level joint PIDs. - - This method steps the joint PIDs. It can be used to separate the rates of which - references are set with the `set_joint_position` or `set_joint_velocity` - methods, and those associated to the PID controllers. During each `update` call, - PIDs gather new measurements from the robot and actuate a new generalized force. - - PIDs are updated only if a time greater than the timestep, configured with - `set_dt`, has passed. This appproach allows calling this `update` method multiple - times in a faster loop while only updating the low-level PIDs when necessary. - - Args: - current_time: The current time used to check if the PIDs have to be triggered. - - Returns: - True if successful, False otherwise. - """ - - @abstractmethod - def joint_position_limits(self, joint_name: str) -> Tuple[float, float]: - """ - Return the position limits of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - A Tuple in the form [min, max] containing the position limit. - """ - - @abstractmethod - def joint_force_limit(self, joint_name: str) -> float: - """ - Return the generalized force limit of the specified joint. - - Args: - joint_name: The name of the joint. - - Returns: - The maximum generalized force supported by the joint. - """ - - @abstractmethod - def set_joint_force_limit(self, joint_name: str, limit: float) -> bool: - """ - Set the maximum effort of the specified joint. - - Args: - joint_name: The name of the joint. - limit: The effort limit. - - Returns: - True if successful, False otherwise. - """ diff --git a/gym_ignition/base/robot/robot_links.py b/gym_ignition/base/robot/robot_links.py deleted file mode 100644 index ee1c60b0e..000000000 --- a/gym_ignition/base/robot/robot_links.py +++ /dev/null @@ -1,111 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -from typing import List, Tuple -from abc import ABC, abstractmethod - - -class RobotLinks(ABC): - """ - Robot links interface. - - This interface provides methods to get and set link-related quantities. - """ - - def __init__(self) -> None: ... - - @abstractmethod - def link_names(self) -> List[str]: - """ - Return a list of link names. The order matches link indices. - - Returns: - The link names. - """ - - @abstractmethod - def link_pose(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the pose of the specified link. - - The pose is composed of a position and a quaternion associated to the link frame, - both expressed in the the world frame. - - Args: - link_name: The name of the link. - - Returns: - A Tuple containing the position and orientation of the link: - - - position: 3D array in the [x, y, z] form. - - orientation: a 4D array containing a quaternion in the [w, x, y, z] form. - """ - - @abstractmethod - def link_velocity(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the velocity of the specified link. - - The velocity is composed of linear velocity (first three elements) and angular - velocity (last three elements) of the link frame. - The linear velocity is the time derivative of the position of the link frame - origin expressed in the world frame. - The angular velocity is expressed with the orientation of the world frame. - - Args: - link_name: The name of the link. - - Returns: - A Tuple containing the linear and angular velocity of the link: - - - linear velocity: a 3D array in the [vx, vy, vz] form. - - angular velocity: a 3D array in the [wx, wy, wz] form. - """ - - @abstractmethod - def link_acceleration(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - """ - Return the acceleration of the specified link. - - The acceleration is composed of linear acceleration (first three elements) and - angular acceleration (last three elements) of the link frame. - The linear acceleration is the second time derivative of the position of the link - frame origin expressed in the world frame. - The angular acceleration is expressed with the orientation of the world frame. - - Args: - link_name: The name of the link. - - Returns: - A Tuple containing the linear and angular acceleration of the link: - - - linear acceleration: a 3D array in the [ax, ay, az] form. - - angular acceleration: a 3D array in the [wdotx, wdoty, wdotz] form. - """ - - @abstractmethod - def apply_external_force(self, - link_name: str, - force: np.ndarray, - torque: np.ndarray) -> bool: - """ - Apply external force and torque to the link. - - The force and the torque are applied at the link origin. - The force is expressed in the world frame. - The torque is expressed with the orientation of the world frame. - - The external force and torque are only applied for the next simulation step, - and then they are automatically reset to zero. To apply a force for more then one - step, call `apply_external_force` before each step. - - Args: - link_name: The name of the link. - force: A 3D array containing the force vector. - torque: A 3D array containing the torque vector. - - Returns: - True if successful, False otherwise. - """ \ No newline at end of file From a113c013946d5b0fe6af5b5e6efaa20362c4865f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:30:58 +0200 Subject: [PATCH 04/35] Remove gazebo robot in favor of ScenarI/O They will be replaced by new proxy resources of the scenario::gazebo::Model class --- gym_ignition/__init__.py | 4 - gym_ignition/robots/__init__.py | 10 - gym_ignition/robots/base/__init__.py | 6 - gym_ignition/robots/base/gazebo_robot.py | 450 ------------ gym_ignition/robots/base/pybullet_robot.py | 726 ------------------- gym_ignition/robots/rt/__init__.py | 3 - gym_ignition/robots/rt/icub.py | 5 - gym_ignition/robots/sim/__init__.py | 7 - gym_ignition/robots/sim/gazebo/__init__.py | 8 - gym_ignition/robots/sim/gazebo/cartpole.py | 36 - gym_ignition/robots/sim/gazebo/icub.py | 5 - gym_ignition/robots/sim/gazebo/panda.py | 53 -- gym_ignition/robots/sim/gazebo/pendulum.py | 40 - gym_ignition/robots/sim/pybullet/__init__.py | 7 - gym_ignition/robots/sim/pybullet/cartpole.py | 42 -- gym_ignition/robots/sim/pybullet/icub.py | 22 - gym_ignition/robots/sim/pybullet/pendulum.py | 42 -- 17 files changed, 1466 deletions(-) delete mode 100644 gym_ignition/robots/__init__.py delete mode 100644 gym_ignition/robots/base/__init__.py delete mode 100644 gym_ignition/robots/base/gazebo_robot.py delete mode 100644 gym_ignition/robots/base/pybullet_robot.py delete mode 100644 gym_ignition/robots/rt/__init__.py delete mode 100644 gym_ignition/robots/rt/icub.py delete mode 100644 gym_ignition/robots/sim/__init__.py delete mode 100644 gym_ignition/robots/sim/gazebo/__init__.py delete mode 100644 gym_ignition/robots/sim/gazebo/cartpole.py delete mode 100644 gym_ignition/robots/sim/gazebo/icub.py delete mode 100644 gym_ignition/robots/sim/gazebo/panda.py delete mode 100644 gym_ignition/robots/sim/gazebo/pendulum.py delete mode 100644 gym_ignition/robots/sim/pybullet/__init__.py delete mode 100644 gym_ignition/robots/sim/pybullet/cartpole.py delete mode 100644 gym_ignition/robots/sim/pybullet/icub.py delete mode 100644 gym_ignition/robots/sim/pybullet/pendulum.py diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index bb63a647a..00afbe553 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -34,10 +34,6 @@ from gym.envs.registration import register from gym_ignition.utils import resource_finder -# Import the robots -from gym_ignition.robots import rt -from gym_ignition.robots import sim - # Import the tasks from gym_ignition.tasks import pendulum_swingup from gym_ignition.tasks import cartpole_discrete diff --git a/gym_ignition/robots/__init__.py b/gym_ignition/robots/__init__.py deleted file mode 100644 index e29fea4ac..000000000 --- a/gym_ignition/robots/__init__.py +++ /dev/null @@ -1,10 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# Robot implementations -from .base import gazebo_robot -from .base import pybullet_robot -# from .base import ros_robot -# from .base import yarp_robot -# from .base import ignition_transport_robot diff --git a/gym_ignition/robots/base/__init__.py b/gym_ignition/robots/base/__init__.py deleted file mode 100644 index e08e6a1c6..000000000 --- a/gym_ignition/robots/base/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -from . import gazebo_robot -from . import pybullet_robot diff --git a/gym_ignition/robots/base/gazebo_robot.py b/gym_ignition/robots/base/gazebo_robot.py deleted file mode 100644 index 3c26cce77..000000000 --- a/gym_ignition/robots/base/gazebo_robot.py +++ /dev/null @@ -1,450 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import string -import random -import numpy as np -from typing import List, Union, Tuple -from gym_ignition import gympp_bindings as bindings -from gym_ignition.utils import logger, resource_finder -from gym_ignition.base.robot import robot_baseframe, robot_initialstate -from gym_ignition.base.robot import robot_abc, robot_joints, robot_links, robot_contacts - - -class GazeboRobot(robot_abc.RobotABC, - robot_links.RobotLinks, - robot_joints.RobotJoints, - robot_contacts.RobotContacts, - robot_baseframe.RobotBaseFrame, - robot_initialstate.RobotInitialState): - - def __init__(self, - model_file: str, - gazebo: bindings.GazeboSimulator, - controller_rate: float = None) -> None: - - # Find the model file - model_abs_path = resource_finder.find_resource(model_file) - - # Initialize the parent classes - robot_abc.RobotABC.__init__(self) - robot_baseframe.RobotBaseFrame.__init__(self) - robot_initialstate.RobotInitialState.__init__(self) - self.model_file = model_abs_path - - # Private attributes - self._gazebo = gazebo - self._robot_name = None - self._gympp_robot = None - self._base_frame = None - self._joint_names = None - self._controller_rate = controller_rate - - # Create a random prefix that will be used for the robot name - letters_and_digits = string.ascii_letters + string.digits - self._prefix = ''.join(random.choice(letters_and_digits) for _ in range(6)) - - # ============================== - # PRIVATE METHODS AND PROPERTIES - # ============================== - - def delete_simulated_robot(self) -> None: - if not self._gazebo or not self._gympp_robot: - logger.warn("Failed to delete robot from the simulation. " - "Simulator not running.") - return - - # Remove the robot from the simulation - ok_model = self._gazebo.removeModel(self._robot_name) - assert ok_model, f"Failed to remove the model '{self._robot_name}' from gazebo" - - @staticmethod - def _to_cpp_controlmode(mode: robot_joints.JointControlMode): - map_cm = { - robot_joints.JointControlMode.POSITION: bindings.JointControlMode_Position, - robot_joints.JointControlMode.POSITION_INTERPOLATED: - bindings.JointControlMode_PositionInterpolated, - robot_joints.JointControlMode.VELOCITY: bindings.JointControlMode_Velocity, - robot_joints.JointControlMode.TORQUE: bindings.JointControlMode_Torque, - } - assert mode in map_cm, f"Unsupported '{mode}' control mode" - return map_cm[mode] - - @staticmethod - def _from_cpp_controlmode(mode) -> robot_joints.JointControlMode: - map_cm = { - bindings.JointControlMode_Position: robot_joints.JointControlMode.POSITION, - bindings.JointControlMode_PositionInterpolated: - robot_joints.JointControlMode.POSITION_INTERPOLATED, - bindings.JointControlMode_Velocity: robot_joints.JointControlMode.VELOCITY, - bindings.JointControlMode_Torque: robot_joints.JointControlMode.TORQUE, - } - assert mode in map_cm, f"Unsupported '{mode}' control mode" - return map_cm[mode] - - @property - def gympp_robot(self) -> bindings.Robot: - if self._gympp_robot: - return self._gympp_robot - - # Find and load the model SDF file - sdf_file = resource_finder.find_resource(self.model_file) - with open(sdf_file, "r") as stream: - sdf_string = stream.read() - - # Get the model name - original_name = bindings.GazeboSimulator.getModelNameFromSDF(sdf_string) - assert original_name, f"Failed to get model name from file {self.model_file}" - - # Create a unique robot name - self._robot_name = self._prefix + "::" + original_name - - initial_base_pose, initial_base_orientation = self.initial_base_pose() - - # Initialize the model data - model_data = bindings.ModelInitData() - model_data.modelName = self._robot_name - model_data.sdfString = sdf_string - model_data.fixedPose = not self.is_floating_base() - model_data.position =initial_base_pose.tolist() - model_data.orientation = initial_base_orientation.tolist() - - if self._base_frame is not None: - model_data.setBaseLink(self._base_frame) - - # Initialize robot controller plugin - plugin_data = bindings.PluginData() - plugin_data.libName = "RobotController" - plugin_data.className = "scenario::plugins::gazebo::RobotController" - - # Insert the model - ok_model = self._gazebo.insertModel(model_data, plugin_data) - assert ok_model, "Failed to insert the model" - - # Extract the robot from the singleton - gympp_robot_weak = bindings.RobotSingleton_get().getRobot(self._robot_name) - - # The robot is a weak pointer. Check that it is valid. - assert not gympp_robot_weak.expired(), "The Robot object has expired" - assert gympp_robot_weak.lock(), \ - "The returned Robot object does not contain a valid interface" - - self._gympp_robot = gympp_robot_weak.lock() - assert self._gympp_robot.valid(), "The Robot object is not valid" - - if self._controller_rate is not None: - logger.debug(f"Robot controller rate: {self._controller_rate} Hz") - ok_dt = self._gympp_robot.setdt(1 / self._controller_rate) - assert ok_dt, "Failed to set the robot controller period" - - s0 = self.initial_joint_positions() - sdot0 = self.initial_joint_velocities() - joint_names = list(self._gympp_robot.jointNames()) - - assert s0.size == len(joint_names) - assert sdot0.size == len(joint_names) - - for idx, name in enumerate(joint_names): - ok_reset = self._gympp_robot.resetJoint(name, s0[idx], sdot0[idx]) - assert ok_reset, f"Failed to initialize the state of joint '{name}'" - - logger.debug( - f"GazeboRobot '{self._gympp_robot.name()}' added to the simulation") - return self._gympp_robot - - # ======== - # RobotABC - # ======== - - def name(self): - return self.gympp_robot.name() - - def valid(self) -> bool: - return self.gympp_robot.valid() - - # =========== - # RobotJoints - # =========== - - def dofs(self) -> int: - return len(self.joint_names()) - - def joint_names(self) -> List[str]: - if self._joint_names is None: - self._joint_names = list(self.gympp_robot.jointNames()) - - return self._joint_names - - def joint_type(self, joint_name: str) -> robot_joints.JointType: - joint_type = self.gympp_robot.jointType(joint_name) - assert joint_type != bindings.JointType_Invalid, \ - f"Type of joint '{joint_name}' not valid" - - if joint_type == bindings.JointType_Revolute: - return robot_joints.JointType.REVOLUTE - elif joint_type == bindings.JointType_Prismatic: - return robot_joints.JointType.PRISMATIC - elif joint_type == bindings.JointType_Fixed: - return robot_joints.JointType.FIXED - elif joint_type == bindings.JointType_Invalid: - return robot_joints.JointType.INVALID - else: - raise Exception(f"Failed to recognize type of joint '{joint_name}'") - - def joint_control_mode(self, joint_name: str) -> robot_joints.JointControlMode: - return self._from_cpp_controlmode(self.gympp_robot.jointControlMode(joint_name)) - - def set_joint_control_mode(self, - joint_name: str, - mode: robot_joints.JointControlMode) -> bool: - return self.gympp_robot.setJointControlMode(joint_name, - self._to_cpp_controlmode(mode)) - - def joint_position(self, joint_name: str) -> float: - return self.gympp_robot.jointPosition(joint_name) - - def joint_velocity(self, joint_name: str) -> float: - return self.gympp_robot.jointVelocity(joint_name) - - def joint_force(self, joint_name: str) -> float: - return self.gympp_robot.jointForce(joint_name) - - def joint_positions(self) -> np.ndarray: - return np.array(self.gympp_robot.jointPositions()) - - def joint_velocities(self) -> np.ndarray: - return np.array(self.gympp_robot.jointVelocities()) - - def joint_pid(self, joint_name: str) -> Union[robot_joints.PID, None]: - gazebo_pid = self.gympp_robot.jointPID(joint_name) - return robot_joints.PID(p=gazebo_pid.p, i=gazebo_pid.i, d=gazebo_pid.d) - - def dt(self) -> float: - return self.gympp_robot.dt() - - def set_dt(self, step_size: float) -> bool: - return self.gympp_robot.setdt(step_size) - - def set_joint_force(self, joint_name: str, force: float, clip: bool = False) -> bool: - return self.gympp_robot.setJointForce(joint_name, force) - - def set_joint_position(self, joint_name: str, position: float) -> bool: - return self.gympp_robot.setJointPositionTarget(joint_name, position) - - def set_joint_velocity(self, joint_name: str, velocity: float) -> bool: - return self.gympp_robot.setJointVelocityTarget(joint_name, velocity) - - def set_joint_interpolated_position(self, joint_name: str, position: float): - raise NotImplementedError - - def set_joint_pid(self, joint_name: str, pid: robot_joints.PID) -> bool: - gazebo_pid = bindings.PID(pid.p, pid.i, pid.d) - return self.gympp_robot.setJointPID(joint_name, gazebo_pid) - - def reset_joint(self, - joint_name: str, - position: float = None, - velocity: float = None) -> bool: - return self.gympp_robot.resetJoint(joint_name, position, velocity) - - def update(self, dt: float): - return self.gympp_robot.update(dt) - - def joint_position_limits(self, joint_name: str) -> Tuple[float, float]: - limit = self.gympp_robot.jointPositionLimits(joint_name) - return float(limit.min), float(limit.max) - - def joint_force_limit(self, joint_name: str) -> float: - limit = self.gympp_robot.jointEffortLimit(joint_name) - return float(limit) - - def set_joint_force_limit(self, joint_name: str, limit: float) -> bool: - if limit < 0: - raise ValueError(limit) - - ok_limit = self.gympp_robot.setJointEffortLimit(joint_name, limit) - return ok_limit - - # ============== - # RobotBaseFrame - # ============== - - def set_as_floating_base(self, floating: bool) -> bool: - if self._gympp_robot is not None and self._is_floating_base != floating: - raise Exception( - "Changing the base type after the creation of the robot is not supported") - - self._is_floating_base = floating - return True - - def is_floating_base(self) -> bool: - return self._is_floating_base - - def set_base_frame(self, frame_name: str) -> bool: - self._base_frame = frame_name - - if self._gympp_robot: - ok_base_frame = self.gympp_robot.setBaseFrame(frame_name) - assert ok_base_frame, "Failed to set base frame" - - return True - - def base_frame(self) -> str: - if self._base_frame is None: - self._base_frame = self.gympp_robot.baseFrame() - - return self._base_frame - - def base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - base_pose = self.gympp_robot.basePose() - position = np.array(base_pose.position) - orientation = np.array(base_pose.orientation) - return position, orientation - - def base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - base_velocity_gympp = self.gympp_robot.baseVelocity() - return np.array(base_velocity_gympp.linear), np.array(base_velocity_gympp.angular) - - def reset_base_pose(self, - position: np.ndarray, - orientation: np.ndarray) -> bool: - - assert position.size == 3, "Position should be a list with 3 elements" - assert orientation.size == 4, "Orientation should be a list with 4 elements" - - if not self._is_floating_base: - logger.error("Changing the pose of a fixed-base robot is not yet supported") - logger.error("Remove and insert again the robot in the new pose") - return False - - ok_pose = self.gympp_robot.resetBasePose(position.tolist(), - orientation.tolist()) - assert ok_pose, "Failed to set base pose" - - return True - - def reset_base_velocity(self, - linear_velocity: np.ndarray, - angular_velocity: np.ndarray) -> bool: - assert linear_velocity.size == 3, \ - "Linear velocity should be a list with 3 elements" - assert angular_velocity.size == 3, \ - "Angular velocity should be a list with 3 elements" - - if not self._is_floating_base: - logger.error("Changing the velocity of a fixed-base robot is not supported") - return False - - ok_velocity = self.gympp_robot.resetBaseVelocity(linear_velocity.tolist(), - angular_velocity.tolist()) - assert ok_velocity, "Failed to reset the base velocity" - - return True - - def base_wrench(self) -> np.ndarray: - raise NotImplementedError - - # ================= - # RobotInitialState - # ================= - - def initial_joint_positions(self) -> np.ndarray: - if self._initial_joint_positions is None: - self._initial_joint_positions = \ - np.array(self.gympp_robot.initialJointPositions()) - - return self._initial_joint_positions - - def set_initial_joint_positions(self, positions: np.ndarray) -> bool: - if self._gympp_robot is not None: - raise Exception("The robot object has been already created") - - self._initial_joint_positions = positions - return True - - def initial_joint_velocities(self) -> np.ndarray: - if self._initial_joint_velocities is None: - self._initial_joint_velocities = np.zeros(self.dofs()) - - return self._initial_joint_velocities - - def set_initial_joint_velocities(self, velocities: np.ndarray) -> bool: - if self._gympp_robot is not None: - raise Exception("The robot object has been already created") - - self._initial_joint_velocities = velocities - return True - - def initial_base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - return self._initial_base_pose - - def set_initial_base_pose(self, - position: np.ndarray, - orientation: np.ndarray) -> bool: - assert position.size == 3, "'position' should be an array with 3 elements" - assert orientation.size == 4, "'orientation' should be an array with 4 elements" - self._initial_base_pose = (position, orientation) - return True - - def initial_base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - return self._initial_base_velocity - - def set_initial_base_velocity(self, linear: np.ndarray, angular: np.ndarray) -> bool: - assert linear.size == 3, "'linear' should be an array with 3 elements" - assert angular.size == 4, "'angular' should be an array with 4 elements" - self._initial_base_velocity = (linear, angular) - return True - - # ============= - # RobotContacts - # ============= - - def links_in_contact(self) -> List[str]: - return list(self._gympp_robot.linksInContact()) - - def contact_data(self, contact_link_name: str) -> List[robot_contacts.ContactData]: - contacts = [] - gazebo_contacts = self._gympp_robot.contactData(contact_link_name) - - for contact in gazebo_contacts: - contact_data = robot_contacts.ContactData(bodyA=contact.bodyA, - bodyB=contact.bodyB, - position=contact.position) - contacts.append(contact_data) - - return contacts - - def total_contact_wrench_on_link(self, contact_link_name: str) -> np.ndarray: - raise NotImplementedError - - # ========== - # RobotLinks - # ========== - - def link_names(self) -> List[str]: - return self.gympp_robot.linkNames() - - def link_pose(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - link_pose_gympp = self.gympp_robot.linkPose(link_name) - return np.array(link_pose_gympp.position), np.array(link_pose_gympp.orientation) - - def link_velocity(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - link_velocity_gympp = self.gympp_robot.linkVelocity(link_name) - return np.array(link_velocity_gympp.linear), np.array(link_velocity_gympp.angular) - - def link_acceleration(self, link_name: str) -> Tuple[np.ndarray, np.ndarray]: - link_acc_gympp = self.gympp_robot.linkAcceleration(link_name) - return np.array(link_acc_gympp.linear), np.array(link_acc_gympp.angular) - - def apply_external_force(self, - link_name: str, - force: np.ndarray, - torque: np.ndarray) -> bool: - assert link_name in self.link_names(), "The link is not part of the model" - - ok_wrench = self.gympp_robot.addExternalWrench( - link_name, list(force), list(torque)) - assert ok_wrench, f"Failed to add external wrench to link '{link_name}'" - - return True diff --git a/gym_ignition/robots/base/pybullet_robot.py b/gym_ignition/robots/base/pybullet_robot.py deleted file mode 100644 index 6696cb456..000000000 --- a/gym_ignition/robots/base/pybullet_robot.py +++ /dev/null @@ -1,726 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import os -import numpy as np -from gym_ignition.base import robot -from gym_ignition.utils import logger, resource_finder -from typing import List, Tuple, Dict, Union, NamedTuple -from gym_ignition.base.robot.robot_joints import JointControlMode - - -class JointControlInfo(NamedTuple): - mode: JointControlMode - PID: robot.PID = None - - -def controlmode_to_pybullet(control_mode: JointControlMode): - import pybullet - JointControlMode2PyBullet = { - JointControlMode.POSITION: pybullet.PD_CONTROL, - JointControlMode.POSITION_INTERPOLATED: None, - JointControlMode.VELOCITY: pybullet.PD_CONTROL, - JointControlMode.TORQUE: pybullet.TORQUE_CONTROL, - } - - return JointControlMode2PyBullet[control_mode] - - -class ContactPyBullet(NamedTuple): - contactFlag: int - bodyUniqueIdA: int - bodyUniqueIdB: int - linkIndexA: int - linkIndexB: int - positionOnA: List[float] - positionOnB: List[float] - contactNormalOnB: List[float] - contactDistance: float - normalForce: float - lateralFriction1: float - lateralFrictionDir1: List[float] - lateralFriction2: float - lateralFrictionDir2: List[float] - - -class JointInfoPyBullet(NamedTuple): - jointIndex: int - jointName: bytes - jointType: int - qIndex: int - uIndex: int - flags: int - jointDamping: float - jointFriction: float - jointLowerLimit: float - jointUpperLimit: float - jointMaxForce: float - jointMaxVelocity: float - linkName: bytes - jointAxis: List - parentFramePos: List - parentFrameOrn: List - parentIndex: int - - -class PyBulletRobot(robot.robot_abc.RobotABC, - robot.robot_joints.RobotJoints, - robot.robot_contacts.RobotContacts, - robot.robot_baseframe.RobotBaseFrame, - robot.robot_initialstate.RobotInitialState): - """ - A "class"`Robot` implementation for the PyBullet simulator. - - Args: - p: An instance of the pybullet simulator. - model_file: The file (URDF, SDF) containing the robot model. - plane_id: the pybullet ID associated with the plane model. It is used to - compute contacts between the robot and the plane. - """ - - def __init__(self, - p, - model_file: str, - plane_id: int = None, - keep_fixed_joints: bool = False) -> None: - - self._pybullet = p - self._plane_id = plane_id - self._keep_fixed_joints = keep_fixed_joints - - # Find the model file - model_abs_path = resource_finder.find_resource(model_file) - - # Initialize the parent classes - robot.robot_abc.RobotABC.__init__(self) - robot.robot_baseframe.RobotBaseFrame.__init__(self) - robot.robot_initialstate.RobotInitialState.__init__(self) - self.model_file = model_abs_path - - # Private attributes - self._robot_id = None - self._links_name2index_dict = None - self._joints_name2index_dict = None - - # TODO - self._base_frame = None - self._base_constraint = None - - # Create a map from the joint name to its control info - self._jointname2jointcontrolinfo = dict() - - # ============================== - # PRIVATE METHODS AND PROPERTIES - # ============================== - - def delete_simulated_robot(self) -> None: - if not self._pybullet or self._robot_id is None: - logger.warn("Failed to delete robot from the simulation. " - "Simulator not running.") - return - - # Remove the robot from the simulation - self._pybullet.removeBody(self._robot_id) - - @property - def _joints_name2index(self) -> Dict[str, int]: - if self._joints_name2index_dict is not None: - return self._joints_name2index_dict - - self._joints_name2index_dict = dict() - joints_info = self._get_joints_info() - - for _, info in joints_info.items(): - joint_idx = info.jointIndex - joint_name = info.jointName.decode() - self._joints_name2index_dict[joint_name] = joint_idx - - return self._joints_name2index_dict - - @property - def _links_name2index(self) -> Dict[str, int]: - if self._links_name2index_dict is not None: - return self._links_name2index_dict - - self._links_name2index_dict = dict() - - for _, info in self._get_joints_info().items(): - self._links_name2index_dict[info.linkName.decode()] = info.jointIndex - - return self._links_name2index_dict - - def _load_model(self, filename: str, **kwargs) -> int: - # Get the file extension - extension = os.path.splitext(filename)[1][1:] - - if extension == "sdf": - model_id = self._pybullet.loadSDF(filename, **kwargs)[0] - else: - import pybullet - model_id = self._pybullet.loadURDF( - fileName=filename, - flags=pybullet.URDF_USE_INERTIA_FROM_FILE, - **kwargs) - - return model_id - - def initialize_model(self) -> None: - # Load the model - self._robot_id = self._load_model(self.model_file) - assert self._robot_id is not None, "Failed to load the robot model" - - # Initialize all the joints in POSITION mode - for name in self.joint_names(): - self._jointname2jointcontrolinfo[name] = JointControlInfo( - mode=JointControlMode.POSITION) - ok_mode = self.set_joint_control_mode(name, JointControlMode.POSITION) - assert ok_mode, \ - f"Failed to initialize the control mode of joint '{name}'" - - # Initialize the joints state - for idx, name in enumerate(self.joint_names()): - self.reset_joint(joint_name=name, - position=self.initial_joint_positions()[idx], - velocity=self.initial_joint_velocities()[idx]) - - # Initialize the base pose - initial_base_position, initial_base_orientation = self.initial_base_pose() - self.reset_base_pose(initial_base_position, initial_base_orientation) - - def _get_joints_info(self) -> Dict[str, JointInfoPyBullet]: - joints_info = {} - - for j in range(self._pybullet.getNumJoints(self._robot_id)): - # Get the joint info from pybullet - joint_info_pybullet = self._pybullet.getJointInfo(self._robot_id, j) - - # Store it in a namedtuple - joint_info = JointInfoPyBullet._make(joint_info_pybullet) - - # Add the dict entry - joints_info[joint_info.jointName.decode()] = joint_info - - return joints_info - - def _get_contact_info(self) -> List[ContactPyBullet]: - # Get the all contact points in the simulation - pybullet_contacts = self._pybullet.getContactPoints() - - # List containing only the contacts that involve the robot model - contacts = [] - - # Extract only the robot contacts - for pybullet_contact in pybullet_contacts: - contact = ContactPyBullet._make(pybullet_contact) - contacts.append(contact) - - return contacts - - # ===================== - # robot.Robot INTERFACE - # ===================== - - def name(self) -> str: - # TODO - pass - - def valid(self) -> bool: - try: - # TODO: improve the check - self._pybullet.getBasePositionAndOrientation(self._robot_id) - return True - except: - return False - - # ================================== - # robot_joints.RobotJoints INTERFACE - # ================================== - - def dofs(self) -> int: - dofs = len(self.joint_names()) - - if self._keep_fixed_joints: - assert dofs == self._pybullet.getNumJoints(self._robot_id), \ - "Number of DoFs does not match with the simulated model" - - return dofs - - def joint_names(self) -> List[str]: - import pybullet - joints_names = [] - - # Get Joint names - for _, info in self._get_joints_info().items(): - if not self._keep_fixed_joints and info.jointType == pybullet.JOINT_FIXED: - continue - - # Strings are byte-encoded. They have to be decoded with UTF-8 (default). - joints_names.append(info.jointName.decode()) - - return joints_names - - def joint_type(self, joint_name: str) -> robot.robot_joints.JointType: - import pybullet - joint_info = self._get_joints_info()[joint_name] - joint_type_pybullet = joint_info.jointType - - if joint_type_pybullet == pybullet.JOINT_FIXED: - return robot.robot_joints.JointType.FIXED - elif joint_type_pybullet == pybullet.JOINT_REVOLUTE: - return robot.robot_joints.JointType.REVOLUTE - elif joint_type_pybullet == pybullet.JOINT_PRISMATIC: - return robot.robot_joints.JointType.PRISMATIC - else: - raise Exception(f"Joint type '{joint_type_pybullet}' not yet supported") - - def joint_control_mode(self, joint_name: str) -> JointControlMode: - return self._jointname2jointcontrolinfo[joint_name].mode - - def set_joint_control_mode(self, joint_name: str, mode: JointControlMode) -> bool: - # Store the control mode and initialize the PID - if mode in {JointControlMode.TORQUE}: - pid = None - self._jointname2jointcontrolinfo[joint_name] = JointControlInfo(mode=mode) - elif mode in {JointControlMode.POSITION, JointControlMode.VELOCITY}: - # Initialize a default PID - pid = robot.PID(p=1, i=0, d=0) - self._jointname2jointcontrolinfo[joint_name] = JointControlInfo( - mode=mode, PID=pid) - elif mode == JointControlMode.POSITION_INTERPOLATED: - raise Exception("Control mode POSITION_INTERPOLATED is not supported") - else: - raise Exception(f"Control mode '{mode}' not recognized") - - mode_pybullet = controlmode_to_pybullet(mode) - joint_idx_pybullet = self._joints_name2index[joint_name] - - # Disable the default joint motorization setting a 0 maximum force - if mode == JointControlMode.TORQUE: - import pybullet - # Disable the PID if was configured - self._pybullet.setJointMotorControl2( - bodyIndex=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=pybullet.PD_CONTROL, - positionGain=0, - velocityGain=0) - # Put the motor in IDLE for torque control - self._pybullet.setJointMotorControl2( - bodyIndex=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=pybullet.VELOCITY_CONTROL, - force=0) - - # Change the control mode of the joint - if mode == JointControlMode.POSITION: - self._pybullet.setJointMotorControl2( - bodyIndex=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=mode_pybullet, - positionGain=pid.p, - velocityGain=pid.d) - elif mode == JointControlMode.VELOCITY: - # TODO: verify that setting the gains in this way processes the reference - # correctly. - self._pybullet.setJointMotorControl2( - bodyIndex=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=mode_pybullet, - positionGain=pid.i, - velocityGain=pid.p) - - return True - - def joint_position(self, joint_name: str) -> float: - joint_idx = self._joints_name2index[joint_name] - return self._pybullet.getJointState(self._robot_id, joint_idx)[0] - - def joint_velocity(self, joint_name: str) -> float: - joint_idx = self._joints_name2index[joint_name] - return self._pybullet.getJointState(self._robot_id, joint_idx)[1] - - def joint_force(self, joint_name: str) -> float: - raise NotImplementedError - - def joint_positions(self) -> List[float]: - joint_states = self._pybullet.getJointStates(self._robot_id, range(self.dofs())) - joint_positions = [state[0] for state in joint_states] - return joint_positions - - def joint_velocities(self) -> List[float]: - joint_states = self._pybullet.getJointStates(self._robot_id, range(self.dofs())) - joint_velocities = [state[1] for state in joint_states] - return joint_velocities - - def joint_pid(self, joint_name: str) -> Union[robot.PID, None]: - return self._jointname2jointcontrolinfo[joint_name].PID - - def dt(self) -> float: - raise NotImplementedError - - def set_dt(self, step_size: float) -> bool: - raise NotImplementedError - - def set_joint_force(self, joint_name: str, force: float, clip: bool = False) -> bool: - - if self._jointname2jointcontrolinfo[joint_name].mode != JointControlMode.TORQUE: - raise Exception("Joint '{}' is not controlled in TORQUE".format(joint_name)) - - joint_idx_pybullet = self._joints_name2index[joint_name] - mode_pybullet = controlmode_to_pybullet(JointControlMode.TORQUE) - - # Clip the force if specified - if clip: - fmin, fmax = self.joint_force_limit(joint_name) - force = fmin if force < fmin else fmax if force > fmax else force - - # Set the joint force - self._pybullet.setJointMotorControl2(bodyUniqueId=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=mode_pybullet, - force=force) - - return True - - def set_joint_position(self, joint_name: str, position: float) -> bool: - - if self._jointname2jointcontrolinfo[joint_name].mode != JointControlMode.POSITION: - raise Exception("Joint '{}' is not controlled in POSITION".format(joint_name)) - - pid = self._jointname2jointcontrolinfo[joint_name].PID - - joint_idx_pybullet = self._joints_name2index[joint_name] - mode_pybullet = controlmode_to_pybullet(JointControlMode.POSITION) - - # Change the control mode of the joint - self._pybullet.setJointMotorControl2( - bodyUniqueId=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=mode_pybullet, - targetVelocity=position, - positionGain=pid.p, - velocityGain=pid.d) - - return True - - def set_joint_velocity(self, joint_name: str, velocity: float) -> bool: - - if self._jointname2jointcontrolinfo[joint_name].mode not in \ - {JointControlMode.POSITION, JointControlMode.VELOCITY}: - raise Exception("Joint '{}' is not controlled in VELOCITY".format(joint_name)) - - pid = self._jointname2jointcontrolinfo[joint_name].PID - - joint_idx_pybullet = self._joints_name2index[joint_name] - mode_pybullet = controlmode_to_pybullet(JointControlMode.POSITION) - - # Change the control mode of the joint - self._pybullet.setJointMotorControl2( - bodyUniqueId=self._robot_id, - jointIndex=joint_idx_pybullet, - controlMode=mode_pybullet, - targetVelocity=velocity, - positionGain=pid.p, - velocityGain=pid.d) - - return True - - def set_joint_interpolated_position(self, joint_name: str, position: float): - return NotImplementedError - - def set_joint_pid(self, joint_name: str, pid: robot.PID) -> bool: - - mode = self._jointname2jointcontrolinfo[joint_name].mode - assert mode != JointControlMode.POSITION_INTERPOLATED - - if mode == JointControlMode.TORQUE: - logger.warn( - f"Joint '{joint_name}' is torque controlled. " - f"Setting the PID has no effect") - return False - - if mode == JointControlMode.POSITION and pid.i != 0.0: - raise Exception("Integral term not supported for POSITION mode") - elif mode == JointControlMode.VELOCITY and pid.d != 0.0: - raise Exception("Derivative term not supported for VELOCITY mode") - - # Store the new PID - self._jointname2jointcontrolinfo[joint_name]._replace(PID=pid) - - # Update the PIDs setting again the control mode - ok_mode = self.set_joint_control_mode(joint_name, mode) - assert ok_mode, "Failed to set the control mode" - - return True - - def reset_joint(self, - joint_name: str, - position: float = None, - velocity: float = None) -> bool: - - joint_idx_pybullet = self._joints_name2index[joint_name] - self._pybullet.resetJointState(bodyUniqueId=self._robot_id, - jointIndex=joint_idx_pybullet, - targetValue=position, - targetVelocity=velocity) - - return True - - def update(self, current_time: float) -> bool: - raise NotImplementedError - - def joint_position_limits(self, joint_name: str) -> Tuple[float, float]: - # Get Joint info - joint_info = self._get_joints_info()[joint_name] - - return joint_info.jointLowerLimit, joint_info.jointUpperLimit - - def joint_force_limit(self, joint_name: str) -> float: - # Get Joint info - joint_info = self._get_joints_info()[joint_name] - - return joint_info.jointMaxForce - - def set_joint_force_limit(self, joint_name: str, limit: float) -> bool: - raise NotImplementedError - - # ===================================== - # robot_joints.RobotBaseFrame INTERFACE - # ===================================== - - def set_as_floating_base(self, floating: bool) -> bool: - # Handle fixed to floating base conversion - if self._robot_id is not None and not self._is_floating_base and floating: - assert self._base_constraint, "No base constraint found" - self._pybullet.removeConstraint(self._base_constraint) - self._base_constraint = None - - # The next call of reset_base_pose will create the constraint - self._is_floating_base = floating - return True - - def is_floating_base(self) -> bool: - if not self._is_floating_base: - assert self._base_constraint is not None - - return self._is_floating_base - - def set_base_frame(self, frame_name: str) -> bool: - # TODO: check that it exists - self._base_frame = frame_name - return True - - def base_frame(self) -> str: - return self._base_frame - - def base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - # Get the values - pos, quat = self._pybullet.getBasePositionAndOrientation(self._robot_id) - - # Convert tuples into lists - pos = np.array(pos) - quat = np.array(quat) - - # PyBullet returns xyzw, the interface instead returns wxyz - quat = np.roll(quat, 1) - - return pos, quat - - def base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - # TODO: double check - # Get the values - lin, ang = self._pybullet.getBaseVelocity(self._robot_id) - - # Convert tuples into lists - lin = np.array(lin) - ang = np.array(ang) - - return lin, ang - - def reset_base_pose(self, position: np.ndarray, orientation: np.ndarray) -> bool: - - assert position.size == 3, "Position should be a list with 3 elements" - assert orientation.size == 4, "Orientation should be a list with 4 elements" - - # PyBullet wants xyzw, but the function argument quaternion is wxyz - orientation = np.roll(orientation, -1) - - # Fixed base robot and constraint already created - if not self._is_floating_base and self._base_constraint: - cur_position, cur_orientation = self.base_pose() - if not np.allclose(position, cur_position) or \ - np.allclose(orientation, cur_orientation): - assert self._base_constraint, "No base constraint found" - self._pybullet.removeConstraint(self._base_constraint) - self._base_constraint = None - - # Fixed base robot and constraint not yet created - if not self._is_floating_base and not self._base_constraint: - # Set a constraint between base_link and world - # TODO: check the floating base link - self._base_constraint = self._pybullet.createConstraint( - self._robot_id, -1, -1, -1, self._pybullet.JOINT_FIXED, - [0, 0, 0], [0, 0, 0], position, orientation) - - self._pybullet.resetBasePositionAndOrientation( - self._robot_id, position, orientation) - - return True - - def reset_base_velocity(self, - linear_velocity: np.ndarray, - angular_velocity: np.ndarray) -> bool: - raise NotImplementedError - - def base_wrench(self) -> np.ndarray: - assert self._base_constraint is not None, "The robot is not fixed base" - constraint_wrench = self._pybullet.getConstraintState(self._base_constraint) - # print("Constraint State =", constraint_wrench) - return np.array(constraint_wrench) - - # ============= - # RobotContacts - # ============= - - def links_in_contact(self) -> List[str]: - # Get the all contact points in the simulation - all_contacts = self._get_contact_info() - - # List containing only the contacts that involve the robot model - contacts_robot = [] - - # Extract only the robot contacts - for contact in all_contacts: - if contact.bodyUniqueIdA == self._robot_id or \ - contact.bodyUniqueIdB == self._robot_id: - contacts_robot.append(contact) - - # List containing the names of the link with active contacts - links_in_contact = [] - - for contact in contacts_robot: - # Link of the link in contact - link_idx_in_contact = None - - # Get the index of the link in contact. - # The robot body could be either body A or body B. - if contact.bodyUniqueIdA == self._robot_id: - link_idx_in_contact = contact.linkIndexA - elif contact.bodyUniqueIdB == self._robot_id: - link_idx_in_contact = contact.linkIndexB - - # Get the link name from the index and add it to the returned list - for link_name, link_idx in self._links_name2index.items(): - if link_idx == link_idx_in_contact: - if link_name not in links_in_contact: - - # Do not consider the contact if the wrench is (almost) zero - wrench = self.total_contact_wrench_on_link(link_name) - - if np.linalg.norm(wrench) < 1e-6: - continue - - # Append the link name to the list of link in contact - links_in_contact.append(link_name) - - return links_in_contact - - def contact_data(self, contact_link_name: str): # TODO - raise NotImplementedError - - def total_contact_wrench_on_link(self, contact_link_name: str) -> np.ndarray: - # TODO: finish the implementation of this method - - # Get the all contact points in the simulation - all_contacts = self._get_contact_info() - - # List containing only the contacts that involve the robot model - contacts_robot = [] - - # Extract only the robot contacts - for contact in all_contacts: - if contact.bodyUniqueIdA == self._robot_id or contact.bodyUniqueIdB == \ - self._robot_id: - contacts_robot.append(contact) - - contact_link_idx = self._links_name2index[contact_link_name] - - # List containing only the contacts that involve the link - contacts_link = [] - - for contact in contacts_robot: - if contact.bodyUniqueIdA == self._robot_id and \ - contact.linkIndexA == contact_link_idx or \ - contact.bodyUniqueIdB == self._robot_id and \ - contact.linkIndexB == contact_link_idx: - contacts_link.append(contact) - - total_force = np.zeros(3) - - for contact in contacts_link: - # TODO: handle that id could be either body a or b - force_1 = contact.normalForce * np.array(contact.contactNormalOnB) - force_2 = contact.lateralFriction1 * np.array(contact.lateralFrictionDir1) - force_3 = contact.lateralFriction2 * np.array(contact.lateralFrictionDir2) - - # TODO: compose the force transforming it in wrench applied to the link frame - total_force += force_1 + force_2 + force_3 - - return total_force - - # ================= - # RobotInitialState - # ================= - - def initial_joint_positions(self) -> np.ndarray: - if self._initial_joint_positions is None: - self._initial_joint_positions = np.zeros(self.dofs()) - - return self._initial_joint_positions - - def set_initial_joint_positions(self, positions: np.ndarray) -> bool: - if self._robot_id is not None: - raise Exception("The robot object has been already created") - - if positions.size != self.dofs(): - return False - - self._initial_joint_positions = positions - return True - - def initial_joint_velocities(self) -> np.ndarray: - if self._initial_joint_velocities is None: - self._initial_joint_velocities = np.zeros(self.dofs()) - - return self._initial_joint_velocities - - def set_initial_joint_velocities(self, velocities: np.ndarray) -> bool: - if self._robot_id is not None: - raise Exception("The robot object has been already created") - - if velocities.size != self.dofs(): - return False - - self._initial_joint_velocities = velocities - return True - - def initial_base_pose(self) -> Tuple[np.ndarray, np.ndarray]: - return self._initial_base_pose - - def set_initial_base_pose(self, - position: np.ndarray, - orientation: np.ndarray) -> bool: - assert position.size == 3, "'position' should be an array with 3 elements" - assert orientation.size == 4, "'orientation' should be an array with 4 elements" - self._initial_base_pose = (position, orientation) - return True - - def initial_base_velocity(self) -> Tuple[np.ndarray, np.ndarray]: - return self._initial_base_velocity - - def set_initial_base_velocity(self, linear: np.ndarray, angular: np.ndarray) -> bool: - assert linear.size == 3, "'linear' should be an array with 3 elements" - assert angular.size == 4, "'angular' should be an array with 4 elements" - self._initial_base_velocity = (linear, angular) - return True diff --git a/gym_ignition/robots/rt/__init__.py b/gym_ignition/robots/rt/__init__.py deleted file mode 100644 index 3805e2d59..000000000 --- a/gym_ignition/robots/rt/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. diff --git a/gym_ignition/robots/rt/icub.py b/gym_ignition/robots/rt/icub.py deleted file mode 100644 index 263e58497..000000000 --- a/gym_ignition/robots/rt/icub.py +++ /dev/null @@ -1,5 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# TODO diff --git a/gym_ignition/robots/sim/__init__.py b/gym_ignition/robots/sim/__init__.py deleted file mode 100644 index 6b7efdba9..000000000 --- a/gym_ignition/robots/sim/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# Import the robots -from . import gazebo -from . import pybullet diff --git a/gym_ignition/robots/sim/gazebo/__init__.py b/gym_ignition/robots/sim/gazebo/__init__.py deleted file mode 100644 index fdeeda00a..000000000 --- a/gym_ignition/robots/sim/gazebo/__init__.py +++ /dev/null @@ -1,8 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -from . import icub -from . import panda -from . import pendulum -from . import cartpole diff --git a/gym_ignition/robots/sim/gazebo/cartpole.py b/gym_ignition/robots/sim/gazebo/cartpole.py deleted file mode 100644 index 2c72aefec..000000000 --- a/gym_ignition/robots/sim/gazebo/cartpole.py +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -import gym_ignition_models -from gym_ignition.robots import gazebo_robot - - -class CartPoleGazeboRobot(gazebo_robot.GazeboRobot): - - def __init__(self, gazebo, model_file: str = None, **kwargs): - - # Get the model - if model_file is None: - model_file = gym_ignition_models.get_model_file("cartpole") - - # Initialize base class - super().__init__(model_file=model_file, - gazebo=gazebo, - controller_rate=kwargs.get("controller_rate")) - - # Initial base position - base_position = np.array([0., 0., 0.]) \ - if "base_position" not in kwargs else kwargs["base_position"] - - # Initial base orientation - base_orientation = np.array([1., 0., 0., 0.]) \ - if "base_orientation" not in kwargs else kwargs["base_orientation"] - - # Set the base pose - ok_base_pose = self.set_initial_base_pose(base_position, base_orientation) - assert ok_base_pose, "Failed to set base pose" - - # Insert the model in the simulation - _ = self.gympp_robot diff --git a/gym_ignition/robots/sim/gazebo/icub.py b/gym_ignition/robots/sim/gazebo/icub.py deleted file mode 100644 index 263e58497..000000000 --- a/gym_ignition/robots/sim/gazebo/icub.py +++ /dev/null @@ -1,5 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# TODO diff --git a/gym_ignition/robots/sim/gazebo/panda.py b/gym_ignition/robots/sim/gazebo/panda.py deleted file mode 100644 index 591cb95b2..000000000 --- a/gym_ignition/robots/sim/gazebo/panda.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -import gym_ignition_models -from gym_ignition.robots import gazebo_robot -from gym_ignition.base.robot import robot_joints - - -class PandaRobot(gazebo_robot.GazeboRobot): - def __init__(self, gazebo, model_file: str = None, **kwargs): - - # Get the model - if model_file is None: - model_file = gym_ignition_models.get_model_file("panda") - - # Initialize base class - super().__init__(model_file=model_file, - gazebo=gazebo, - controller_rate=kwargs.get("controller_rate")) - - base_position = np.array([0., 0., 0.]) \ - if "base_position" not in kwargs else kwargs["base_position"] - - base_orientation = np.array([1., 0., 0., 0.]) \ - if "base_orientation" not in kwargs else kwargs["base_orientation"] - - ok_base_pose = self.set_initial_base_pose(base_position, base_orientation) - assert ok_base_pose, "Failed to set initial base pose" - - # Insert the model in the simulation - _ = self.gympp_robot - - # From: - # https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml - pid_gains_1000hz = { - 'panda_joint1': robot_joints.PID(p=50, i=0, d=20), - 'panda_joint2': robot_joints.PID(p=10000, i=0, d=500), - 'panda_joint3': robot_joints.PID(p=100, i=0, d=10), - 'panda_joint4': robot_joints.PID(p=1000, i=0, d=50), - 'panda_joint5': robot_joints.PID(p=100, i=0, d=10), - 'panda_joint6': robot_joints.PID(p=100, i=0, d=10), - 'panda_joint7': robot_joints.PID(p=10, i=0.5, d=0.1), - 'panda_finger_joint1': robot_joints.PID(p=100, i=0, d=50), - 'panda_finger_joint2': robot_joints.PID(p=100, i=0, d=50), - } - - assert set(self.joint_names()) == set(pid_gains_1000hz.keys()) - - for joint_name, pid in pid_gains_1000hz.items(): - ok_pid = self.set_joint_pid(joint_name=joint_name, pid=pid) - assert ok_pid diff --git a/gym_ignition/robots/sim/gazebo/pendulum.py b/gym_ignition/robots/sim/gazebo/pendulum.py deleted file mode 100644 index 7665ffe21..000000000 --- a/gym_ignition/robots/sim/gazebo/pendulum.py +++ /dev/null @@ -1,40 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -import gym_ignition_models -from gym_ignition.robots import gazebo_robot - - -class PendulumGazeboRobot(gazebo_robot.GazeboRobot): - - def __init__(self, gazebo, model_file: str = None, **kwargs): - - # Get the model - if model_file is None: - model_file = gym_ignition_models.get_model_file("pendulum") - - # Initialize base class - super().__init__(model_file=model_file, - gazebo=gazebo, - controller_rate=kwargs.get("controller_rate")) - - # Configure the pendulum as fixed-base robot - ok_floating = self.set_as_floating_base(False) - assert ok_floating, "Failed to set the robot as fixed base" - - # Initial base position - base_position = np.array([0., 0., 0.]) \ - if "base_position" not in kwargs else kwargs["base_position"] - - # Initial base orientation - base_orientation = np.array([1., 0., 0., 0.]) \ - if "base_orientation" not in kwargs else kwargs["base_orientation"] - - # Set the base pose - ok_base_pose = self.set_initial_base_pose(base_position, base_orientation) - assert ok_base_pose, "Failed to set base pose" - - # Insert the model in the simulation - _ = self.gympp_robot diff --git a/gym_ignition/robots/sim/pybullet/__init__.py b/gym_ignition/robots/sim/pybullet/__init__.py deleted file mode 100644 index 0fcd575be..000000000 --- a/gym_ignition/robots/sim/pybullet/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -from . import icub -from . import pendulum -from . import cartpole diff --git a/gym_ignition/robots/sim/pybullet/cartpole.py b/gym_ignition/robots/sim/pybullet/cartpole.py deleted file mode 100644 index db98867c0..000000000 --- a/gym_ignition/robots/sim/pybullet/cartpole.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -import gym_ignition_models -from gym_ignition.robots import pybullet_robot - - -class CartPolePyBulletRobot(pybullet_robot.PyBulletRobot): - - def __init__(self, p, plane_id: int, model_file: str = None, **kwargs): - - # Get the model - if model_file is None: - model_file = gym_ignition_models.get_model_file("pendulum") - - # Initialize base class - super().__init__( - p=p, - model_file=model_file, - plane_id=plane_id, - keep_fixed_joints=False) - - # Configure the pendulum as fixed-base robot - ok_floating = self.set_as_floating_base(False) - assert ok_floating, "Failed to set the robot as fixed base" - - # Initial base position - base_position = np.array([0., 0., 0.]) \ - if "base_position" not in kwargs else kwargs["base_position"] - - # Initial base orientation - base_orientation = np.array([1., 0., 0., 0.]) \ - if "base_orientation" not in kwargs else kwargs["base_orientation"] - - # Set the base pose - ok_base_pose = self.set_initial_base_pose(base_position, base_orientation) - assert ok_base_pose, "Failed to set base pose" - - # Insert the model in the simulation - self.initialize_model() diff --git a/gym_ignition/robots/sim/pybullet/icub.py b/gym_ignition/robots/sim/pybullet/icub.py deleted file mode 100644 index 86f016a83..000000000 --- a/gym_ignition/robots/sim/pybullet/icub.py +++ /dev/null @@ -1,22 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -from gym_ignition.robots import pybullet_robot - - -class ICubPyBulletRobot(pybullet_robot.PyBulletRobot): - def __init__(self, p, model_file: str, plane_id: int, **kwargs): - # Initialize base class - super().__init__( - p=p, - model_file=model_file, - plane_id=plane_id, - keep_fixed_joints=False) - - # Set the base frame - ok_base_frame = self.set_base_frame("root_link") - assert ok_base_frame, "Failed to set base frame" - - # Insert the model in the simulation - self.initialize_model() diff --git a/gym_ignition/robots/sim/pybullet/pendulum.py b/gym_ignition/robots/sim/pybullet/pendulum.py deleted file mode 100644 index 16da99fdf..000000000 --- a/gym_ignition/robots/sim/pybullet/pendulum.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import numpy as np -import gym_ignition_models -from gym_ignition.robots import pybullet_robot - - -class PendulumPyBulletRobot(pybullet_robot.PyBulletRobot): - - def __init__(self, p, plane_id: int, model_file: str = None, **kwargs): - - # Get the model - if model_file is None: - model_file = gym_ignition_models.get_model_file("pendulum") - - # Initialize base class - super().__init__( - p=p, - model_file=model_file, - plane_id=plane_id, - keep_fixed_joints=False) - - # Configure the pendulum as fixed-base robot - ok_floating = self.set_as_floating_base(False) - assert ok_floating, "Failed to set the robot as fixed base" - - # Initial base position - base_position = np.array([0., 0., 0.3]) \ - if "base_position" not in kwargs else kwargs["base_position"] - - # Initial base orientation - base_orientation = np.array([1., 0., 0., 0.]) \ - if "base_orientation" not in kwargs else kwargs["base_orientation"] - - # Set the base pose - ok_base_pose = self.set_initial_base_pose(base_position, base_orientation) - assert ok_base_pose, "Failed to set base pose" - - # Insert the model in the simulation - self.initialize_model() From d111c9ad684be03b1fa5e4b3db646dc964ade8be Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:32:20 +0200 Subject: [PATCH 05/35] Move tasks to the new gym_ignition_environments package --- gym_ignition/__init__.py | 5 ----- .../tasks/__init__.py | 0 .../tasks/cartpole_continuous.py | 0 .../tasks/cartpole_discrete.py | 0 .../tasks/pendulum_swingup.py | 0 5 files changed, 5 deletions(-) rename {gym_ignition => gym_ignition_environments}/tasks/__init__.py (100%) rename {gym_ignition => gym_ignition_environments}/tasks/cartpole_continuous.py (100%) rename {gym_ignition => gym_ignition_environments}/tasks/cartpole_discrete.py (100%) rename {gym_ignition => gym_ignition_environments}/tasks/pendulum_swingup.py (100%) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 00afbe553..fb2fed502 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -34,11 +34,6 @@ from gym.envs.registration import register from gym_ignition.utils import resource_finder -# Import the tasks -from gym_ignition.tasks import pendulum_swingup -from gym_ignition.tasks import cartpole_discrete -from gym_ignition.tasks import cartpole_continuous - # ====================== # GYMPP C++ ENVIRONMENTS # ====================== diff --git a/gym_ignition/tasks/__init__.py b/gym_ignition_environments/tasks/__init__.py similarity index 100% rename from gym_ignition/tasks/__init__.py rename to gym_ignition_environments/tasks/__init__.py diff --git a/gym_ignition/tasks/cartpole_continuous.py b/gym_ignition_environments/tasks/cartpole_continuous.py similarity index 100% rename from gym_ignition/tasks/cartpole_continuous.py rename to gym_ignition_environments/tasks/cartpole_continuous.py diff --git a/gym_ignition/tasks/cartpole_discrete.py b/gym_ignition_environments/tasks/cartpole_discrete.py similarity index 100% rename from gym_ignition/tasks/cartpole_discrete.py rename to gym_ignition_environments/tasks/cartpole_discrete.py diff --git a/gym_ignition/tasks/pendulum_swingup.py b/gym_ignition_environments/tasks/pendulum_swingup.py similarity index 100% rename from gym_ignition/tasks/pendulum_swingup.py rename to gym_ignition_environments/tasks/pendulum_swingup.py From 7658ea3e3aca5240f6939c406f800b1e0493f782 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:34:45 +0200 Subject: [PATCH 06/35] Remove deprecated pybullet runtime It will be replace by the new bullet plugin of ignition physics --- gym_ignition/runtimes/__init__.py | 1 - gym_ignition/runtimes/pybullet_runtime.py | 333 ---------------------- 2 files changed, 334 deletions(-) delete mode 100644 gym_ignition/runtimes/pybullet_runtime.py diff --git a/gym_ignition/runtimes/__init__.py b/gym_ignition/runtimes/__init__.py index c06ee9478..7c1c57004 100644 --- a/gym_ignition/runtimes/__init__.py +++ b/gym_ignition/runtimes/__init__.py @@ -4,4 +4,3 @@ from . import gazebo_runtime from . import realtime_runtime -from . import pybullet_runtime diff --git a/gym_ignition/runtimes/pybullet_runtime.py b/gym_ignition/runtimes/pybullet_runtime.py deleted file mode 100644 index 9246ebee0..000000000 --- a/gym_ignition/runtimes/pybullet_runtime.py +++ /dev/null @@ -1,333 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import os -import time -import numpy as np -from gym_ignition import base, robots -from gym_ignition.base.robot import robot_abc -from gym_ignition.utils import logger, resource_finder -from gym_ignition.utils.typing import State, Action, Observation, SeedList - - -class PyBulletRuntime(base.runtime.Runtime): - metadata = {'render.modes': ['human']} - - def __init__(self, - task_cls: type, - robot_cls: type, - rtf: float, - agent_rate: float, - physics_rate: float, - model: str = None, - world: str = "plane_implicit.urdf", - **kwargs): - - # Save the keyworded arguments. - # We use them to build the task and the robot objects, and allow custom class - # to accept user-defined parameters. - self._kwargs = kwargs - - # Store the type of the class that provides Robot interface - self._robot_cls = robot_cls - - # Marks the first execution - self._first_run = True - - # URDF or SDF model files - self._world = world - self._model = model - - # Simulation attributes - self._rtf = rtf - self._now = None - self._bias = 0.0 - self._timestamp = 0.0 - - self._physics_rate = physics_rate - - self._plane_id = None - self._pybullet = None - self._render_enabled = False - self._render_called = False - - # Calculate the rate between agent and physics rate - physics_steps_per_agent_update = physics_rate / agent_rate - self._num_of_physics_steps = int(physics_steps_per_agent_update) - - assert physics_rate >= agent_rate, "Agent cannot run faster than physics" - - # If there is an incompatibility between the agent and physics rates, round the - # iterations and notify the user - if physics_steps_per_agent_update != round(physics_steps_per_agent_update): - self._num_of_physics_steps = round(physics_steps_per_agent_update) - logger.warn("The real physics rate will be {} Hz".format( - agent_rate * self._num_of_physics_steps)) - - logger.debug(f"RTF = {rtf}") - logger.debug(f"Agent rate = {agent_rate} Hz") - logger.debug(f"Physics rate = {agent_rate * self._num_of_physics_steps} Hz") - - logger.debug("Initializing the Task") - task = task_cls(agent_rate=agent_rate, **kwargs) - - assert isinstance(task, base.task.Task), \ - "'task_cls' object must inherit from Task" - - # Wrap the task with this runtime - super().__init__(task=task, agent_rate=agent_rate) - - # Initialize the simulator and the robot - self.task.robot = self._get_robot() - - # Initialize the spaces - self.task.action_space, self.task.observation_space = self.task.create_spaces() - - # Seed the environment - self.seed() - - # ======================= - # PyBulletRuntime METHODS - # ======================= - - @property - def pybullet(self): - if self._pybullet is not None: - return self._pybullet - - logger.debug("Creating PyBullet simulator") - - if self._render_enabled: - import pybullet - from pybullet_utils import bullet_client - self._pybullet = bullet_client.BulletClient(pybullet.GUI) - else: - # Connects to an existing instance or, if it fails, creates an headless - # simulation (DIRECT) - from pybullet_utils import bullet_client - self._pybullet = bullet_client.BulletClient() - - assert self._pybullet, "Failed to create the bullet client" - - # Find the ground plane - import pybullet_data - resource_finder.add_path(pybullet_data.getDataPath()) - world_abs_path = resource_finder.find_resource(self._world) - - # Load the ground plane - self._plane_id = self._load_model(world_abs_path) - - # Configure the physics engine - self._pybullet.setGravity(0, 0, -9.81) - self._pybullet.setPhysicsEngineParameter(numSolverIterations=10) - - # Configure the physics engine with a single big time step divided in multiple - # substeps. As an alternative, we could use a single substep and step the - # simulation multiple times. - self._pybullet.setTimeStep(1.0 / self._physics_rate / self._num_of_physics_steps) - self._pybullet.setPhysicsEngineParameter(numSubSteps=self._num_of_physics_steps) - - # Disable real-time update. We step the simulation when needed. - self._pybullet.setRealTimeSimulation(0) - - logger.info("PyBullet Physic Engine Parameters:") - logger.info(str(self._pybullet.getPhysicsEngineParameters())) - - step_time = 1.0 / self._physics_rate / self._rtf - logger.info(f"Nominal step time: {step_time} seconds") - - logger.debug("PyBullet simulator created") - return self._pybullet - - def _get_robot(self) -> robot_abc.RobotABC: - if not self.pybullet: - raise Exception("Failed to instantiate the pybullet simulator") - - if not issubclass(self._robot_cls, robots.pybullet_robot.PyBulletRobot): - raise Exception("The 'robot_cls' must inherit from PyBulletRobot") - - # Build the robot object - logger.debug("Creating the robot object") - robot = self._robot_cls(plane_id=self._plane_id, - model_file=self._model, - p=self._pybullet, - **self._kwargs) - - if self.task.robot_features: - self.task.robot_features.has_all_features(robot) - - if not robot.valid(): - raise Exception("The robot is not valid") - - logger.debug("Robot object created") - return robot - - def _load_model(self, filename: str, **kwargs) -> int: - logger.debug(f"Loading model {filename}") - - # Get the file extension - extension = os.path.splitext(filename)[1][1:] - - if extension == "sdf": - model_id = self._pybullet.loadSDF(filename, **kwargs)[0] - else: - import pybullet - model_id = self._pybullet.loadURDF( - filename, - flags=pybullet.URDF_USE_INERTIA_FROM_FILE, - **kwargs) - - return model_id - - def _enforce_rtf(self): - if self._now is not None: - # Compute the actual elapsed time from last cycle - elapsed = time.time() - self._now - - # Nominal timestep with the desired RTF - expected_dt = 1.0 / self.agent_rate / self._rtf - - # Compute the amount of sleep time to match the desired RTF - sleep_amount = expected_dt - elapsed - self._bias - - # Sleep if needed - real_sleep = 0 - if sleep_amount > 0: - now = time.time() - time.sleep(sleep_amount) - real_sleep = time.time() - now - - # We use a low-filtered bias to compensate delays due to code running outside - # the step method - self._bias = \ - 0.01 * (real_sleep - np.max([sleep_amount, 0.0])) + \ - 0.99 * self._bias - - # Update the time for the next cycle - self._now = time.time() - - # ================= - # Runtime interface - # ================= - - def timestamp(self) -> float: - return self._timestamp - - # =============== - # gym.Env METHODS - # =============== - - def step(self, action: Action) -> State: - if not self.action_space.contains(action): - logger.warn("The action does not belong to the action space") - - # Update the timestamp - self._timestamp += 1.0 / self.agent_rate - - # Set the action - ok_action = self.task.set_action(action) - assert ok_action, "Failed to set the action" - - # Step the simulator - self.pybullet.stepSimulation() - - # Get the observation - observation = self.task.get_observation() - - if not self.observation_space.contains(observation): - logger.warn("The observation does not belong to the observation space") - - # Get the reward - reward = self.task.get_reward() - assert reward is not None, "Failed to get the reward" - - # Check termination - done = self.task.is_done() - - # Enforce the real-time factor - self._enforce_rtf() - - return State((observation, reward, done, {})) - - def reset(self) -> Observation: - # Initialize pybullet if not yet done - p = self.pybullet - assert p, "PyBullet object not valid" - - # Remove the robot and insert a new one - if not self._first_run: - logger.debug("Hard reset: deleting the robot") - self.task.robot.delete_simulated_robot() - - # Gazebo needs a dummy step to process model removal. - # This line unifies the behaviour of the simulators. - p.stepSimulation() - - logger.debug("Hard reset: creating new robot") - self.task.robot = self._get_robot() - else: - self._first_run = False - - # Reset the environment - ok_reset = self.task.reset_task() - assert ok_reset, "Failed to reset the task" - - # Get the observation - observation = self.task.get_observation() - - if not self.observation_space.contains(observation): - logger.warn("The observation does not belong to the observation space") - - # Reset the timestamp - self._timestamp = 0.0 - - return Observation(observation) - - def render(self, mode: str = 'human', **kwargs) -> None: - if mode != "human": - raise Exception(f"Render mode '{mode}' not yet supported") - - # If render has been already called once, and the simulator is ok, return - if self._render_enabled and self._pybullet: - return - - # Enable rendering - self._render_enabled = True - - # If the simulator is already allocated (in DIRECT mode, headless), we have to - # disconnect it, starting it in GUI mode, and recreate the robot object. - if self._pybullet is not None: - logger.warn( - "The simulator was allocated in DIRECT mode before calling render. " - "The simulation has been reset. All changes to the simulation state " - "will be lost.") - - # Disconnect the simulator and deletes the robot object - self.close() - - # Create a new simulator and robot object - self.task._robot = self._get_robot() - - def close(self) -> None: - # Disconnect the simulator - self._pybullet = None - - # Delete the outdated robot object. - # It was connected to the deleted simulator instance. - if self.task._robot: - self.task._robot = None - - def seed(self, seed: int = None) -> SeedList: - # This method also seeds the spaces. To create them, the robot object is often - # needed. Here we check that is has been created. - assert self.task.has_robot(), "The robot has not yet been created" - - # Seed the wrapped environment (task) - seed = self.env.seed(seed) - - # Update the spaces of the wrapper - self.action_space = self.task.action_space - self.observation_space = self.task.observation_space - - return seed From 5f802f0aec984623a8d7f71b2687c0d83c2712aa Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:36:07 +0200 Subject: [PATCH 07/35] Remove pybullet envs --- gym_ignition/__init__.py | 32 -------------------------------- 1 file changed, 32 deletions(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index fb2fed502..2fa3ddb5d 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -88,35 +88,3 @@ 'agent_rate': 1000, 'physics_rate': 1000, }) - -# ===================== -# PYBULLET ENVIRONMENTS -# ===================== - -register( - id='Pendulum-PyBullet-v0', - entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', - max_episode_steps=5000, - kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, - 'robot_cls': sim.pybullet.pendulum.PendulumPyBulletRobot, - 'model': "Pendulum/Pendulum.urdf", - 'world': "plane_implicit.urdf", - 'rtf': max_float, - 'agent_rate': 1000, - 'physics_rate': 1000, - }) - -register( - id='CartPoleDiscrete-PyBullet-v0', - entry_point='gym_ignition.runtimes.pybullet_runtime:PyBulletRuntime', - max_episode_steps=5000, - kwargs={ - # PyBulletRuntime - 'task_cls': cartpole_discrete.CartPoleDiscrete, - 'robot_cls': sim.pybullet.cartpole.CartPolePyBulletRobot, - 'model': "CartPole/CartPole.urdf", - 'world': "plane_implicit.urdf", - 'rtf': max_float, - 'agent_rate': 1000, - 'physics_rate': 1000, - }) From 146e397012d421ed4c6af02ee15cc7315c0375c4 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 31 Mar 2020 18:40:09 +0200 Subject: [PATCH 08/35] Move environments to gym_ignition_environments TODO: add the envs there --- gym_ignition/__init__.py | 69 +++------------------------------------- 1 file changed, 4 insertions(+), 65 deletions(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 2fa3ddb5d..f821c61c9 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -21,70 +21,9 @@ else: import gympp_bindings + # try: + # import gympp_bindings + # except: + # pass -# Configure OS environment variables -from gym_ignition.utils import gazebo_env_vars, resource_finder -gazebo_env_vars.setup_gazebo_env_vars() -resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") -# ========================= -# REGISTER THE ENVIRONMENTS -# ========================= - -from gym.envs.registration import register -from gym_ignition.utils import resource_finder - -# ====================== -# GYMPP C++ ENVIRONMENTS -# ====================== - -import numpy -max_float = float(numpy.finfo(numpy.float32).max) - -register( - id='CartPoleDiscrete-Gympp-v0', - max_episode_steps=5000, - entry_point='gym_ignition.gympp.cartpole:CartPoleDiscrete') - -# ============================ -# IGNITION GAZEBO ENVIRONMENTS -# ============================ - -register( - id='Pendulum-Gazebo-v0', - entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', - max_episode_steps=5000, - kwargs={'task_cls': pendulum_swingup.PendulumSwingUp, - 'robot_cls': sim.gazebo.pendulum.PendulumGazeboRobot, - 'model': "Pendulum/Pendulum.urdf", - 'world': "DefaultEmptyWorld.world", - 'rtf': max_float, - 'agent_rate': 1000, - 'physics_rate': 1000, - }) - -register( - id='CartPoleDiscrete-Gazebo-v0', - entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', - max_episode_steps=5000, - kwargs={'task_cls': cartpole_discrete.CartPoleDiscrete, - 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, - 'model': "CartPole/CartPole.urdf", - 'world': "DefaultEmptyWorld.world", - 'rtf': max_float, - 'agent_rate': 1000, - 'physics_rate': 1000, - }) - -register( - id='CartPoleContinuous-Gazebo-v0', - entry_point='gym_ignition.runtimes.gazebo_runtime:GazeboRuntime', - max_episode_steps=5000, - kwargs={'task_cls': cartpole_continuous.CartPoleContinuous, - 'robot_cls': sim.gazebo.cartpole.CartPoleGazeboRobot, - 'model': "CartPole/CartPole.urdf", - 'world': "DefaultEmptyWorld.world", - 'rtf': max_float, - 'agent_rate': 1000, - 'physics_rate': 1000, - }) From 511a4c0260179a1d7c071a5adb32afa05b03a38f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 9 Apr 2020 17:04:25 +0200 Subject: [PATCH 09/35] Move gympp_env to experimental --- gym_ignition/base/__init__.py | 2 -- gym_ignition/{base => experimental}/gympp_env.py | 0 2 files changed, 2 deletions(-) rename gym_ignition/{base => experimental}/gympp_env.py (100%) diff --git a/gym_ignition/base/__init__.py b/gym_ignition/base/__init__.py index a1842f008..3e00d270b 100644 --- a/gym_ignition/base/__init__.py +++ b/gym_ignition/base/__init__.py @@ -6,5 +6,3 @@ from . import task from . import runtime -# Base C++ environment -from . import gympp_env diff --git a/gym_ignition/base/gympp_env.py b/gym_ignition/experimental/gympp_env.py similarity index 100% rename from gym_ignition/base/gympp_env.py rename to gym_ignition/experimental/gympp_env.py From e49366dbda67b25b66fbffddbd27293a71c588aa Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:34:23 +0200 Subject: [PATCH 10/35] Update base.task.Task --- gym_ignition/base/task.py | 190 +++++++++++++++++++++++--------------- 1 file changed, 115 insertions(+), 75 deletions(-) diff --git a/gym_ignition/base/task.py b/gym_ignition/base/task.py index 3a3dc12c1..8737b9b06 100644 --- a/gym_ignition/base/task.py +++ b/gym_ignition/base/task.py @@ -7,79 +7,99 @@ import numpy as np from typing import Tuple from gym.utils import seeding -from gym_ignition.base.robot import robot_abc, RobotFeatures +from gym_ignition import scenario_bindings as bindings from gym_ignition.utils.typing import ActionSpace, ObservationSpace from gym_ignition.utils.typing import Action, Observation, Reward, SeedList -class Task(gym.Env, abc.ABC): +class Task(abc.ABC): """ Interface to define a decision-making task. - The Task is the central interface of each environment. + The Task is the central interface of each environment implementation. It defines the logic of the environment in a format that is agnostic of both the - runtime (either simulated or real-time) and the robot. - - Beyond containing the logic of the task, objects that inherit from this interface - expose an empty gym.Env interface. This is required because the real environment - exposed to the agent consists of a Task wrapped by a runtime wrapper, that inherits - from gym.Wrapper. Runtime wrappers implement the gym.Env interface, and particularly - gym.Env.step. This method, depending on the runtime, can interface with a physics - engine and step the simulator, or can handle real-time execution. - - In order to make Task objects generic from the runtime, also the interfacing with the - robot needs to be abstracted. In fact, the access of robot data depends on the - selected runtime. For example, in simulation data can be directly gathered from the - physics engine, and in a real-time setting can be asked through a robotic middleware. - Tasks abstract this interfacing by operating on a Robot interface, which is then - specialized for the different runtimes. + runtime (either simulated or real-time) and the models it operates on. + + :py:class:`~gym_ignition.base.runtime.Runtime` instances are the real objects returned + to the users when they call :py:class:`gym.make`. Depending on the type of the + runtime, it could contain one or more :py:class:`Task` objects. + The :py:class:`~gym_ignition.base.runtime.Runtime` is a relay class that calls the + logic of the :py:class:`Task` from its interface methods and implements the real + :py:meth:`gym.Env.step`. + In simulated runtimes, this method will step the physics engine, instead in real-time + runtimes, it will enforce real-time execution. + + A :py:class:`Task` object is meant to be: + + - Independent from the selected :py:class:`~gym_ignition.base.runtime.Runtime`. + In fact, it defines only the decision making logic; + - Independent from the :py:class:`~scenario_bindings.Model` objects it operates on. + This is achieved thanks to the model abstraction provided by + :cpp:class:`scenario::gazebo::Model`. + + The population of the world where the task operates is demanded to a + :py:class:`gym.Wrapper` object. """ + action_space: gym.spaces.Space = None + observation_space: gym.spaces.Space = None + def __init__(self, agent_rate: float) -> None: - # Robot object associated with the task - self._robot = None - # Rate of the agent, that matches the rate at which the Gym methods are called + # World object + self._world = None + + #: Rate of the agent. + #: It matches the rate at which the :py:class:`Gym.Env` methods are called. self.agent_rate = agent_rate - # Random Number Generator - self.np_random, self._seed = seeding.np_random() + #: RNG available to the object to ensure reproducibility. + #: Use it for all the random resources. + self.np_random: np.random.RandomState - # Optional public attribute to check robot features - self.robot_features = None + # Initialize the RNG and the seed + self.np_random, self._seed = seeding.np_random() # ========== # PROPERTIES # ========== @property - def robot(self) -> RobotFeatures: - if self._robot: - assert self._robot.valid(), "The robot interface is not valid" - return self._robot + def world(self) -> bindings.World: + """ + Get the world where the task is operating. - raise Exception("The robot interface object was never stored") + Returns: + The world object. + """ - @robot.setter - def robot(self, robot: robot_abc.RobotABC) -> None: - if not robot.valid(): - raise Exception("Robot object is not valid") + if self._world: + assert self._world.id() != 0, "The world is not valid" + return self._world - if self.robot_features is not None: - self.robot_features.has_all_features(robot) + raise Exception("The world was never stored") - # Set the robot - self._robot = robot + @world.setter + def world(self, world: bindings.World) -> None: + + if world.id() == 0: + raise Exception("World not valid") + + # Store the world + self._world = world + + def has_world(self) -> bool: + """ + Check if the world was stored. + + Returns: + True if the task has a valid world, False otherwise. + """ - def has_robot(self) -> bool: - if self._robot is None: - return False - else: - assert self._robot.valid(), "The robot object is not valid" - return True + return self._world is not None and self._world.id() != 0 # ============== - # TASK INTERFACE + # Task Interface # ============== @abc.abstractmethod @@ -87,35 +107,47 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: """ Create the action and observations spaces. + Note: + This method does not currently have access to the Models part of the + environment. If the Task is meant to work on different models, we recommend + using their URDF / SDF model to extract the information you need + (e.g. number of DoFs, joint position limits, etc). Since actions and + observations are often normalized, in many cases there's no need to extract + a lot of information from the model file. + + Raises: + RuntimeError: In case of failure. + Returns: A tuple containing the action and observation spaces. """ @abc.abstractmethod - def reset_task(self) -> bool: + def reset_task(self) -> None: """ Reset the task. - This method contains the logic of resetting the environment. - It is called in the gym.Env.reset method. + This method contains the logic for resetting the task. + It is called in the :py:meth:`gym.Env.reset` method of the corresponding + environment. - Returns: - True if successful, False otherwise. + Raises: + RuntimeError: In case of failure. """ @abc.abstractmethod - def set_action(self, action: Action) -> bool: + def set_action(self, action: Action) -> None: """ Set the task action. - This method contains the logic of setting the environment action. - It is called in the beginning of the gym.Env.step method. + This method contains the logic for setting the environment action. + It is called in the beginning of the :py:meth:`gym.Env.step` method. Args: action: The action to set. - Returns: - True if successful, False otherwise. + Raises: + RuntimeError: In case of failure. """ @abc.abstractmethod @@ -123,8 +155,12 @@ def get_observation(self) -> Observation: """ Return the task observation. - This method contains the logic of constructing the environment observation. - It is called in the end of both gym.Env.reset and gym.Env.step methods. + This method contains the logic for constructing the environment observation. + It is called in the end of both :py:meth:`gym.Env.reset` and + :py:meth:`gym.Env.step` methods. + + Raises: + RuntimeError: In case of failure. Returns: The task observation. @@ -135,8 +171,11 @@ def get_reward(self) -> Reward: """ Return the task reward. - This method contains the logic of computing the environment reward. - It is called in the end of the gym.Env.step method. + This method contains the logic for computing the environment reward. + It is called in the end of the :py:meth:`gym.Env.step` method. + + Raises: + RuntimeError: In case of failure. Returns: The scalar reward. @@ -147,31 +186,32 @@ def is_done(self) -> bool: """ Returns the task termination flag. - This method contains the logic of computing when the environment is terminated. - Subsequent actions should be preceded by an environment reset. - It is called in the end of the gym.Env.step method. + This method contains the logic for defining when the environment has terminated. + Subsequent calls to :py:meth:`Task.set_action` should be preceded by a task + reset through :py:meth:`Task.reset_task`. + + It is called in the end of the :py:meth:`gym.Env.step` method. + + Raises: + RuntimeError: In case of failure. Returns: True if the environment terminated, False otherwise. """ - # ================= - # gym.Env INTERFACE - # ================= - - def step(self, action): - raise NotImplementedError + def seed_task(self, seed: int = None) -> SeedList: + """ + Seed the task. - def reset(self,): - raise NotImplementedError + This method configures the :py:attr:`Task.np_random` RNG. - def render(self, mode='human'): - raise NotImplementedError + Args: + seed: The seed number. - def close(self): - raise NotImplementedError + Return: + The list of seeds used by the task. + """ - def seed(self, seed: int = None) -> SeedList: # Create the seed if not passed self._seed = np.random.randint(2**32 - 1) if seed is None else seed From afe72d5e2647c15be9bceae808b59bf754b3b4c6 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:25:03 +0200 Subject: [PATCH 11/35] Update base.runtime.Runtime --- gym_ignition/base/runtime.py | 90 +++++++++++++++++++++--------------- 1 file changed, 54 insertions(+), 36 deletions(-) diff --git a/gym_ignition/base/runtime.py b/gym_ignition/base/runtime.py index b5243eabe..6ce2f73ab 100644 --- a/gym_ignition/base/runtime.py +++ b/gym_ignition/base/runtime.py @@ -4,52 +4,70 @@ import abc import gym -from gym_ignition.base.task import Task +from gym_ignition import base -class Runtime(gym.Wrapper, abc.ABC): +class Runtime(gym.Env, abc.ABC): """ - Base class for defining Task runtimes. + Base class for defining executors of :py:class:`~gym_ignition.base.task.Task` + objects. - The runtime is the executor of a Task. Tasks are supposed to be generic and are not - tied to any specific Runtime. Implementations of the Runtime class contain all the - logic to define how to execute the Task, allowing to reuse the same Task class - e.g. on different simulators or in a real-time setting. + :py:class:`~gym_ignition.base.task.Task` classes are supposed to be generic and are + not tied to any specific runtime. Implementations of a runtime class contain all the + logic to define how to execute the task, allowing to run the same + :py:class:`~gym_ignition.base.task.Task` class on different simulators or in a + real-time setting. - Runtime objects are the real environments returned to the users when they use the - gym.make factory method. Despite they inherit from gym.Wrapper, they are exposed to - the user as gym.Env objects. - """ + Runtimes are the real :py:class:`gym.Env` objects returned to the users when they call + the :py:class:`gym.make` factory method. - # Add a spec class variable to make the wrapper look like an environment. - # This class variable overrides the gym.Wrapper.spec property and enables registering - # runtime objects in the environment factory, despite they inherit from Wrapper. - spec = None + Args: + task: the :py:class:`~gym_ignition.base.task.Task` object to handle. + agent_rate: the rate at which the environment will be called. Sometimes tasks need + to know this information. - def __init__(self, task: Task, agent_rate: float): - # Initialize the gym.Wrapper class - super().__init__(env=task) + Example: - # All runtimes provide to the user the nominal rate of their execution - self.agent_rate = agent_rate + Here is minimal example of how the :py:class:`Runtime`, :py:class:`gym.Env` and + :py:class:`~gym_ignition.base.task.Task` could be integrated: - @property - def task(self): - return self.env + .. code-block:: python - @property - def unwrapped(self): - """ - Expose Runtime objects as real gym.Env objects. + class FooRuntime(Runtime): - This method disables the gym.Wrapper logic to returned the wrapped environment. - In fact, the wrapped environment is a Task which contains not a implemented - gym.Env interface. + def __init__(self, task): + super().__init__(task=task, agent_rate=agent_rate) + self.action_space, self.observation_space = self.task.create_spaces() - Returns: - The gym.Env environment of the Runtime. - """ - return self + def reset(self): + self.task.reset_task() + return self.task.get_observation() + + def step(self, action): + self.task.set_action(action) + + # [...] code that performs the real step [...] + + done = self.task.is_done() + reward = self.task.get_reward() + observation = self.task.get_observation() + + return observation, reward, done, {} + + def close(self): + pass + + Note: + Runtimes can handle only one :py:class:`~gym_ignition.base.runtime.Task` object. + """ + + def __init__(self, task: base.task.Task, agent_rate: float): + + #: Task handled by the runtime. + self.task: base.task.Task = task + + #: Rate of environment execution. + self.agent_rate = agent_rate @abc.abstractmethod def timestamp(self) -> float: @@ -58,8 +76,8 @@ def timestamp(self) -> float: In real-time environments, the timestamp is the time read from the host system. In simulated environments, the timestamp is the simulated time, which might not - match the real-time in the case of RTF different than 1. + match the real-time in the case of a real-time factor different than 1. Returns: - A float indicating the current environment timestamp + The current environment timestamp. """ From 70404c1791448bd2cb101ff17aa42ae75a2325dc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:31:41 +0200 Subject: [PATCH 12/35] Update runtimes.gazebo_runtime --- gym_ignition/runtimes/gazebo_runtime.py | 334 +++++++++++++----------- 1 file changed, 184 insertions(+), 150 deletions(-) diff --git a/gym_ignition/runtimes/gazebo_runtime.py b/gym_ignition/runtimes/gazebo_runtime.py index 3d00b04bd..029c72478 100644 --- a/gym_ignition/runtimes/gazebo_runtime.py +++ b/gym_ignition/runtimes/gazebo_runtime.py @@ -2,163 +2,96 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym +import gym_ignition_models from gym_ignition import base -from gym_ignition.utils import logger from gym_ignition.base import runtime -from gym_ignition.base.robot import robot_abc -from gym_ignition import gympp_bindings as bindings -from gym_ignition.utils.typing import State, Action, Observation, SeedList +from gym_ignition.utils import logger +from gym_ignition.utils.typing import * +from gym_ignition.utils import scenario +from gym_ignition import scenario_bindings as bindings class GazeboRuntime(runtime.Runtime): + """ + Implementation of :py:class:`~gym_ignition.base.runtime.Runtime` for the Ignition + Gazebo simulator. + + Args: + task_cls: The class of the handled task. + agent_rate: The rate at which the environment is called. + physics_rate: The rate of the physics engine. + real_time_factor: The desired RTF of the simulation. + world (optional): The path to an SDF world file. The world should not contain any + physics plugin. + """ + metadata = {'render.modes': ['human']} def __init__(self, task_cls: type, - robot_cls: type, - rtf: float, agent_rate: float, physics_rate: float, - model: str = None, - world: str = "DefaultEmptyWorld.world", + real_time_factor: float, + world: str = None, **kwargs): - # Save the keyworded arguments. - # We use them to build the task and the robot objects, and allow custom class - # to accept user-defined parameters. - self._kwargs = kwargs - - # Store the type of the class that provides RobotABC interface - self._robot_cls = robot_cls - # Delete and create a new robot every environment reset self._first_run = True - # SDF files - self._model = model - self._world = world - - # Gazebo simulation attributes - self._rtf = rtf + # Gazebo attributes + self._gazebo = None self._physics_rate = physics_rate - self._gazebo_wrapper = None + self._real_time_factor = real_time_factor + + # World attributes + self._world = None + self._world_sdf = world + self._world_name = None # Create the Task object task = task_cls(agent_rate=agent_rate, **kwargs) - assert isinstance(task, base.task.Task), \ - "'task_cls' object must inherit from Task" + if not isinstance(task, base.task.Task): + raise RuntimeError("The task is not compatible with the runtime") - # Wrap the environment with this class + # Wrap the task with the runtime super().__init__(task=task, agent_rate=agent_rate) - # Initialize the simulator and the robot - self.task.robot = self._get_robot() + # Trigger the initialization of the simulator and the world + _ = self.gazebo + + # Store the world in the task + self.task.world = self.world # Initialize the spaces - self.task.action_space, self.task.observation_space = self.task.create_spaces() + self.action_space, self.observation_space = self.task.create_spaces() + + # Store the spaces also in the task + self.task.action_space = self.action_space + self.task.observation_space = self.observation_space # Seed the environment self.seed() - # ========== - # PROPERTIES - # ========== - - @property - def gazebo(self) -> bindings.GazeboSimulator: - if self._gazebo_wrapper: - assert self._gazebo_wrapper.getPhysicsData().rtf == self._rtf, \ - "The RTF of gazebo does not match the configuration" - assert 1 / self._gazebo_wrapper.getPhysicsData().maxStepSize == \ - self._physics_rate, \ - "The physics rate of gazebo does not match the configuration" - - return self._gazebo_wrapper - - # ================= - # INITIALIZE GAZEBO - # ================= - - assert self._rtf, "Real Time Factor was not set" - assert self.agent_rate, "Agent rate was not set" - assert self._physics_rate, "Physics rate was not set" - - logger.debug("Starting gazebo") - logger.debug(f"Agent rate: {self.agent_rate} Hz") - logger.debug(f"Physics rate: {self._physics_rate} Hz") - logger.debug(f"Simulation RTF: {self._rtf}") - - # Compute the number of physics iteration to execute at every environment step - num_of_iterations_per_gazebo_step = self._physics_rate / self.agent_rate - - if num_of_iterations_per_gazebo_step != int(num_of_iterations_per_gazebo_step): - logger.warn("Rounding the number of iterations to {} from the nominal {}" - .format(int(num_of_iterations_per_gazebo_step), - num_of_iterations_per_gazebo_step)) - else: - logger.debug("Setting {} iteration per simulator step" - .format(int(num_of_iterations_per_gazebo_step))) - - # Create the GazeboSimulator object - self._gazebo_wrapper = bindings.GazeboSimulator( - int(num_of_iterations_per_gazebo_step), - self._rtf, - self._physics_rate) - - # Set the verbosity - logger.set_level(gym.logger.MIN_LEVEL) - - # Initialize the world - world_ok = self._gazebo_wrapper.setupGazeboWorld(self._world) - assert world_ok, "Failed to initialize the gazebo world" - - # Initialize the ignition gazebo wrapper - gazebo_initialized = self._gazebo_wrapper.initialize() - assert gazebo_initialized, "Failed to initialize ignition gazebo" - - return self._gazebo_wrapper - - def _get_robot(self): - if not self.gazebo: - raise Exception("Failed to instantiate the gazebo simulator") - - # Build the robot object - logger.debug("Creating the robot object") - robot = self._robot_cls(model_file=self._model, - gazebo=self.gazebo, - **self._kwargs) - assert isinstance(robot, robot_abc.RobotABC), \ - "'robot' object must inherit from RobotABC" - - # Check the requested robot features - if self.task.robot_features: - self.task.robot_features.has_all_features(robot) - - if not robot.valid(): - raise Exception("The robot is not valid") - - return robot - # ================= # Runtime interface # ================= def timestamp(self) -> float: - return self.gazebo.getSimulatedTime() - # =============== - # gym.Env METHODS - # =============== + return self.world.time() + + # ================= + # gym.Env interface + # ================= def step(self, action: Action) -> State: + if not self.action_space.contains(action): logger.warn("The action does not belong to the action space") # Set the action - ok_action = self.task.set_action(action) - assert ok_action, "Failed to set the action" + self.task.set_action(action) # Step the simulator ok_gazebo = self.gazebo.run() @@ -166,44 +99,34 @@ def step(self, action: Action) -> State: # Get the observation observation = self.task.get_observation() + assert isinstance(observation, np.ndarray) if not self.observation_space.contains(observation): logger.warn("The observation does not belong to the observation space") # Get the reward - # TODO: use the wrapper method? reward = self.task.get_reward() - assert reward is not None, "Failed to get the reward" + assert isinstance(reward, float), "Failed to get the reward" # Check termination done = self.task.is_done() - return State((observation, reward, done, {})) + return State((Observation(observation), Reward(reward), Done(done), Info({}))) def reset(self) -> Observation: - # Initialize gazebo if not yet done - gazebo = self.gazebo - assert gazebo, "Gazebo object not valid" - # Remove the robot and insert a new one - if not self._first_run: - logger.debug("Hard reset: deleting the robot") - self.task.robot.delete_simulated_robot() + # Reset the task + self.task.reset_task() - # Execute a dummy step to process model removal - self.gazebo.run() + # Execute a paused step + ok_run = self.gazebo.run(paused=True) - logger.debug("Hard reset: creating new robot") - self.task.robot = self._get_robot() - else: - self._first_run = False - - # Reset the environment - ok_reset = self.task.reset_task() - assert ok_reset, "Failed to reset the task" + if not ok_run: + raise RuntimeError("Failed to run Gazebo") # Get the observation observation = self.task.get_observation() + assert isinstance(observation, np.ndarray) if not self.observation_space.contains(observation): logger.warn("The observation does not belong to the observation space") @@ -211,30 +134,141 @@ def reset(self) -> Observation: return Observation(observation) def render(self, mode: str = 'human', **kwargs) -> None: + if mode == 'human': - # Initialize gazebo if not yet done - gazebo = self.gazebo - assert gazebo, "Gazebo object not valid" - gui_ok = gazebo.gui() - assert gui_ok, "Failed to render the environment" + gui_ok = self.gazebo.gui() + + if not gui_ok: + raise RuntimeError("Failed to render the environment") + return - raise Exception("Render mode '{}' not supported".format(mode)) + raise Exception(f"Render mode '{mode}' not supported") def close(self) -> None: - self.gazebo.close() + + ok_close = self.gazebo.close() + + if not ok_close: + raise RuntimeError("Failed to close Gazebo") def seed(self, seed: int = None) -> SeedList: - # This method also seeds the spaces. To create them, the robot object is often - # needed. Here we check that is has been created. - assert self.task.has_robot(), "The robot has not yet been created" - # Seed the wrapped environment (task) - seed = self.env.seed(seed) + # This method also seeds the spaces. To create them, the task could use the world. + # Here we check that is has been created. + if not self.task.has_world(): + raise RuntimeError("The world has never been created") - # Update the spaces of the wrapper - self.action_space = self.env.action_space - self.observation_space = self.env.observation_space + # Seed the task (it will also seed the spaces) + seed = self.task.seed_task(seed) return seed + + # ============================== + # Properties and Private Methods + # ============================== + + @property + def gazebo(self) -> bindings.GazeboSimulator: + + if self._gazebo is not None: + assert self._gazebo.initialized() + return self._gazebo + + # Compute the number of physics iteration to execute at every environment step + num_of_steps_per_run = self._physics_rate / self.agent_rate + + if num_of_steps_per_run != int(num_of_steps_per_run): + logger.warn("Rounding the number of iterations to {} from the nominal {}" + .format(int(num_of_steps_per_run), num_of_steps_per_run)) + + # Create the simulator + gazebo = bindings.GazeboSimulator(1.0 / self._physics_rate, + self._real_time_factor, + int(num_of_steps_per_run)) + + # Store the simulator + self._gazebo = gazebo + + # Insert the world (it will initialize the simulator) + _ = self.world + assert self._gazebo.initialized() + + return self._gazebo + + @property + def world(self) -> bindings.World: + + if self._world is not None: + assert self.gazebo.initialized() + return self._world + + if self._gazebo is None: + raise RuntimeError("Gazebo has not yet been created") + + if self._gazebo.initialized(): + raise RuntimeError("Gazebo was already initialized, cannot insert world") + + if self._world_sdf is None: + self._world_sdf = "" + self._world_name = scenario.get_unique_world_name("default") + + else: + sdf_world_name = bindings.getWorldNameFromSdf(self._world_sdf) + self._world_name = scenario.get_unique_world_name(sdf_world_name) + + # Load the world + ok_world = self._gazebo.insertWorldFromSDF(self._world_sdf, self._world_name) + + if not ok_world: + raise RuntimeError("Failed to load SDF world") + + # Initialize the simulator + ok_initialize = self._gazebo.initialize() + + if not ok_initialize: + raise RuntimeError("Failed to initialize Gazebo") + + if not self._gazebo.initialized(): + raise RuntimeError("Gazebo was not initialized") + + # Get the world + world = self._gazebo.getWorld(self._world_name) + + assert self._world_sdf is not None + assert self._world_name in self._gazebo.worldNames() + + if self._world_sdf == "": + + # Insert the ground plane + ok_ground = world.insertModel( + gym_ignition_models.get_model_file("ground_plane")) + + if not ok_ground: + raise RuntimeError("Failed to insert the ground plane") + + # Load the physics + self._load_physics(world) + + # Store the world + self._world = world + + return self._world + + @staticmethod + def _load_physics(world: bindings.World) -> None: + """ + Load the physics in the world. + + Note: + This class do not yet supports randomizing the physics. Likely overriding this + method will enable the implementation of physics randomization. + """ + + # Insert the physics + ok_physics = world.insertWorldPlugin("libPhysicsSystem.so", + "scenario::plugins::gazebo::Physics") + + if not ok_physics: + raise RuntimeError("Failed to insert the physics plugin") From 3981215a6ee740f47973946b077fa67ecc9256ed Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:32:10 +0200 Subject: [PATCH 13/35] Update runtimes.realtime_runtime --- gym_ignition/runtimes/realtime_runtime.py | 36 ++++++++++++----------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/gym_ignition/runtimes/realtime_runtime.py b/gym_ignition/runtimes/realtime_runtime.py index d0f5f95dc..82a04a2ac 100644 --- a/gym_ignition/runtimes/realtime_runtime.py +++ b/gym_ignition/runtimes/realtime_runtime.py @@ -2,17 +2,25 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import gym from gym_ignition.base import runtime, task -from gym_ignition.utils.typing import State, Action, Observation +from gym_ignition.utils.typing import State, Action, Observation, Done, Info class RealTimeRuntime(runtime.Runtime): + """ + Implementation of :py:class:`~gym_ignition.base.runtime.Runtime` for real-time + execution. + + Warning: + This class is not yet complete. + """ + def __init__(self, task_cls: type, robot_cls: type, agent_rate: float, **kwargs): + # Build the environment task_object = task_cls(**kwargs) @@ -21,26 +29,22 @@ def __init__(self, super().__init__(task=task_object, agent_rate=agent_rate) - # TODO: This class is only a draft raise NotImplementedError - # ========== - # PROPERTIES - # ========== + # ================= + # Runtime interface + # ================= - @property - def agent_rate(self) -> float: - raise NotImplementedError + def timestamp(self) -> float: - @agent_rate.setter - def agent_rate(self, agent_rate: float) -> None: raise NotImplementedError - # =============== - # gym.Env METHODS - # =============== + # ================= + # gym.Env interface + # ================= def step(self, action: Action) -> State: + # Validate action and robot assert self.action_space.contains(action), \ "%r (%s) invalid" % (action, type(action)) @@ -57,17 +61,15 @@ def step(self, action: Action) -> State: "%r (%s) invalid" % (observation, type(observation)) # Get the reward - # TODO: use the wrapper method? reward = self.task.get_reward() assert reward, "Failed to get the reward" # Check termination done = self.task.is_done() - return State((observation, reward, done, {})) + return State((observation, reward, Done(done), Info({}))) def reset(self) -> Observation: - # TODO realtime reset # Get the observation observation = self.task.get_observation() From acba20e3a79f0b2e4ea9304da2b0917a144a93e8 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 18:52:57 +0200 Subject: [PATCH 14/35] Update environment configuration --- cpp/scenario/gazebo/CMakeLists.txt | 5 ++ .../gazebo/include/scenario/gazebo/utils.h | 1 + cpp/scenario/gazebo/src/utils.cpp | 11 ++++ gym_ignition/utils/gazebo_env_vars.py | 51 ++++++++----------- 4 files changed, 38 insertions(+), 30 deletions(-) diff --git a/cpp/scenario/gazebo/CMakeLists.txt b/cpp/scenario/gazebo/CMakeLists.txt index fdf5398cf..fb69bd3a0 100644 --- a/cpp/scenario/gazebo/CMakeLists.txt +++ b/cpp/scenario/gazebo/CMakeLists.txt @@ -94,6 +94,11 @@ target_link_libraries(ScenarioGazebo set_target_properties(ScenarioGazebo PROPERTIES PUBLIC_HEADER "${SCENARIO_PUBLIC_HDRS}") +if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") + target_compile_options(ScenarioGazebo PRIVATE + -DGYMIGNITION_CMAKE_INSTALL_PREFIX="${CMAKE_INSTALL_PREFIX}") +endif() + # =============== # GazeboSimulator # =============== diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h index 0059058c9..286126d8c 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h @@ -50,6 +50,7 @@ namespace scenario { std::string getModelFileFromFuel(const std::string& URI, const bool useCache = false); std::string getRandomString(const size_t length); + std::string getInstallPrefix(); } // namespace utils } // namespace gazebo } // namespace scenario diff --git a/cpp/scenario/gazebo/src/utils.cpp b/cpp/scenario/gazebo/src/utils.cpp index 7b6579abb..ba85771fa 100644 --- a/cpp/scenario/gazebo/src/utils.cpp +++ b/cpp/scenario/gazebo/src/utils.cpp @@ -231,3 +231,14 @@ std::string utils::getRandomString(const size_t length) std::generate_n(str.begin(), length, randchar); return str; } + +std::string utils::getInstallPrefix() +{ +#ifdef GYMIGNITION_CMAKE_INSTALL_PREFIX + return GYMIGNITION_CMAKE_INSTALL_PREFIX; +#else + gymppDebug << "User installation detected. The install prefix " + << "could be detected from the Python module path." << std::endl; + return ""; +#endif +} diff --git a/gym_ignition/utils/gazebo_env_vars.py b/gym_ignition/utils/gazebo_env_vars.py index 04a14fcaf..a5fefccd4 100644 --- a/gym_ignition/utils/gazebo_env_vars.py +++ b/gym_ignition/utils/gazebo_env_vars.py @@ -3,49 +3,40 @@ # GNU Lesser General Public License v2.1 or any later version. import os -from pathlib import Path -from gym_ignition.utils import logger, misc +from gym_ignition.utils import logger def setup_gazebo_env_vars() -> None: + """ + Configure the environment depending on the detected installation method + (User or Developer). + """ + # Configure the environment - ign_gazebo_resource_path = os.environ.get("IGN_GAZEBO_RESOURCE_PATH") - ign_gazebo_system_plugin_path = os.environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH") + ign_gazebo_system_plugin_path = "" + + if "IGN_GAZEBO_SYSTEM_PLUGIN_PATH" in os.environ: + ign_gazebo_system_plugin_path = os.environ.get("IGN_GAZEBO_SYSTEM_PLUGIN_PATH") - if not ign_gazebo_resource_path: - ign_gazebo_resource_path = "" + # Get the install prefix from C++. It is defined only in Developer mode. + from gym_ignition import scenario_bindings + install_prefix = scenario_bindings.getInstallPrefix() - if not ign_gazebo_system_plugin_path: - ign_gazebo_system_plugin_path = "" + if install_prefix != "": + detected_mode = "Developer" + else: + detected_mode = "User" - if misc.installed_in_editable_mode(): - logger.debug("Developer setup") + logger.debug(f"{detected_mode} setup") - # Detect the install prefix - import gympp_bindings - site_packages_path = Path(gympp_bindings.__file__).parent - install_prefix = site_packages_path.parent.parent.parent + # Add the plugins path + if detected_mode == "Developer": logger.debug(f"Detected install prefix: '{install_prefix}'") - - # Add the plugins path ign_gazebo_system_plugin_path += f":{install_prefix}/lib/gympp/plugins" - - # Add the worlds and models path - ign_gazebo_resource_path += f":{install_prefix}/share/gympp/gazebo/worlds" - ign_gazebo_resource_path += f":{install_prefix}/share/gympp/gazebo/models" + ign_gazebo_system_plugin_path += f":{install_prefix}/lib/scenario/plugins" else: - logger.debug("User setup") - - # Add the plugins path import gym_ignition ign_gazebo_system_plugin_path += f":{gym_ignition.__path__[0]}/plugins" - # Add the worlds and models path - import gym_ignition_data - data_path = gym_ignition_data.__path__[0] - ign_gazebo_resource_path += f":{data_path}:/{data_path}/worlds" - - os.environ["IGN_GAZEBO_RESOURCE_PATH"] = ign_gazebo_resource_path os.environ["IGN_GAZEBO_SYSTEM_PLUGIN_PATH"] = ign_gazebo_system_plugin_path - logger.debug(f"IGN_GAZEBO_RESOURCE_PATH={ign_gazebo_resource_path}") logger.debug(f"IGN_GAZEBO_SYSTEM_PLUGIN_PATH={ign_gazebo_system_plugin_path}") From 2663f1934e074b0bae8651bc7130ac0a109420da Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:40:40 +0200 Subject: [PATCH 15/35] Update utils.logger --- gym_ignition/utils/logger.py | 35 +++++++++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/gym_ignition/utils/logger.py b/gym_ignition/utils/logger.py index d46df4bb2..3df58c89a 100644 --- a/gym_ignition/utils/logger.py +++ b/gym_ignition/utils/logger.py @@ -8,13 +8,17 @@ from gym.logger import debug, info, error try: - from gym_ignition import gympp_bindings as bindings + from gym_ignition import scenario_bindings as bindings bindings_found = True -except: +except ImportError: bindings_found = False def custom_formatwarning(msg, *args, **kwargs): + """ + Custom format that overrides :py:func:`warnings.formatwarning`. + """ + if logger.MIN_LEVEL is logger.DEBUG: warning = "{}:{} {}: {}\n".format(args[1], args[2], args[0].__name__, msg) else: @@ -24,6 +28,10 @@ def custom_formatwarning(msg, *args, **kwargs): def warn(msg: str, *args) -> None: + """ + Custom definition of :py:func:`gym.logger.warn` function. + """ + if logger.MIN_LEVEL <= logger.WARN: warnings.warn(colorize('%s: %s' % ('WARN', msg % args), 'yellow'), stacklevel=2) @@ -33,6 +41,21 @@ def warn(msg: str, *args) -> None: def set_level(level: int) -> None: + """ + Set the verbosity level of both :py:mod:`gym` and :py:mod:`gym_ignition`. + + Accepted values: + + - :py:const:`gym.logger.DEBUG` (10) + - :py:const:`gym.logger.INFO` (20) + - :py:const:`gym.logger.WARN` (30) + - :py:const:`gym.logger.ERROR` (40) + - :py:const:`gym.logger.DISABLED` (50) + + Args: + level: The desired verbosity level. + """ + # Set the gym verbosity logger.set_level(level) @@ -41,12 +64,12 @@ def set_level(level: int) -> None: # Set the gympp verbosity if logger.MIN_LEVEL <= logger.DEBUG: - bindings.GazeboSimulator.setVerbosity(4) + bindings.setVerbosity(4) elif logger.MIN_LEVEL <= logger.INFO: - bindings.GazeboSimulator.setVerbosity(3) + bindings.setVerbosity(3) elif logger.MIN_LEVEL <= logger.WARN: - bindings.GazeboSimulator.setVerbosity(2) + bindings.setVerbosity(2) elif logger.MIN_LEVEL <= logger.ERROR: - bindings.GazeboSimulator.setVerbosity(1) + bindings.setVerbosity(1) else: raise Exception("Verbosity level not recognized") From 6fbb653c7368a99a4c3e04d79a6c309bc7f03790 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:35:36 +0200 Subject: [PATCH 16/35] Remove utils.misc --- gym_ignition/utils/__init__.py | 1 - gym_ignition/utils/misc.py | 12 ------------ 2 files changed, 13 deletions(-) diff --git a/gym_ignition/utils/__init__.py b/gym_ignition/utils/__init__.py index 8e1ce6cf5..7111712ad 100644 --- a/gym_ignition/utils/__init__.py +++ b/gym_ignition/utils/__init__.py @@ -3,7 +3,6 @@ # GNU Lesser General Public License v2.1 or any later version. from .typing import * -from . import misc from . import resource_finder from . import gazebo_env_vars from . import inverse_kinematics_nlp diff --git a/gym_ignition/utils/misc.py b/gym_ignition/utils/misc.py index 2933cb0ab..e69de29bb 100644 --- a/gym_ignition/utils/misc.py +++ b/gym_ignition/utils/misc.py @@ -1,12 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - - -def installed_in_editable_mode() -> bool: - # Note: the editable mode detection mechanism can be improved - import gym_ignition - import gympp_bindings - from pathlib import Path - return \ - Path(gympp_bindings.__file__).parent != Path(gym_ignition.__file__).parent.parent From 9e08947dfb9d6e16daae866015a5856926866c2c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:35:57 +0200 Subject: [PATCH 17/35] Update utils.typing --- gym_ignition/utils/typing.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gym_ignition/utils/typing.py b/gym_ignition/utils/typing.py index dd2a4582d..ad6a34661 100644 --- a/gym_ignition/utils/typing.py +++ b/gym_ignition/utils/typing.py @@ -6,13 +6,15 @@ import numpy as np from typing import List, Tuple, Dict, Union, NewType -Action = NewType('Action', Union[np.ndarray, np.number]) -Observation = NewType('Observation', np.ndarray) +Done = NewType('Done', bool) +Info = NewType('Info', Dict) Reward = NewType('Reward', float) +Observation = NewType('Observation', np.ndarray) +Action = NewType('Action', Union[np.ndarray, np.number]) SeedList = NewType('SeedList', List[int]) -State = NewType('State', Tuple[Observation, Reward, bool, Dict]) +State = NewType('State', Tuple[Observation, Reward, Done, Info]) ActionSpace = NewType('ActionSpace', gym.spaces.Space) ObservationSpace = NewType('ObservationSpace', gym.spaces.Space) From 03e8a543198e422496415bce14e618047531405f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:41:23 +0200 Subject: [PATCH 18/35] Expose ECMSingleton to Python --- bindings/scenario/scenario_bindings.i | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/bindings/scenario/scenario_bindings.i b/bindings/scenario/scenario_bindings.i index 09b86cda9..162e3d977 100644 --- a/bindings/scenario/scenario_bindings.i +++ b/bindings/scenario/scenario_bindings.i @@ -8,6 +8,7 @@ #include "scenario/gazebo/Model.h" #include "scenario/gazebo/utils.h" #include "scenario/gazebo/World.h" +#include "scenario/plugins/gazebo/ECMSingleton.h" #include %} @@ -65,3 +66,10 @@ // GazeboSimulator %shared_ptr(scenario::gazebo::GazeboSimulator) %include "scenario/gazebo/GazeboSimulator.h" + +// ECMSingleton +%ignore scenario::plugins::gazebo::ECMSingleton::clean; +%ignore scenario::plugins::gazebo::ECMSingleton::getECM; +%ignore scenario::plugins::gazebo::ECMSingleton::getEventManager; +%ignore scenario::plugins::gazebo::ECMSingleton::storePtrs; +%include "scenario/plugins/gazebo/ECMSingleton.h" From 72606962517bf392684944979a1ba06ab0e7a44c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 12:36:56 +0200 Subject: [PATCH 19/35] New utils.scenario helper module --- gym_ignition/utils/__init__.py | 1 + gym_ignition/utils/scenario.py | 102 +++++++++++++++++++++++++++++++++ 2 files changed, 103 insertions(+) create mode 100644 gym_ignition/utils/scenario.py diff --git a/gym_ignition/utils/__init__.py b/gym_ignition/utils/__init__.py index 7111712ad..62c15e08b 100644 --- a/gym_ignition/utils/__init__.py +++ b/gym_ignition/utils/__init__.py @@ -3,6 +3,7 @@ # GNU Lesser General Public License v2.1 or any later version. from .typing import * +from . import scenario from . import resource_finder from . import gazebo_env_vars from . import inverse_kinematics_nlp diff --git a/gym_ignition/utils/scenario.py b/gym_ignition/utils/scenario.py new file mode 100644 index 000000000..04656dd98 --- /dev/null +++ b/gym_ignition/utils/scenario.py @@ -0,0 +1,102 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +from typing import Tuple +import gym_ignition_models +from gym_ignition import scenario_bindings as bindings + + +def get_unique_model_name(world: bindings.World, model_name: str) -> str: + """ + Get a unique model name given a world configuration. + + This function find the first available model name starting from the argument and + appending a integer postfix until the resulting name is unique in the world. + + Tentatives example: `cartpole`, `cartpole1`, `cartpole2`, ... + + Args: + world: An initialized world. + model_name: The first model name attempt. + + Raises: + ValueError: If the world is not valid. + + Returns: + The unique model name calculated from the original name. + """ + + if world.id() == 0: + raise ValueError("The world is not valid") + + postfix = 0 + model_name_tentative = f"{model_name}" + + while model_name_tentative in world.modelNames(): + + postfix += 1 + model_name_tentative = f"{model_name}{postfix}" + + return model_name_tentative + + +def get_unique_world_name(world_name: str) -> str: + + postfix = 0 + world_name_tentative = f"{world_name}" + ecm_singleton = bindings.ECMSingleton_Instance() + + while world_name_tentative in ecm_singleton.worldNames(): + postfix += 1 + world_name_tentative = f"{world_name}{postfix}" + + return world_name_tentative + + +def init_gazebo_sim(step_size: float = 0.001, + real_time_factor: float = 1.0, + steps_per_run: int = 1) -> Tuple[bindings.GazeboSimulator, + bindings.World]: + """ + Initialize a Gazebo simulation with an empty world and default physics. + + Args: + step_size: Gazebo step size. + real_time_factor: The desired real time factor of the simulation. + steps_per_run: How many steps should be executed at each Gazebo run. + + Raises: + RuntimeError: If the initialization of either the simulator or the world fails. + + Returns: + * **gazebo** -- The Gazebo simulator. + * **world** -- The default world. + """ + + # Create the simulator + gazebo = bindings.GazeboSimulator(step_size, real_time_factor, steps_per_run) + + # Initialize the simulator + ok_initialize = gazebo.initialize() + + if not ok_initialize: + raise RuntimeError("Failed to initialize Gazebo") + + # Get the world + world = gazebo.getWorld() + + # Insert the ground plane + ok_ground = world.insertModel(gym_ignition_models.get_model_file("ground_plane")) + + if not ok_ground: + raise RuntimeError("Failed to insert the ground plane") + + # Insert the physics + ok_physics = world.insertWorldPlugin("libPhysicsSystem.so", + "scenario::plugins::gazebo::Physics") + + if not ok_physics: + raise RuntimeError("Failed to insert the physics plugin") + + return gazebo, world From 4d6db916f5320cbe4e9ec27775330abb33c6efe9 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:40:30 +0200 Subject: [PATCH 20/35] Update main init file --- gym_ignition/__init__.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index f821c61c9..84b06c5ef 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -9,21 +9,29 @@ if sys.platform.startswith('linux') or sys.platform.startswith('darwin'): import os dlopen_flags = sys.getdlopenflags() - if "gympp_bindings" not in sys.modules: + if "scenario_bindings" not in sys.modules: sys.setdlopenflags(dlopen_flags | os.RTLD_GLOBAL) else: sys.setdlopenflags(dlopen_flags | os.RTLD_LAZY | os.RTLD_NOLOAD | os.RTLD_GLOBAL) - import gympp_bindings + import scenario_bindings + scenario_bindings.setVerbosity() + + try: + import gympp_bindings + except ImportError: + pass # Restore the flags sys.setdlopenflags(dlopen_flags) else: - import gympp_bindings + import scenario_bindings + scenario_bindings.setVerbosity() + + try: + import gympp_bindings + except ImportError: + pass - # try: - # import gympp_bindings - # except: - # pass From 97241fcaace48c8fd0757f479a0cbc3a6f0a60fc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:44:20 +0200 Subject: [PATCH 21/35] Configure the ScenarI/O verbosity from Python --- gym_ignition/__init__.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 84b06c5ef..68992a879 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -34,4 +34,5 @@ pass - +# Configure the verbosity depending on the selected CMAKE_BUILD_TYPE +scenario_bindings.setVerbosity() From 889d4558670878eef096ea25ed8502a9ad0a2b85 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:46:16 +0200 Subject: [PATCH 22/35] Configure the environment in the package init file --- gym_ignition/__init__.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 68992a879..b1db057c0 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -2,6 +2,8 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from gym_ignition.utils import gazebo_env_vars + # Import SWIG bindings # See https://github.com/robotology/gym-ignition/issues/7 # https://stackoverflow.com/a/45473441/12150968 @@ -36,3 +38,6 @@ # Configure the verbosity depending on the selected CMAKE_BUILD_TYPE scenario_bindings.setVerbosity() + +# Configure OS environment variables +gazebo_env_vars.setup_gazebo_env_vars() From 57338d1c935dafc511b24f3206ff4935cbd1ef97 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 14:48:23 +0200 Subject: [PATCH 23/35] Find by default resources from IGN_GAZEBO_RESOURCE_PATH --- gym_ignition/__init__.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index b1db057c0..8a762bad8 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -2,7 +2,8 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from gym_ignition.utils import gazebo_env_vars +import os +from gym_ignition.utils import gazebo_env_vars, resource_finder # Import SWIG bindings # See https://github.com/robotology/gym-ignition/issues/7 @@ -41,3 +42,7 @@ # Configure OS environment variables gazebo_env_vars.setup_gazebo_env_vars() + +# Add IGN_GAZEBO_RESOURCE_PATH to the default search path +if "IGN_GAZEBO_RESOURCE_PATH" in os.environ: + resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") From e684c60d5d2497607a4691a26b4ba939cb213deb Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 17:25:53 +0200 Subject: [PATCH 24/35] Update demo tasks --- gym_ignition_environments/tasks/__init__.py | 4 +- .../tasks/cartpole_continuous.py | 172 ----------------- .../tasks/cartpole_continuous_balancing.py | 122 ++++++++++++ .../tasks/cartpole_discrete.py | 174 ------------------ .../tasks/cartpole_discrete_balancing.py | 120 ++++++++++++ .../tasks/pendulum_swingup.py | 131 ++++++------- 6 files changed, 296 insertions(+), 427 deletions(-) delete mode 100644 gym_ignition_environments/tasks/cartpole_continuous.py create mode 100644 gym_ignition_environments/tasks/cartpole_continuous_balancing.py delete mode 100644 gym_ignition_environments/tasks/cartpole_discrete.py create mode 100644 gym_ignition_environments/tasks/cartpole_discrete_balancing.py diff --git a/gym_ignition_environments/tasks/__init__.py b/gym_ignition_environments/tasks/__init__.py index 7e2b75594..40611a181 100644 --- a/gym_ignition_environments/tasks/__init__.py +++ b/gym_ignition_environments/tasks/__init__.py @@ -3,5 +3,5 @@ # GNU Lesser General Public License v2.1 or any later version. from . import pendulum_swingup -from . import cartpole_discrete -from . import cartpole_continuous +from . import cartpole_discrete_balancing +from . import cartpole_continuous_balancing diff --git a/gym_ignition_environments/tasks/cartpole_continuous.py b/gym_ignition_environments/tasks/cartpole_continuous.py deleted file mode 100644 index 345497ec8..000000000 --- a/gym_ignition_environments/tasks/cartpole_continuous.py +++ /dev/null @@ -1,172 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import abc -import gym -import numpy as np -from typing import Tuple -from gym_ignition.base import task -from gym_ignition.utils import logger -from gym_ignition.utils.typing import Action, Reward, Observation -from gym_ignition.utils.typing import ActionSpace, ObservationSpace -from gym_ignition.base.robot import robot_abc, feature_detector, robot_joints - - -@feature_detector -class RobotFeatures(robot_abc.RobotABC, - robot_joints.RobotJoints, - abc.ABC): - pass - - -class CartPoleContinuous(task.Task, abc.ABC): - - def __init__(self, - agent_rate: float, - reward_cart_at_center: bool = True, - **kwargs) -> None: - - # Initialize the Task base class - super().__init__(agent_rate=agent_rate) - - # Store the requested robot features for this task - self.robot_features = RobotFeatures - - # Private attributes - self._steps_beyond_done = None - self._reward_cart_at_center = reward_cart_at_center - - # Variables limits - self._x_threshold = 2.5 - self._x_threshold_reset = 2.4 - - def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: - # Configure action space - max_force = 50 - action_space = gym.spaces.Box(np.array([-max_force]), - np.array([max_force]), - dtype=np.float32) - - # Configure observation limits - high = np.array([ - self._x_threshold, # x - np.finfo(np.float32).max, # x_dot - np.finfo(np.float32).max, # theta - np.finfo(np.float32).max # theta_dot - ]) - - # Configure the observation space - observation_space = gym.spaces.Box(-high, high, dtype=np.float32) - - return action_space, observation_space - - def set_action(self, action: Action) -> bool: - assert self.action_space.contains(action), \ - "%r (%s) invalid" % (action, type(action)) - - # Get the robot object - robot = self.robot - - # Read the action and send the force to the cart - force = action.tolist()[0] - ok = robot.set_joint_force("linear", force) - - if not ok: - raise Exception("Failed to set the force to the cart") - - return True - - def get_observation(self) -> Observation: - # Get the robot object - robot = self.robot - - # Get the new joint positions - x = robot.joint_position("linear") - theta = np.rad2deg(robot.joint_position("pivot")) - - # Get the new joint velocities - x_dot = robot.joint_velocity("linear") - theta_dot = np.rad2deg(robot.joint_velocity("pivot")) - - # Create the observation object - observation = Observation(np.array([x, x_dot, theta, theta_dot])) - - # Return the observation - return observation - - def get_reward(self) -> Reward: - # Calculate the reward - if not self.is_done(): - reward = 1.0 - else: - if self._steps_beyond_done is None: - # Pole just fell - self._steps_beyond_done = 0 - reward = 1.0 - else: - self._steps_beyond_done += 1 - reward = 0.0 - - # Warn the user to call reset - if self._steps_beyond_done == 1: - logger.warn("You are calling 'step()' even though this environment " - "has already returned done = True. You should always " - "call 'reset()' once you receive 'done = True' -- any " - "further steps are undefined behavior.") - - if self._reward_cart_at_center: - # Get the observation - observation = self.get_observation() - x = observation[0] - x_dot = observation[1] - - reward = reward \ - - 0.10 * np.abs(x) \ - - 0.10 * np.abs(x_dot) \ - - 10.0 * (x >= self._x_threshold_reset) - - return reward - - def is_done(self) -> bool: - # Get the observation - observation = self.get_observation() - - # Get x and theta - x = observation[0] - - # Calculate if the environment reached its termination - done = np.abs(x) >= self._x_threshold_reset - - return done - - def reset_task(self) -> bool: - # Initialize the environment with a new random state using the random number - # generator provided by the Task. - new_state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) - new_state[2] += np.pi - - for joint in {"linear", "pivot"}: - desired_control_mode = robot_joints.JointControlMode.TORQUE - - # TODO: temporary workaround for robot implementation without this method, - # e.g. FactoryRobot, which does not need to call it at the moment. - try: - if self.robot.joint_control_mode(joint) != desired_control_mode: - ok_mode = self.robot.set_joint_control_mode(joint, - desired_control_mode) - assert ok_mode, f"Failed to set control mode for joint '{joint}'" - except Exception: - logger.warn("This runtime does not support setting the control mode") - pass - - ok1 = self.robot.reset_joint("linear", new_state[0], new_state[1]) - ok2 = self.robot.reset_joint("pivot", new_state[2], new_state[3]) - - if not (ok1 and ok2): - raise Exception("Failed to reset robot state") - - # Reset the flag that assures reset is properly called - self._steps_beyond_done = None - - return ok1 and ok2 diff --git a/gym_ignition_environments/tasks/cartpole_continuous_balancing.py b/gym_ignition_environments/tasks/cartpole_continuous_balancing.py new file mode 100644 index 000000000..e5b0e04d7 --- /dev/null +++ b/gym_ignition_environments/tasks/cartpole_continuous_balancing.py @@ -0,0 +1,122 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import abc +import gym +import numpy as np +from typing import Tuple +from gym_ignition.base import task +from gym_ignition.utils.typing import Action, Reward, Observation +from gym_ignition.utils.typing import ActionSpace, ObservationSpace + + +class CartPoleContinuousBalancing(task.Task, abc.ABC): + + def __init__(self, + agent_rate: float, + reward_cart_at_center: bool = True, + **kwargs): + + # Initialize the Task base class + task.Task.__init__(self, agent_rate=agent_rate) + + # Name of the cartpole model + self.model_name = None + + # Space for resetting the task + self._reset_space = None + + # Private attributes + self._reward_cart_at_center = reward_cart_at_center + + # Variables limits + self._x_threshold = 2.4 + self._q_threshold = np.deg2rad(12) + + def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: + + # Create the action space + max_force = 50.0 + action_space = gym.spaces.Box(low=np.array([-max_force]), + high=np.array([max_force]), + dtype=np.float32) + + # Configure reset limits + high = np.array([ + self._x_threshold, # x + np.finfo(np.float32).max, # dx + self._q_threshold, # q + np.finfo(np.float32).max # dq + ]) + + # Configure the reset space + self._reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) + + # Configure the observation space + obs_high = high.copy() * 1.2 + observation_space = gym.spaces.Box(low=-obs_high, + high=obs_high, + dtype=np.float32) + + return action_space, observation_space + + def set_action(self, action: Action) -> None: + + # Get the force value + force = action.tolist()[0] + + # Set the force value + model = self.world.getModel(self.model_name) + ok_force = model.getJoint("linear").setGeneralizedForceTarget(force) + + if not ok_force: + raise RuntimeError("Failed to set the force to the cart") + + def get_observation(self) -> Observation: + + # Get the model + model = self.world.getModel(self.model_name) + + # Get the new joint positions and velocities + q, x = model.jointPositions(["pivot", "linear"]) + dq, dx = model.jointVelocities(["pivot", "linear"]) + + # Create the observation + observation = Observation(np.array([x, dx, q, dq])) + + # Return the observation + return observation + + def get_reward(self) -> Reward: + + # Calculate the reward + reward = 1.0 if not self.is_done() else 0.0 + + if self._reward_cart_at_center: + # Get the observation + x, dx, _, _ = self.get_observation() + + reward = reward \ + - 0.10 * np.abs(x) \ + - 0.10 * np.abs(dx) \ + - 10.0 * (x >= self._x_threshold) + + return reward + + def is_done(self) -> bool: + + # Get the observation + observation = self.get_observation() + + # The environment is done if the observation is outside its space + done = not self.observation_space.contains(observation) + + return done + + def reset_task(self) -> None: + + if self.model_name not in self.world.modelNames(): + raise RuntimeError("Cartpole model not found in the world") diff --git a/gym_ignition_environments/tasks/cartpole_discrete.py b/gym_ignition_environments/tasks/cartpole_discrete.py deleted file mode 100644 index de6691b98..000000000 --- a/gym_ignition_environments/tasks/cartpole_discrete.py +++ /dev/null @@ -1,174 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import abc -import gym -import numpy as np -from typing import Tuple -from gym_ignition.base import task -from gym_ignition.utils import logger -from gym_ignition.utils.typing import Action, Reward, Observation -from gym_ignition.utils.typing import ActionSpace, ObservationSpace -from gym_ignition.base.robot import robot_abc, feature_detector, robot_joints - - -@feature_detector -class RobotFeatures(robot_abc.RobotABC, - robot_joints.RobotJoints, - abc.ABC): - pass - - -class CartPoleDiscrete(task.Task, abc.ABC): - - def __init__(self, - agent_rate: float, - reward_cart_at_center: bool = True, - **kwargs) -> None: - - # Initialize the Task base class - super().__init__(agent_rate=agent_rate) - - # Store the requested robot features for this task - self.robot_features = RobotFeatures - - # Private attributes - self._force_mag = 20.0 - self._steps_beyond_done = None - self._reward_cart_at_center = reward_cart_at_center - - # Variables limits - self._x_threshold = 2.4 - self._theta_threshold_radians = np.deg2rad(12) - - def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: - # Configure action space - action_space = gym.spaces.Discrete(2) - - # Configure observation limits - high = np.array([ - self._x_threshold * 2, # x - np.finfo(np.float32).max, # x_dot - np.rad2deg(self._theta_threshold_radians * 2), # theta - np.finfo(np.float32).max # theta_dot - ]) - - # Configure the observation space - observation_space = gym.spaces.Box(-high, high, dtype=np.float32) - - return action_space, observation_space - - def set_action(self, action: Action) -> bool: - assert self.action_space.contains(action), \ - "%r (%s) invalid" % (action, type(action)) - - # Get the robot object - robot = self.robot - - # Read the action and send the force to the cart - force = self._force_mag if action == 1 else -self._force_mag - ok = robot.set_joint_force("linear", force) - - if not ok: - raise Exception("Failed to set the force to the cart") - - return True - - def get_observation(self) -> Observation: - # Get the robot object - robot = self.robot - - # Get the new joint positions - x = robot.joint_position("linear") - theta = np.rad2deg(robot.joint_position("pivot")) - - # Get the new joint velocities - x_dot = robot.joint_velocity("linear") - theta_dot = np.rad2deg(robot.joint_velocity("pivot")) - - # Create the observation object - observation = Observation(np.array([x, x_dot, theta, theta_dot])) - - # Return the observation - return observation - - def get_reward(self) -> Reward: - # Calculate the reward - if not self.is_done(): - reward = 1.0 - else: - if self._steps_beyond_done is None: - # Pole just fell - self._steps_beyond_done = 0 - reward = 1.0 - else: - self._steps_beyond_done += 1 - reward = 0.0 - - # Warn the user to call reset - if self._steps_beyond_done == 1: - logger.warn("You are calling 'step()' even though this environment " - "has already returned done = True. You should always " - "call 'reset()' once you receive 'done = True' -- any " - "further steps are undefined behavior.") - - if self._reward_cart_at_center: - # Get the observation - observation = self.get_observation() - x = observation[0] - x_dot = observation[1] - - # Update the reward - reward = reward \ - - np.abs(x) \ - - np.abs(x_dot) \ - - 10 * (x >= self._x_threshold) - - return reward - - def is_done(self) -> bool: - # Get the observation - observation = self.get_observation() - - # Get x and theta - x = observation[0] - theta = observation[2] - - # Calculate if the environment reached its termination - done = \ - np.abs(x) > self._x_threshold or \ - np.abs(theta) > np.rad2deg(self._theta_threshold_radians) - - return done - - def reset_task(self) -> bool: - # Initialize the environment with a new random state using the random number - # generator provided by the Task. - new_state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) - new_state[2] = self.np_random.uniform(low=-np.deg2rad(10), - high=np.deg2rad(10)) - - # Set the joints in torque control mode - for joint in {"linear", "pivot"}: - desired_control_mode = robot_joints.JointControlMode.TORQUE - - # TODO: temporary workaround for robot implementation without this method, - # e.g. FactoryRobot, which does not need to call it at the moment. - try: - if self.robot.joint_control_mode(joint) != desired_control_mode: - ok_mode = self.robot.set_joint_control_mode(joint, - desired_control_mode) - assert ok_mode, f"Failed to set control mode for joint '{joint}'" - except Exception: - logger.warn("This runtime does not support setting the control mode") - pass - - # Reset position and velocity - ok1 = self.robot.reset_joint("linear", new_state[0], new_state[1]) - ok2 = self.robot.reset_joint("pivot", new_state[2], new_state[3]) - - # Reset the flag that assures reset is properly called - self._steps_beyond_done = None - - return ok1 and ok2 diff --git a/gym_ignition_environments/tasks/cartpole_discrete_balancing.py b/gym_ignition_environments/tasks/cartpole_discrete_balancing.py new file mode 100644 index 000000000..e2fcc4302 --- /dev/null +++ b/gym_ignition_environments/tasks/cartpole_discrete_balancing.py @@ -0,0 +1,120 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import abc +import gym +import numpy as np +from typing import Tuple +from gym_ignition.base import task +from gym_ignition.utils.typing import Action, Reward, Observation +from gym_ignition.utils.typing import ActionSpace, ObservationSpace + + +class CartPoleDiscreteBalancing(task.Task, abc.ABC): + + def __init__(self, + agent_rate: float, + reward_cart_at_center: bool = True, + **kwargs) -> None: + + # Initialize the Task base class + task.Task.__init__(self, agent_rate=agent_rate) + + # Name of the cartpole model + self.model_name = None + + # Space for resetting the task + self._reset_space = None + + # Private attributes + self._force_mag = 20.0 + self._reward_cart_at_center = reward_cart_at_center + + # Variables limits + self._x_threshold = 2.4 + self._q_threshold = np.deg2rad(12) + + def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: + + # Configure action space: [0, 1] + action_space = gym.spaces.Discrete(2) + + # Configure reset limits + high = np.array([ + self._x_threshold, # x + np.finfo(np.float32).max, # dx + self._q_threshold, # q + np.finfo(np.float32).max # dq + ]) + + # Configure the reset space + self._reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) + + # Configure the observation space + obs_high = high.copy() * 1.2 + observation_space = gym.spaces.Box(low=-obs_high, + high=obs_high, + dtype=np.float32) + + return action_space, observation_space + + def set_action(self, action: Action) -> None: + + # Calculate the force + force = self._force_mag if action == 1 else -self._force_mag + + # Set the force value + model = self.world.getModel(self.model_name) + ok_force = model.getJoint("linear").setGeneralizedForceTarget(force) + + if not ok_force: + raise RuntimeError("Failed to set the force to the cart") + + def get_observation(self) -> Observation: + + # Get the model + model = self.world.getModel(self.model_name) + + # Get the new joint positions and velocities + q, x = model.jointPositions(["pivot", "linear"]) + dq, dx = model.jointVelocities(["pivot", "linear"]) + + # Create the observation + observation = Observation(np.array([x, dx, q, dq])) + + # Return the observation + return observation + + def get_reward(self) -> Reward: + + # Calculate the reward + reward = 1.0 if not self.is_done() else 0.0 + + if self._reward_cart_at_center: + # Get the observation + x, dx, _, _ = self.get_observation() + + reward = reward \ + - 0.10 * np.abs(x) \ + - 0.10 * np.abs(dx) \ + - 10.0 * (x >= 0.9 * self._x_threshold) + + return reward + + def is_done(self) -> bool: + + # Get the observation + observation = self.get_observation() + + # The environment is done if the observation is outside its space + done = not self._reset_space.contains(observation) + + return done + + def reset_task(self) -> None: + + if self.model_name not in self.world.modelNames(): + raise RuntimeError("Cartpole model not found in the world") diff --git a/gym_ignition_environments/tasks/pendulum_swingup.py b/gym_ignition_environments/tasks/pendulum_swingup.py index f5ef4475c..55cff673d 100644 --- a/gym_ignition_environments/tasks/pendulum_swingup.py +++ b/gym_ignition_environments/tasks/pendulum_swingup.py @@ -7,125 +7,98 @@ import numpy as np from typing import Tuple from gym_ignition.base import task -from gym_ignition.utils import logger from gym_ignition.utils.typing import Action, Observation, Reward from gym_ignition.utils.typing import ActionSpace, ObservationSpace -from gym_ignition.base.robot import robot_abc, feature_detector, robot_joints - - -@feature_detector -class RobotFeatures(robot_abc.RobotABC, - robot_joints.RobotJoints, - abc.ABC): - pass class PendulumSwingUp(task.Task, abc.ABC): - def __init__(self, agent_rate: float, **kwargs) -> None: + def __init__(self, agent_rate: float, **kwargs): # Initialize the Task base class - super().__init__(agent_rate=agent_rate) + task.Task.__init__(self, agent_rate=agent_rate) - # Store the requested robot features for this task - self.robot_features = RobotFeatures + # Name of the pendulum model + self.model_name = None # Limits - self._max_speed = 8.0 - self._max_torque = 2.0 - - # Private attributes - self._last_a = None + self._max_speed = 10.0 + self._max_torque = 50.0 def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: - action_space = gym.spaces.Box(low=-self._max_torque, high=self._max_torque, - shape=(1,), dtype=np.float32) - high = np.array( - [1., # cos(theta) - 1., # sin(theta) - self._max_speed]) + action_space = gym.spaces.Box(low=-self._max_torque, + high=self._max_torque, + shape=(1,), + dtype=np.float32) + + high = np.array([ + 1.0, # cos(theta) + 1.0, # sin(theta) + self._max_speed + ]) + observation_space = gym.spaces.Box(low=-high, high=high, dtype=np.float32) return action_space, observation_space - def set_action(self, action: Action) -> bool: - # Validate the action - assert self.action_space.contains(action), \ - "%r (%s) invalid" % (action, type(action)) - - # Store the last action. It is used to calculate the reward. - self._last_a = action + def set_action(self, action: Action) -> None: - # Read the action and send the force to the cart + # Get the force value force = action.tolist()[0] - ok = self.robot.set_joint_force("pivot", force) - if not ok: - raise Exception("Failed to set the force to the pendulum") + # Set the force value + model = self.world.getModel(self.model_name) + ok_force = model.getJoint("pivot").setGeneralizedForceTarget(force) - return True + if not ok_force: + raise RuntimeError("Failed to set the force to the pendulum") def get_observation(self) -> Observation: - # Get the robot object - robot = self.robot - # Get the new pendulum position and velocity - theta = robot.joint_position("pivot") - theta_dot = robot.joint_velocity("pivot") + # Get the model + model = self.world.getModel(self.model_name) - # Create the observation object - observation = Observation(np.array([np.cos(theta), np.sin(theta), theta_dot])) + # Get the new joint position and velocity + q = model.getJoint("pivot").position() + dq = model.getJoint("pivot").velocity() + + # Create the observation + observation = Observation(np.array([np.cos(q), np.sin(q), dq])) # Return the observation return observation def get_reward(self) -> Reward: - # This environments is done only if the observation goes outside its limits. + + # This environment is done only if the observation goes outside its limits. # Since it can happen only when velocity is too high, penalize this happening. - if self.is_done(): - return Reward(-10000) + cost = 100.0 if self.is_done() else 0.0 + + # Get the model + model = self.world.getModel(self.model_name) - # Get the data from the robot object - theta = self.robot.joint_position("pivot") - theta_dot = self.robot.joint_velocity("pivot") + # Get the pendulum state + q = model.getJoint("pivot").position() + dq = model.getJoint("pivot").velocity() + tau = model.getJoint("pivot").generalizedForceTarget() - cost = \ - theta * theta + \ - 0.1 * theta_dot * theta_dot +\ - 0.001 * self._last_a + # Calculate the cost + cost += (q ** 2) + 0.1 * (dq ** 2) + 0.001 * (tau ** 2) return Reward(-cost) def is_done(self) -> bool: - if not self.observation_space.contains(self.get_observation()): - logger.warn("Observation is outside its space. Marking the episode as done.") - return True - - # This environment is episodic and always reach the max_episode_steps - return False - - def reset_task(self) -> bool: - # Sample the angular velocity from the observation space - _, _, theta_dot = self.observation_space.sample().tolist() - # Sample the angular position from an uniform rng - theta = self.np_random.uniform(0, 2 * np.pi) + # Get the observation + observation = self.get_observation() - try: - desired_control_mode = robot_joints.JointControlMode.TORQUE - if self.robot.joint_control_mode("pivot") != desired_control_mode: - ok_mode = self.robot.set_joint_control_mode("pivot", desired_control_mode) - assert ok_mode, "Failed to set pendulum control mode" - except Exception: - logger.warn("Failed to set control mode. Is it supported by the runtime?") - pass + # The environment is done if the observation is outside its space + done = not self.observation_space.contains(observation) - # Reset the robot state - ok_reset = self.robot.reset_joint("pivot", theta, theta_dot) - assert ok_reset, "Failed to reset the pendulum" + return done - # Clean the last applied force - self._last_a = None + def reset_task(self) -> None: - return True + if self.model_name not in self.world.modelNames(): + raise RuntimeError("The cartpole model was not inserted in the world") From db84c8e0c4cddd847934325fecb336eb640c6bbb Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 15:29:15 +0200 Subject: [PATCH 25/35] Add new CartPoleContinuousSwingup task --- gym_ignition_environments/tasks/__init__.py | 6 + .../tasks/cartpole_continuous_swingup.py | 121 ++++++++++++++++++ 2 files changed, 127 insertions(+) create mode 100644 gym_ignition_environments/tasks/cartpole_continuous_swingup.py diff --git a/gym_ignition_environments/tasks/__init__.py b/gym_ignition_environments/tasks/__init__.py index 40611a181..e33fc6c82 100644 --- a/gym_ignition_environments/tasks/__init__.py +++ b/gym_ignition_environments/tasks/__init__.py @@ -2,6 +2,12 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +# Pendulum from . import pendulum_swingup + +# Cartpole discrete from . import cartpole_discrete_balancing + +# Cartpole continuous +from . import cartpole_continuous_swingup from . import cartpole_continuous_balancing diff --git a/gym_ignition_environments/tasks/cartpole_continuous_swingup.py b/gym_ignition_environments/tasks/cartpole_continuous_swingup.py new file mode 100644 index 000000000..65178c625 --- /dev/null +++ b/gym_ignition_environments/tasks/cartpole_continuous_swingup.py @@ -0,0 +1,121 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import abc +import gym +import numpy as np +from typing import Tuple +from gym_ignition.base import task +from gym_ignition.utils.typing import Action, Reward, Observation +from gym_ignition.utils.typing import ActionSpace, ObservationSpace + + +class CartPoleContinuousSwingup(task.Task, abc.ABC): + + def __init__(self, + agent_rate: float, + reward_cart_at_center: bool = True, + **kwargs): + + # Initialize the Task base class + task.Task.__init__(self, agent_rate=agent_rate) + + # Name of the cartpole model + self.model_name = None + + # Space for resetting the task + self._reset_space = None + + # Private attributes + self._reward_cart_at_center = reward_cart_at_center + + # Variables limits + self._x_threshold = 2.4 + + def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: + + # Create the action space + max_force = 50.0 + action_space = gym.spaces.Box(low=np.array([-max_force]), + high=np.array([max_force]), + dtype=np.float32) + + # Configure reset limits + high = np.array([ + self._x_threshold, # x + np.finfo(np.float32).max, # dx + np.finfo(np.float32).max, # q + np.finfo(np.float32).max # dq + ]) + + # Configure the reset space + self._reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) + + # Configure the observation space + obs_high = high.copy() * 1.2 + observation_space = gym.spaces.Box(low=-obs_high, + high=obs_high, + dtype=np.float32) + + return action_space, observation_space + + def set_action(self, action: Action) -> None: + + # Get the force value + force = action.tolist()[0] + + # Set the force value + model = self.world.getModel(self.model_name) + ok_force = model.getJoint("linear").setGeneralizedForceTarget(force) + + if not ok_force: + raise RuntimeError("Failed to set the force to the cart") + + def get_observation(self) -> Observation: + + # Get the model + model = self.world.getModel(self.model_name) + + # Get the new joint positions and velocities + q, x = model.jointPositions(["pivot", "linear"]) + dq, dx = model.jointVelocities(["pivot", "linear"]) + + # Create the observation + observation = Observation(np.array([x, dx, q, dq])) + + # Return the observation + return observation + + def get_reward(self) -> Reward: + + # Calculate the reward + reward = 1.0 if not self.is_done() else 0.0 + + if self._reward_cart_at_center: + # Get the observation + x, dx, _, _ = self.get_observation() + + reward = reward \ + - 0.10 * np.abs(x) \ + - 0.10 * np.abs(dx) \ + - 10.0 * (x >= self._x_threshold) + + return reward + + def is_done(self) -> bool: + + # Get the observation + observation = self.get_observation() + + # The environment is done if the observation is outside its space + done = not self.observation_space.contains(observation) + + return done + + def reset_task(self) -> None: + + if self.model_name not in self.world.modelNames(): + raise RuntimeError("Cartpole model not found in the world") From 5ed481e3d5363e05959444d66a40e3daad6ed35e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 16:17:34 +0200 Subject: [PATCH 26/35] Fix import loop --- gym_ignition/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 8a762bad8..07213ae1d 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -3,7 +3,6 @@ # GNU Lesser General Public License v2.1 or any later version. import os -from gym_ignition.utils import gazebo_env_vars, resource_finder # Import SWIG bindings # See https://github.com/robotology/gym-ignition/issues/7 @@ -41,6 +40,7 @@ scenario_bindings.setVerbosity() # Configure OS environment variables +from gym_ignition.utils import gazebo_env_vars, resource_finder gazebo_env_vars.setup_gazebo_env_vars() # Add IGN_GAZEBO_RESOURCE_PATH to the default search path From 6062f12b7b9b192ff3852e2c4e94661f6a69d176 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 15 Apr 2020 17:14:40 +0200 Subject: [PATCH 27/35] Setup the environment variables for gym-ignition-models --- gym_ignition/__init__.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gym_ignition/__init__.py b/gym_ignition/__init__.py index 07213ae1d..8f2a32d39 100644 --- a/gym_ignition/__init__.py +++ b/gym_ignition/__init__.py @@ -2,7 +2,10 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -import os +# Workaround for https://github.com/osrf/sdformat/issues/227. +# It has to be done before loading the bindings. +import gym_ignition_models +gym_ignition_models.setup_environment() # Import SWIG bindings # See https://github.com/robotology/gym-ignition/issues/7 @@ -44,5 +47,6 @@ gazebo_env_vars.setup_gazebo_env_vars() # Add IGN_GAZEBO_RESOURCE_PATH to the default search path +import os if "IGN_GAZEBO_RESOURCE_PATH" in os.environ: resource_finder.add_path_from_env_var("IGN_GAZEBO_RESOURCE_PATH") From 80f0db7dbe3c2c10056810ade365fad01dde5dfc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:20:28 +0200 Subject: [PATCH 28/35] Store the seed in task.Task as public member --- gym_ignition/base/task.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gym_ignition/base/task.py b/gym_ignition/base/task.py index 8737b9b06..56c52032b 100644 --- a/gym_ignition/base/task.py +++ b/gym_ignition/base/task.py @@ -57,8 +57,11 @@ def __init__(self, agent_rate: float) -> None: #: Use it for all the random resources. self.np_random: np.random.RandomState + #: The seed of the task + self.seed: int + # Initialize the RNG and the seed - self.np_random, self._seed = seeding.np_random() + self.np_random, self.seed = seeding.np_random() # ========== # PROPERTIES @@ -213,14 +216,14 @@ def seed_task(self, seed: int = None) -> SeedList: """ # Create the seed if not passed - self._seed = np.random.randint(2**32 - 1) if seed is None else seed + seed = np.random.randint(2**32 - 1) if seed is None else seed # Get an instance of the random number generator from gym utils. # This is necessary to have an independent rng for each environment. - self.np_random, new_seed = seeding.np_random(self._seed) + self.np_random, self.seed = seeding.np_random(seed) # Seed the spaces - self.action_space.seed(new_seed) - self.observation_space.seed(new_seed) + self.action_space.seed(self.seed) + self.observation_space.seed(self.seed) - return SeedList([new_seed]) + return SeedList([self.seed]) From cf1743ae91852a3cd6bff637aa6d94e6f441d044 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:22:19 +0200 Subject: [PATCH 29/35] Add a contextmanager to change locally the verbosity --- gym_ignition/utils/logger.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gym_ignition/utils/logger.py b/gym_ignition/utils/logger.py index 3df58c89a..1c011dcf3 100644 --- a/gym_ignition/utils/logger.py +++ b/gym_ignition/utils/logger.py @@ -2,7 +2,9 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +import gym import warnings +import contextlib from gym import logger from gym.utils import colorize from gym.logger import debug, info, error @@ -73,3 +75,12 @@ def set_level(level: int) -> None: bindings.setVerbosity(1) else: raise Exception("Verbosity level not recognized") + + +@contextlib.contextmanager +def verbosity(level: int): + + old_level = gym.logger.MIN_LEVEL + gym.logger.set_level(level=level) + yield None + gym.logger.set_level(old_level) \ No newline at end of file From e632660d6f7a4f71bdd1ceae202a552e440b738d Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:23:01 +0200 Subject: [PATCH 30/35] Helper to write a string to a tempfile --- gym_ignition/utils/__init__.py | 1 + gym_ignition/utils/misc.py | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/gym_ignition/utils/__init__.py b/gym_ignition/utils/__init__.py index 62c15e08b..31a098b0f 100644 --- a/gym_ignition/utils/__init__.py +++ b/gym_ignition/utils/__init__.py @@ -2,6 +2,7 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from . import misc from .typing import * from . import scenario from . import resource_finder diff --git a/gym_ignition/utils/misc.py b/gym_ignition/utils/misc.py index e69de29bb..1a53dbd1a 100644 --- a/gym_ignition/utils/misc.py +++ b/gym_ignition/utils/misc.py @@ -0,0 +1,15 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import tempfile + + +def string_to_file(string: str) -> str: + + handle, tmpfile = tempfile.mkstemp() + + with open(handle, 'w') as f: + f.write(string) + + return tmpfile From 5ec561a51d3a06648668b3160930002a867b38c8 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:24:45 +0200 Subject: [PATCH 31/35] Update demo tasks --- .../tasks/cartpole_continuous_balancing.py | 48 +++++++++---- .../tasks/cartpole_continuous_swingup.py | 72 +++++++++++++------ .../tasks/cartpole_discrete_balancing.py | 48 +++++++++---- .../tasks/pendulum_swingup.py | 26 +++++++ 4 files changed, 150 insertions(+), 44 deletions(-) diff --git a/gym_ignition_environments/tasks/cartpole_continuous_balancing.py b/gym_ignition_environments/tasks/cartpole_continuous_balancing.py index e5b0e04d7..c5826cd22 100644 --- a/gym_ignition_environments/tasks/cartpole_continuous_balancing.py +++ b/gym_ignition_environments/tasks/cartpole_continuous_balancing.py @@ -7,6 +7,7 @@ import numpy as np from typing import Tuple from gym_ignition.base import task +from gym_ignition import scenario_bindings as bindings from gym_ignition.utils.typing import Action, Reward, Observation from gym_ignition.utils.typing import ActionSpace, ObservationSpace @@ -25,35 +26,37 @@ def __init__(self, self.model_name = None # Space for resetting the task - self._reset_space = None + self.reset_space = None # Private attributes self._reward_cart_at_center = reward_cart_at_center # Variables limits - self._x_threshold = 2.4 - self._q_threshold = np.deg2rad(12) + self._x_threshold = 2.4 # m + self._dx_threshold = 20.0 # m /s + self._q_threshold = np.deg2rad(12) # rad + self._dq_threshold = np.deg2rad(3 * 360) # rad / s def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: # Create the action space - max_force = 50.0 + max_force = 50.0 # Nm action_space = gym.spaces.Box(low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float32) # Configure reset limits high = np.array([ - self._x_threshold, # x - np.finfo(np.float32).max, # dx - self._q_threshold, # q - np.finfo(np.float32).max # dq + self._x_threshold, # x + self._dx_threshold, # dx + self._q_threshold, # q + self._dq_threshold # dq ]) # Configure the reset space - self._reset_space = gym.spaces.Box(low=-high, - high=high, - dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) # Configure the observation space obs_high = high.copy() * 1.2 @@ -96,6 +99,7 @@ def get_reward(self) -> Reward: reward = 1.0 if not self.is_done() else 0.0 if self._reward_cart_at_center: + # Get the observation x, dx, _, _ = self.get_observation() @@ -112,7 +116,7 @@ def is_done(self) -> bool: observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.observation_space.contains(observation) + done = not self.reset_space.contains(observation) return done @@ -120,3 +124,23 @@ def reset_task(self) -> None: if self.model_name not in self.world.modelNames(): raise RuntimeError("Cartpole model not found in the world") + + # Get the model + model = self.world.getModel(self.model_name) + + # Control the cart in force mode + linear = model.getJoint("linear") + ok_control_mode = linear.setControlMode(bindings.JointControlMode_Force) + + if not ok_control_mode: + raise RuntimeError("Failed to change the control mode of the cartpole") + + # Create a new cartpole state + x, dx, q, dq = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) + + # Reset the cartpole state + ok_reset_pos = model.resetJointPositions([x, q], ["linear", "pivot"]) + ok_reset_vel = model.resetJointVelocities([dx, dq], ["linear", "pivot"]) + + if not ok_reset_pos and not ok_reset_vel: + raise RuntimeError("Failed to reset the cartpole state") diff --git a/gym_ignition_environments/tasks/cartpole_continuous_swingup.py b/gym_ignition_environments/tasks/cartpole_continuous_swingup.py index 65178c625..a9321ca31 100644 --- a/gym_ignition_environments/tasks/cartpole_continuous_swingup.py +++ b/gym_ignition_environments/tasks/cartpole_continuous_swingup.py @@ -7,6 +7,7 @@ import numpy as np from typing import Tuple from gym_ignition.base import task +from gym_ignition import scenario_bindings as bindings from gym_ignition.utils.typing import Action, Reward, Observation from gym_ignition.utils.typing import ActionSpace, ObservationSpace @@ -25,34 +26,37 @@ def __init__(self, self.model_name = None # Space for resetting the task - self._reset_space = None + self.reset_space = None # Private attributes self._reward_cart_at_center = reward_cart_at_center # Variables limits - self._x_threshold = 2.4 + self._x_threshold = 2.4 # m + self._dx_threshold = 20.0 # m /s + self._q_threshold = np.deg2rad(5 * 360) # rad + self._dq_threshold = np.deg2rad(3 * 360) # rad / s def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: # Create the action space - max_force = 50.0 + max_force = 200.0 # Nm action_space = gym.spaces.Box(low=np.array([-max_force]), high=np.array([max_force]), dtype=np.float32) # Configure reset limits high = np.array([ - self._x_threshold, # x - np.finfo(np.float32).max, # dx - np.finfo(np.float32).max, # q - np.finfo(np.float32).max # dq + self._x_threshold, # x + self._dx_threshold, # dx + self._q_threshold, # q + self._dq_threshold # dq ]) # Configure the reset space - self._reset_space = gym.spaces.Box(low=-high, - high=high, - dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) # Configure the observation space obs_high = high.copy() * 1.2 @@ -91,17 +95,24 @@ def get_observation(self) -> Observation: def get_reward(self) -> Reward: - # Calculate the reward - reward = 1.0 if not self.is_done() else 0.0 + # Get the model + model = self.world.getModel(self.model_name) + + # Get the pendulum position + q = model.getJoint("pivot").position() - if self._reward_cart_at_center: - # Get the observation - x, dx, _, _ = self.get_observation() + # Get the cart state + x = model.getJoint("linear").position() + dx = model.getJoint("linear").velocity() - reward = reward \ - - 0.10 * np.abs(x) \ - - 0.10 * np.abs(dx) \ - - 10.0 * (x >= self._x_threshold) + # Reward is [0, 1] for q=[0, pi] + reward = (np.cos(q) + 1) / 2 + + # Penalize cart velocities + reward -= 0.1 * (dx ** 2) + + # Penalize positions close to the end of the rail + reward -= 10.0 * (x >= 0.8 * self._x_threshold) return reward @@ -111,7 +122,7 @@ def is_done(self) -> bool: observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self.observation_space.contains(observation) + done = not self.reset_space.contains(observation) return done @@ -119,3 +130,24 @@ def reset_task(self) -> None: if self.model_name not in self.world.modelNames(): raise RuntimeError("Cartpole model not found in the world") + + # Get the model + model = self.world.getModel(self.model_name) + + # Control the cart in force mode + linear = model.getJoint("linear") + ok_control_mode = linear.setControlMode(bindings.JointControlMode_Force) + + if not ok_control_mode: + raise RuntimeError("Failed to change the control mode of the cartpole") + + # Create a new cartpole state + q = np.pi - np.deg2rad(self.np_random.uniform(low=-60, high=60)) + x, dx, dq = self.np_random.uniform(low=-0.05, high=0.05, size=(3,)) + + # Reset the cartpole state + ok_reset_pos = model.resetJointPositions([x, q], ["linear", "pivot"]) + ok_reset_vel = model.resetJointVelocities([dx, dq], ["linear", "pivot"]) + + if not ok_reset_pos and not ok_reset_vel: + raise RuntimeError("Failed to reset the cartpole state") diff --git a/gym_ignition_environments/tasks/cartpole_discrete_balancing.py b/gym_ignition_environments/tasks/cartpole_discrete_balancing.py index e2fcc4302..fd571537e 100644 --- a/gym_ignition_environments/tasks/cartpole_discrete_balancing.py +++ b/gym_ignition_environments/tasks/cartpole_discrete_balancing.py @@ -7,6 +7,7 @@ import numpy as np from typing import Tuple from gym_ignition.base import task +from gym_ignition import scenario_bindings as bindings from gym_ignition.utils.typing import Action, Reward, Observation from gym_ignition.utils.typing import ActionSpace, ObservationSpace @@ -25,15 +26,17 @@ def __init__(self, self.model_name = None # Space for resetting the task - self._reset_space = None + self.reset_space = None # Private attributes - self._force_mag = 20.0 + self._force_mag = 20.0 # Nm self._reward_cart_at_center = reward_cart_at_center # Variables limits - self._x_threshold = 2.4 - self._q_threshold = np.deg2rad(12) + self._x_threshold = 2.4 # m + self._dx_threshold = 20.0 # m /s + self._q_threshold = np.deg2rad(12) # rad + self._dq_threshold = np.deg2rad(3 * 360) # rad / s def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: @@ -42,16 +45,16 @@ def create_spaces(self) -> Tuple[ActionSpace, ObservationSpace]: # Configure reset limits high = np.array([ - self._x_threshold, # x - np.finfo(np.float32).max, # dx - self._q_threshold, # q - np.finfo(np.float32).max # dq + self._x_threshold, # x + self._dx_threshold, # dx + self._q_threshold, # q + self._dq_threshold # dq ]) # Configure the reset space - self._reset_space = gym.spaces.Box(low=-high, - high=high, - dtype=np.float32) + self.reset_space = gym.spaces.Box(low=-high, + high=high, + dtype=np.float32) # Configure the observation space obs_high = high.copy() * 1.2 @@ -94,6 +97,7 @@ def get_reward(self) -> Reward: reward = 1.0 if not self.is_done() else 0.0 if self._reward_cart_at_center: + # Get the observation x, dx, _, _ = self.get_observation() @@ -110,7 +114,7 @@ def is_done(self) -> bool: observation = self.get_observation() # The environment is done if the observation is outside its space - done = not self._reset_space.contains(observation) + done = not self.reset_space.contains(observation) return done @@ -118,3 +122,23 @@ def reset_task(self) -> None: if self.model_name not in self.world.modelNames(): raise RuntimeError("Cartpole model not found in the world") + + # Get the model + model = self.world.getModel(self.model_name) + + # Control the cart in force mode + linear = model.getJoint("linear") + ok_control_mode = linear.setControlMode(bindings.JointControlMode_Force) + + if not ok_control_mode: + raise RuntimeError("Failed to change the control mode of the cartpole") + + # Create a new cartpole state + x, dx, q, dq = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) + + # Reset the cartpole state + ok_reset_pos = model.resetJointPositions([x, q], ["linear", "pivot"]) + ok_reset_vel = model.resetJointVelocities([dx, dq], ["linear", "pivot"]) + + if not ok_reset_pos and not ok_reset_vel: + raise RuntimeError("Failed to reset the cartpole state") diff --git a/gym_ignition_environments/tasks/pendulum_swingup.py b/gym_ignition_environments/tasks/pendulum_swingup.py index 55cff673d..7659f648b 100644 --- a/gym_ignition_environments/tasks/pendulum_swingup.py +++ b/gym_ignition_environments/tasks/pendulum_swingup.py @@ -7,6 +7,7 @@ import numpy as np from typing import Tuple from gym_ignition.base import task +from gym_ignition import scenario_bindings as bindings from gym_ignition.utils.typing import Action, Observation, Reward from gym_ignition.utils.typing import ActionSpace, ObservationSpace @@ -102,3 +103,28 @@ def reset_task(self) -> None: if self.model_name not in self.world.modelNames(): raise RuntimeError("The cartpole model was not inserted in the world") + + # Get the model + model = self.world.getModel(self.model_name) + + # Control the pendulum in force mode + pivot = model.getJoint("pivot") + ok_control_mode = pivot.setControlMode(bindings.JointControlMode_Force) + + if not ok_control_mode: + raise RuntimeError("Failed to change the control mode of the pendulum") + + # Sample an observation + cos_q, sin_q, dq = self.observation_space.sample() + + # Compute the angle + q = np.arctan2(sin_q, cos_q) + + # Convert to float + q, dq = float(q), float(dq) + + # Reset the pendulum state + ok_reset = pivot.reset(q, dq) + + if not ok_reset: + raise RuntimeError("Failed to reset the pendulum state") From 23253279a698d2978dcd11ad996eafac77056612 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:32:03 +0200 Subject: [PATCH 32/35] New interfaces for ScenarI/O models --- gym_ignition/scenario/__init__.py | 6 ++++++ gym_ignition/scenario/model_with_file.py | 17 +++++++++++++++++ gym_ignition/scenario/model_wrapper.py | 20 ++++++++++++++++++++ 3 files changed, 43 insertions(+) create mode 100644 gym_ignition/scenario/__init__.py create mode 100644 gym_ignition/scenario/model_with_file.py create mode 100644 gym_ignition/scenario/model_wrapper.py diff --git a/gym_ignition/scenario/__init__.py b/gym_ignition/scenario/__init__.py new file mode 100644 index 000000000..05c190634 --- /dev/null +++ b/gym_ignition/scenario/__init__.py @@ -0,0 +1,6 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +from . import model_wrapper +from . import model_with_file diff --git a/gym_ignition/scenario/model_with_file.py b/gym_ignition/scenario/model_with_file.py new file mode 100644 index 000000000..d64df1e48 --- /dev/null +++ b/gym_ignition/scenario/model_with_file.py @@ -0,0 +1,17 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import abc + + +class ModelWithFile(abc.ABC): + + def __init__(self): + + super().__init__() + + @classmethod + @abc.abstractmethod + def get_model_file(cls) -> str: + pass diff --git a/gym_ignition/scenario/model_wrapper.py b/gym_ignition/scenario/model_wrapper.py new file mode 100644 index 000000000..a1c1e7458 --- /dev/null +++ b/gym_ignition/scenario/model_wrapper.py @@ -0,0 +1,20 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import abc +from gym_ignition import scenario_bindings as bindings + + +class ModelWrapper(bindings.Model, abc.ABC): + + def __init__(self, model: bindings.Model): + + # No need to call bindings.Model.__init__()! + abc.ABC.__init__(self) + + self.model = model + + def __getattr__(self, name): + + return getattr(self.model, name) From 981285eb315dd09f86fcc8a41f6efe60eb81df64 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Sun, 19 Apr 2020 18:34:30 +0200 Subject: [PATCH 33/35] New cartpole and pendulum Python models They are relay classes that wrap a scenario::base::Model class --- gym_ignition_environments/models/__init__.py | 6 +++ gym_ignition_environments/models/cartpole.py | 48 ++++++++++++++++++++ gym_ignition_environments/models/pendulum.py | 48 ++++++++++++++++++++ 3 files changed, 102 insertions(+) create mode 100644 gym_ignition_environments/models/__init__.py create mode 100644 gym_ignition_environments/models/cartpole.py create mode 100644 gym_ignition_environments/models/pendulum.py diff --git a/gym_ignition_environments/models/__init__.py b/gym_ignition_environments/models/__init__.py new file mode 100644 index 000000000..74aab8250 --- /dev/null +++ b/gym_ignition_environments/models/__init__.py @@ -0,0 +1,6 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +from . import cartpole +from . import pendulum diff --git a/gym_ignition_environments/models/cartpole.py b/gym_ignition_environments/models/cartpole.py new file mode 100644 index 000000000..10058ca37 --- /dev/null +++ b/gym_ignition_environments/models/cartpole.py @@ -0,0 +1,48 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +from typing import List +from gym_ignition import scenario +from gym_ignition import scenario_bindings as bindings +from gym_ignition.utils.scenario import get_unique_model_name + + +class CartPole(scenario.model_wrapper.ModelWrapper, + scenario.model_with_file.ModelWithFile): + + def __init__(self, + world: bindings.World, + position: List[float] = (0.0, 0.0, 0.0), + orientation: List[float] = (1.0, 0, 0, 0), + model_file: str = None): + + # Get a unique model name + model_name = get_unique_model_name(world, "cartpole") + + # Initial pose + initial_pose = bindings.Pose(position, orientation) + + # Get the model file (URDF or SDF) and allow to override it + if model_file is None: + model_file = CartPole.get_model_file() + + # Insert the model + ok_model = world.insertModel(model_file, + initial_pose, + model_name) + + if not ok_model: + raise RuntimeError("Failed to insert model") + + # Get the model + model = world.getModel(model_name) + + # Initialize base class + super().__init__(model=model) + + @classmethod + def get_model_file(cls) -> str: + + import gym_ignition_models + return gym_ignition_models.get_model_file("cartpole") diff --git a/gym_ignition_environments/models/pendulum.py b/gym_ignition_environments/models/pendulum.py new file mode 100644 index 000000000..b33651b9d --- /dev/null +++ b/gym_ignition_environments/models/pendulum.py @@ -0,0 +1,48 @@ +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +from typing import List +from gym_ignition import scenario +from gym_ignition import scenario_bindings as bindings +from gym_ignition.utils.scenario import get_unique_model_name + + +class Pendulum(scenario.model_wrapper.ModelWrapper, + scenario.model_with_file.ModelWithFile): + + def __init__(self, + world: bindings.World, + position: List[float] = (0.0, 0.0, 0.0), + orientation: List[float] = (1.0, 0, 0, 0), + model_file: str = None): + + # Get a unique model name + model_name = get_unique_model_name(world, "pendulum") + + # Initial pose + initial_pose = bindings.Pose(position, orientation) + + # Get the model file (URDF or SDF) and allow to override it + if model_file is None: + model_file = Pendulum.get_model_file() + + # Insert the model + ok_model = world.insertModel(model_file, + initial_pose, + model_name) + + if not ok_model: + raise RuntimeError("Failed to insert model") + + # Get the model + model = world.getModel(model_name) + + # Initialize base class + super().__init__(model=model) + + @classmethod + def get_model_file(cls) -> str: + + import gym_ignition_models + return gym_ignition_models.get_model_file("pendulum") From 4ad6ed9bc43dff277450c72b6716a944039d762f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 20 Apr 2020 12:58:56 +0200 Subject: [PATCH 34/35] Remove pybullet dependency --- setup.py | 1 - 1 file changed, 1 deletion(-) diff --git a/setup.py b/setup.py index 5ff9fb282..a37ed8bdc 100644 --- a/setup.py +++ b/setup.py @@ -146,7 +146,6 @@ def build_extension(self, ext): install_requires=[ 'gym >= 0.13.1', 'numpy', - 'pybullet', 'gym_ignition_models', ], packages=find_packages(), From 9e8f58e4ee3a06baad1e5d227f0a29d9f30b2a04 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 20 Apr 2020 14:44:21 +0200 Subject: [PATCH 35/35] Document utils.h --- .../gazebo/include/scenario/gazebo/utils.h | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h index 286126d8c..f17896672 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h @@ -38,18 +38,129 @@ namespace scenario { namespace gazebo { namespace utils { + /** + * Set the verbosity process-wise. + * + * Accepted levels are the following: + * + * - ``<= 0``: No messages. + * - ``1``: Error messages. + * - ``2``: Error and warning messages. + * - ``3``: Error, warning, and info messages. + * - ``>= 4``: Error, warning, info, and debug messages. + * + * If called without specifying the level, it will use + * level 2 or level 4 depending if the project was compiled + * respectively with Release or Debug flags. + * + * @param level The verbosity level. + */ void setVerbosity(const int level = DEFAULT_VERBOSITY); + + /** + * Find a SDF file in the filesystem. + * + * The search path is defined with the ``IGN_GAZEBO_RESOURCE_PATH`` + * environment variable. + * + * @param fileName The SDF file name. + * @return The absolute path to the file if found, an empty string + * otherwise. + */ std::string findSdfFile(const std::string& fileName); + + /** + * Check if a SDF string is valid. + * + * An SDF string could contain for instance an SDF model or + * an SDF world, and it is valid if it can be parsed successfully + * by the SDFormat library. + * + * @param sdfString The SDF string to check. + * @return True if the SDF string is valid, false otherwise. + */ bool sdfStringValid(const std::string& sdfString); + + /** + * Get an SDF string from a SDF file. + * + * @param fileName An SDF file. It could be either an absolute path + * to the file or the file name if the parent folder is part + * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * @return The SDF string if the file was found and is valid, an + * empty string otherwise. + */ std::string getSdfString(const std::string& fileName); + + /** + * Get the name of a model from a SDF file. + * + * @param fileName An SDF file. It could be either an absolute path + * to the file or the file name if the parent folder is part + * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * @param modelIndex The index of the model in the SDF file. By + * default it finds the first model. + * @return The name of the model. + */ std::string getModelNameFromSdf(const std::string& fileName, const size_t modelIndex = 0); + + /** + * Get the name of a world from a SDF file. + * + * @param fileName An SDF file. It could be either an absolute path + * to the file or the file name if the parent folder is part + * of the ``IGN_GAZEBO_RESOURCE_PATH`` environment variable. + * @param worldIndex The index of the world in the SDF file. By + * default it finds the first world. + * @return The name of the world. + */ std::string getWorldNameFromSdf(const std::string& fileName, const size_t worldIndex = 0); + + /** + * Return a SDF string with an empty world. + * + * An empty world only has a sun and the default DART + * physics profile enabled. + * + * @note The empty world does not have any ground plane. + * + * @return A SDF string with the empty world. + */ std::string getEmptyWorld(); + + /** + * Get a SDF model file from a Fuel URI. + * + * A valid URI has the following form: + * + * ``https://fuel.ignitionrobotics.org/openrobotics/models/model_name`` + * + * @param URI A valid Fuel URI. + * @param useCache Load the model from the local cache. + * @return The absolute path to the SDF model. + */ std::string getModelFileFromFuel(const std::string& URI, const bool useCache = false); + + /** + * Generate a random alpha numeric string. + * + * @param length The length of the string. + * @return The random string. + */ std::string getRandomString(const size_t length); + + /** + * Get the install prefix used by the CMake project. + * + * @note It is defined only if the project is installed in + * Developer mode. + * + * @return A string with the install prefix if the project is + * installed in Developer mode, an empty string otherwise. + */ std::string getInstallPrefix(); } // namespace utils } // namespace gazebo