[gym] Make sure environments are fully configured at init. (#229)

* [core] Catch some seldom exceptions. * [core] Enable to call 'initialize' multiple times for 'Engine*'. * [core] 'isSimulationRunning' is set to True at the end of 'start' instead of the beginning. * [core|python] Several critical bug fixes. * [gym] Make sure the environment is completely initialized at '__init__', which is expected by OpenAI Gym API. * [gym] Fix some internal buffersnot properly reset. * [gym] Fix base env '_setup' method not called at reset. * [gym] Send zero motor efforts at reset since the initial action is undefined. Co-authored-by: Alexis Duburcq <[email protected]>
duburcqa · Nov 19, 2020 · c0059de · c0059de
1 parent 2e47f6f
commit c0059de
Show file tree

Hide file tree

Showing 17 changed files with 452 additions and 397 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.4.11)
+set(BUILD_VERSION 1.4.12)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

diff --git a/core/src/control/AbstractController.cc b/core/src/control/AbstractController.cc
@@ -33,8 +33,11 @@ namespace jiminy
             return hresult_t::ERROR_INIT_FAILED;
         }
 
+        // Backup robot
         robot_ = robot;
-        sensorsData_ = robot_->getSensorsData();
+
+        // Reset the controller completely
+        reset(true);
 
         try
         {

diff --git a/core/src/engine/Engine.cc b/core/src/engine/Engine.cc
@@ -23,6 +23,19 @@ namespace jiminy
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
+        // Make sure the simulation is properly stopped
+        if (isSimulationRunning_)
+        {
+            stop();
+        }
+
+        // Remove the existing system if already initialized
+        if(isInitialized_)
+        {
+            removeSystem("");  // It cannot fail at this point
+            isInitialized_ = false;
+        }
+
         /* Add the system without associated name, since
            it is irrelevant for a single robot engine. */
         returnCode = addSystem("", std::move(robot),

diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -67,6 +67,13 @@ namespace jiminy
                                           std::shared_ptr<AbstractController> controller,
                                           callbackFunctor_t callbackFct)
     {
+        // Make sure that no simulation is running
+        if (isSimulationRunning_)
+        {
+            PRINT_ERROR("A simulation is already running. Stop it before adding a new system.")
+            return hresult_t::ERROR_GENERIC;
+        }
+
         if (!robot->getIsInitialized())
         {
             PRINT_ERROR("Robot not initialized.")
@@ -119,8 +126,18 @@ namespace jiminy
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
-        // Remove every coupling forces involving the system
-        returnCode = removeCouplingForces(systemName);
+        // Make sure that no simulation is running
+        if (isSimulationRunning_)
+        {
+            PRINT_ERROR("A simulation is already running. Stop it before removing a system.")
+            returnCode = hresult_t::ERROR_GENERIC;
+        }
+
+        if (returnCode == hresult_t::SUCCESS)
+        {
+            // Remove every coupling forces involving the system
+            returnCode = removeCouplingForces(systemName);
+        }
 
         if (returnCode == hresult_t::SUCCESS)
         {
@@ -195,8 +212,18 @@ namespace jiminy
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
+        // Make sure that no simulation is running
+        if (isSimulationRunning_)
+        {
+            PRINT_ERROR("A simulation is already running. Stop it before adding coupling forces.")
+            returnCode = hresult_t::ERROR_GENERIC;
+        }
+
         int32_t systemIdx1;
-        returnCode = getSystemIdx(systemName1, systemIdx1);
+        if (returnCode == hresult_t::SUCCESS)
+        {
+            returnCode = getSystemIdx(systemName1, systemIdx1);
+        }
 
         int32_t systemIdx2;
         if (returnCode == hresult_t::SUCCESS)
@@ -239,8 +266,18 @@ namespace jiminy
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
+        // Make sure that no simulation is running
+        if (isSimulationRunning_)
+        {
+            PRINT_ERROR("A simulation is already running. Stop it before removing coupling forces.")
+            returnCode = hresult_t::ERROR_GENERIC;
+        }
+
         systemHolder_t * system1;
-        returnCode = getSystem(systemName1, system1);
+        if (returnCode == hresult_t::SUCCESS)
+        {
+            returnCode = getSystem(systemName1, system1);
+        }
 
         if (returnCode == hresult_t::SUCCESS)
         {
@@ -268,8 +305,18 @@ namespace jiminy
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
+        // Make sure that no simulation is running
+        if (isSimulationRunning_)
+        {
+            PRINT_ERROR("A simulation is already running. Stop it before removing coupling forces.")
+            returnCode = hresult_t::ERROR_GENERIC;
+        }
+
         systemHolder_t * system;
-        returnCode = getSystem(systemName, system);
+        if (returnCode == hresult_t::SUCCESS)
+        {
+            returnCode = getSystem(systemName, system);
+        }
 
         if (returnCode == hresult_t::SUCCESS)
         {
@@ -580,9 +627,6 @@ namespace jiminy
         // Reset the robot, controller, engine, and registered impulse forces if requested
         reset(resetRandomNumbers, resetDynamicForceRegister);
 
-        // At this point, consider that the simulation is running
-        isSimulationRunning_ = true;
-
         auto systemIt = systems_.begin();
         auto systemDataIt = systemsDataHolder_.begin();
         for ( ; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
@@ -807,6 +851,14 @@ namespace jiminy
             }
         }
 
+        // At this point, consider that the simulation is running
+        isSimulationRunning_ = true;
+
+        if (returnCode != hresult_t::SUCCESS)
+        {
+            stop();
+        }
+
         return returnCode;
     }
 
@@ -2129,7 +2181,7 @@ namespace jiminy
 
             // Convert contact force from the global frame to the local frame to store it in contactForces_
             pinocchio::SE3 const & transformContactInJoint = system.robot->pncModel_.frames[frameIdx].placement;
-            system.robot->contactForces_[i] = transformContactInJoint.act(fextLocal);
+            system.robot->contactForces_[i] = transformContactInJoint.actInv(fextLocal);
         }
 
         // Compute the force at collision bodies

diff --git a/python/gym_jiminy/common/gym_jiminy/common/block_bases.py b/python/gym_jiminy/common/gym_jiminy/common/block_bases.py
@@ -208,7 +208,9 @@ def _refresh_observation_space(self) -> None:
             it, then using `BaseObserverBlock` or `BlockInterface` directly
             is probably the way to go.
         """
+        # Assertion(s) for type checker
         assert self.env is not None
+
         self.observation_space = self.env.action_space
 
     def reset(self, env: Union[gym.Wrapper, BaseJiminyEnv]) -> None:
@@ -219,10 +221,11 @@ def reset(self, env: Union[gym.Wrapper, BaseJiminyEnv]) -> None:
 
         :param env: Environment to control, eventually already wrapped.
         """
+        # Call base implementation
         super().reset(env)
 
         # Assertion(s) for type checker
-        assert self.env is not None and self.env.control_dt is not None
+        assert self.env is not None
 
         self.control_dt = self.env.control_dt * self.update_ratio
 
@@ -334,7 +337,9 @@ def _refresh_action_space(self) -> None:
             it, then using `BaseControllerBlock` or `BlockInterface` directly
             is probably the way to go.
         """
+        # Assertion(s) for type checker
         assert self.env is not None
+
         self.action_space = self.env.observation_space
 
     def reset(self, env: Union[gym.Wrapper, BaseJiminyEnv]) -> None:
@@ -345,10 +350,11 @@ def reset(self, env: Union[gym.Wrapper, BaseJiminyEnv]) -> None:
 
         :param env: Environment to observe, eventually already wrapped.
         """
+        # Call base implementation
         super().reset(env)
 
         # Assertion(s) for type checker
-        assert self.env is not None and self.env.observe_dt is not None
+        assert self.env is not None
 
         self.observe_dt = self.env.observe_dt * self.update_ratio
 

diff --git a/python/gym_jiminy/common/gym_jiminy/common/control_impl.py b/python/gym_jiminy/common/gym_jiminy/common/control_impl.py
@@ -77,9 +77,6 @@ def _setup(self) -> None:
 
         It updates the mapping from motors to encoders indices.
         """
-        # Assertion(s) for type checker
-        assert self.robot is not None and self.system_state is not None
-
         # Refresh the mapping between the motors and encoders
         encoder_joints = []
         for name in self.robot.sensors_names[encoder.type]: