Merge pull request #177 from diegoferigo/refactor/randomizers

Add Randomizers: model and physics

cpp/scenario/gazebo/include/scenario/gazebo/utils.h

@@ -162,6 +162,24 @@ namespace scenario {
              *         installed in Developer mode, an empty string otherwise.
              */
             std::string getInstallPrefix();
+
+            /**
+             * Convert a URDF file to a SDF string.
+             *
+             * @param urdfFile The absolute path to the URDF file.
+             * @return The SDF string if the file exists and it was successfully
+             *         converted, an empty string otherwise.
+             */
+            std::string URDFFileToSDFString(const std::string& urdfFile);
+
+            /**
+             * Convert a URDF string to a SDF string.
+             *
+             * @param urdfFile A URDF string.
+             * @return The SDF string if the URDF string was successfully
+             *         converted, an empty string otherwise.
+             */
+            std::string URDFStringToSDFString(const std::string& urdfString);
         } // namespace utils
     } // namespace gazebo
 } // namespace scenario

cpp/scenario/gazebo/src/utils.cpp

@@ -242,3 +242,25 @@ std::string utils::getInstallPrefix()
     return "";
 #endif
 }
+
+std::string utils::URDFFileToSDFString(const std::string& urdfFile)
+{
+    auto root = getSdfRootFromFile(urdfFile);
+
+    if (!root) {
+        return "";
+    }
+
+    return root->Element()->ToString("");
+}
+
+std::string utils::URDFStringToSDFString(const std::string& urdfString)
+{
+    auto root = getSdfRootFromString(urdfString);
+
+    if (!root) {
+        return "";
+    }
+
+    return root->Element()->ToString("");
+}

examples/python/launch_cartpole.py

@@ -4,30 +4,46 @@
 
 import gym
 import time
+import functools
+from gym_ignition.utils import logger
+from gym_ignition_environments import randomizers
 
-# Set gym verbosity
-gym.logger.set_level(gym.logger.INFO)
-assert gym.logger.set_level(gym.logger.DEBUG) or True
+# Set verbosity
+logger.set_level(gym.logger.ERROR)
+# logger.set_level(gym.logger.DEBUG)
+
+# Available tasks
+env_id = "CartPoleDiscreteBalancing-Gazebo-v0"
+# env_id = "CartPoleContinuousBalancing-Gazebo-v0"
+# env_id = "CartPoleContinuousSwingup-Gazebo-v0"
+
+
+def make_env_from_id(env_id: str, **kwargs) -> gym.Env:
+    import gym
+    import gym_ignition_environments
+    return gym.make(env_id, **kwargs)
 
-# Register gym-ignition environments
-import gym_ignition
-from gym_ignition.utils import logger
 
-# Create the environment
-# env = gym.make("CartPole-v1")
-# env = gym.make("CartPoleDiscrete-Gympp-v0")
-env = gym.make("CartPoleDiscrete-Gazebo-v0")
-# env = gym.make("CartPoleContinuous-Gazebo-v0")
-# env = gym.make("CartPoleDiscrete-PyBullet-v0")
+# Create a partial function passing the environment id
+make_env = functools.partial(make_env_from_id, env_id=env_id)
+
+# Wrap the environment with the randomizer.
+# This is a simple example no randomization are applied.
+env = randomizers.cartpole_no_rand.CartpoleEnvNoRandomizations(env=make_env)
+
+# Wrap the environment with the randomizer.
+# This is a complex example that randomizes both the physics and the model.
+# env = randomizers.cartpole.CartpoleEnvRandomizer(
+#     env=make_env, seed=42, num_physics_rollouts=2)
 
 # Enable the rendering
-env.render('human')
-time.sleep(3)
+# env.render('human')
 
 # Initialize the seed
 env.seed(42)
 
-for epoch in range(30):
+for epoch in range(10):
+
     # Reset the environment
     observation = env.reset()
 
@@ -36,12 +52,13 @@
     totalReward = 0
 
     while not done:
+
         # Execute a random action
         action = env.action_space.sample()
         observation, reward, done, _ = env.step(action)
 
-        # Render the environment
-        # It is not required to call this in the loop
+        # Render the environment.
+        # It is not required to call this in the loop if physics is not randomized.
         # env.render('human')
 
         # Accumulate the reward
@@ -53,7 +70,7 @@
             msg += "\t%.6f" % value
         logger.debug(msg)
 
-    logger.info(f"Total reward for episode #{epoch}: {totalReward}")
+    print(f"Reward episode #{epoch}: {totalReward}")
 
 env.close()
 time.sleep(5)

python/gym_ignition/randomizers/__init__.py

@@ -0,0 +1,9 @@
+# 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 base
+from . import model
+from . import physics
+
+from . import gazebo_env_randomizer

python/gym_ignition/randomizers/base/__init__.py

@@ -0,0 +1,7 @@
+# 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 task
+from . import model
+from . import physics

python/gym_ignition/randomizers/base/model.py

@@ -0,0 +1,27 @@
+# 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 ModelRandomizer(abc.ABC):
+
+    @abc.abstractmethod
+    def randomize_model(self) -> str:
+        """
+        Randomize the model.
+
+        Return:
+            A string with the randomized model.
+        """
+        pass
+
+    def seed_model_randomizer(self, seed: int) -> None:
+        """
+        Seed the randomizer to ensure reproducibility.
+
+        Args:
+            seed: The seed number.
+        """
+        pass

python/gym_ignition/randomizers/base/physics.py

@@ -0,0 +1,72 @@
+# 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 PhysicsRandomizer(abc.ABC):
+    """
+    Abstract class that provides the machinery for randomizing physics in a Ignition
+    Gazebo simulation.
+
+    Args:
+        randomize_after_rollouts_num: defines after many rollouts physics should be
+        randomized (i.e. the amount of times :py:meth:`gym.Env.reset` is called).
+    """
+
+    def __init__(self, randomize_after_rollouts_num: int = 0):
+
+        self._rollout_counter = randomize_after_rollouts_num
+        self.randomize_after_rollouts_num = randomize_after_rollouts_num
+
+    @abc.abstractmethod
+    def randomize_physics(self, world: bindings.World) -> None:
+        """
+        Method that insert and configures the physics of a world.
+
+        By default this method loads a plugin that uses DART with no randomizations.
+        Randomizing physics engine parameters or changing physics engine backend could be
+        done by redefining this method and passing it to
+        :py:class:`~gym_ignition.runtimes.gazebo_runtime.GazeboRuntime`.
+
+        Args:
+            world: A world object without physics.
+        """
+        pass
+
+    def seed_physics_randomizer(self, seed: int) -> None:
+        """
+        Seed the randomizer to ensure reproducibility.
+
+        Args:
+            seed: The seed number.
+        """
+        pass
+
+    def increase_rollout_counter(self) -> None:
+        """
+        Increase the rollouts counter.
+        """
+
+        if self.randomize_after_rollouts_num != 0:
+            assert self._rollout_counter != 0
+            self._rollout_counter -= 1
+
+    def physics_expired(self) -> bool:
+        """
+        Checks if the physics needs to be randomized.
+
+        Return:
+            True if the physics has expired, false otherwise.
+        """
+
+        if self.randomize_after_rollouts_num == 0:
+            return False
+
+        if self._rollout_counter == 0:
+            self._rollout_counter = self.randomize_after_rollouts_num
+            return True
+
+        return False

python/gym_ignition/randomizers/base/task.py

@@ -0,0 +1,38 @@
+# 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 base
+from gym_ignition import scenario_bindings as bindings
+
+
+class TaskRandomizer(abc.ABC):
+
+    @abc.abstractmethod
+    def randomize_task(self,
+                       task: base.task.Task,
+                       gazebo: bindings.GazeboSimulator,
+                       **kwargs) -> None:
+        """
+        Randomize a :py:class:`~gym_ignition.base.task.Task` instance.
+
+        Args:
+            task: the task to randomize.
+            gazebo: a :py:class:`~scenario_bindings.GazeboSimulator` instance.
+
+        Note:
+            Note that each task has a :py:attr:`~gym_ignition.base.task.Task.world`
+            property that provides access to the simulated
+            :py:class:`scenario_bindings.World`.
+        """
+        pass
+
+    def seed_task_randomizer(self, seed: int) -> None:
+        """
+        Seed the randomizer to ensure reproducibility.
+
+        Args:
+            seed: The seed number.
+        """
+        pass