[Gym] Generic locomotion envs - PART VI (#200)

* [core] More specific Utilities error messages.
* [core] Add support of collision with bodies associated with multiple geometries.
* [misc] Multi-level data folder architecture.
* [misc] Rename gym env_base.py in env_bases.py.
* [python] Enable to specify collision bodies, contact points, and ground model in TOML config file.
* [python] Add IMU sensor to root body by default systematically if no Gazebo IMU has been found.
* [python] BaseJiminyEngine now creates a temporary toml file if not available.
* [gym] Enable to refresh observation and action spaces separately. Enable to use custom PID gains.
* [gym] Remove 'TorqueControl' from walker name since it is the default.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>

core/src/Utilities.cc

@@ -774,36 +774,39 @@ namespace jiminy
         }
 
         auto const & joint = model.joints[idIn];
+        std::string const & jointTypeStr = joint.shortname();
 
-        if (joint.shortname() == "JointModelFreeFlyer")
+        if (jointTypeStr == "JointModelFreeFlyer")
         {
             jointTypeOut = joint_t::FREE;
         }
-        else if (joint.shortname() == "JointModelSpherical")
+        else if (jointTypeStr == "JointModelSpherical")
         {
             jointTypeOut = joint_t::SPHERICAL;
         }
-        else if (joint.shortname() == "JointModelPlanar")
+        else if (jointTypeStr == "JointModelPlanar")
         {
             jointTypeOut = joint_t::PLANAR;
         }
-        else if (joint.shortname() == "JointModelPX" ||
-                    joint.shortname() == "JointModelPY" ||
-                    joint.shortname() == "JointModelPZ")
+        else if (jointTypeStr == "JointModelPX" ||
+                 jointTypeStr == "JointModelPY" ||
+                 jointTypeStr == "JointModelPZ")
         {
             jointTypeOut = joint_t::LINEAR;
         }
-        else if (joint.shortname() == "JointModelRX" ||
-                    joint.shortname() == "JointModelRY" ||
-                    joint.shortname() == "JointModelRZ")
+        else if (jointTypeStr == "JointModelRX" ||
+                 jointTypeStr == "JointModelRY" ||
+                 jointTypeStr == "JointModelRZ" ||
+                 jointTypeStr == "JointModelRevoluteUnaligned")
         {
             jointTypeOut = joint_t::ROTARY;
         }
         else
         {
             // Unknown joint, throw an error to avoid any wrong manipulation.
             jointTypeOut = joint_t::NONE;
-            std::cout << "Error - Utilities::getJointTypeFromIdx - Unknown joint type." << std::endl;
+            std::cout << "Error - Utilities::getJointTypeFromIdx - Unknown joint type '"
+                      <<  jointTypeStr << "'." << std::endl;
             return hresult_t::ERROR_GENERIC;
         }
 
@@ -892,7 +895,8 @@ namespace jiminy
     {
         if (!model.existFrame(frameName))
         {
-            std::cout << "Error - Utilities::getFrameIdx - Frame not found in urdf." << std::endl;
+            std::cout << "Error - Utilities::getFrameIdx - Frame '"
+                      << frameName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -927,7 +931,8 @@ namespace jiminy
     {
         if (!model.existBodyName(bodyName))
         {
-            std::cout << "Error - Utilities::getFrameIdx - Frame not found in urdf." << std::endl;
+            std::cout << "Error - Utilities::getBodyIdx - Body '"
+                      << bodyName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -964,7 +969,8 @@ namespace jiminy
 
         if (!model.existJointName(jointName))
         {
-            std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl;
+            std::cout << "Error - Utilities::getJointPositionIdx - Joint '"
+                      << jointName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -985,7 +991,8 @@ namespace jiminy
 
         if (!model.existJointName(jointName))
         {
-            std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl;
+            std::cout << "Error - Utilities::getJointPositionIdx - Joint '"
+                      << jointName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -1045,7 +1052,8 @@ namespace jiminy
 
         if (!model.existJointName(jointName))
         {
-            std::cout << "Error - Utilities::getJointPositionIdx - Joint not found in urdf." << std::endl;
+            std::cout << "Error - Utilities::getJointModelIdx - Joint '"
+                      << jointName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -1085,7 +1093,8 @@ namespace jiminy
 
         if (!model.existJointName(jointName))
         {
-            std::cout << "Error - getJointVelocityIdx - Frame not found in urdf." << std::endl;
+            std::cout << "Error - getJointVelocityIdx - Joint '"
+                      << jointName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
@@ -1106,7 +1115,8 @@ namespace jiminy
 
         if (!model.existJointName(jointName))
         {
-            std::cout << "Error - getJointVelocityIdx - Frame not found in urdf." << std::endl;
+            std::cout << "Error - getJointVelocityIdx - Joint '"
+                      << jointName << "' not found in robot model." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 

core/src/engine/EngineMultiRobot.cc

@@ -1554,8 +1554,8 @@ namespace jiminy
         || (EPS < controllerUpdatePeriod && controllerUpdatePeriod < SIMULATION_MIN_TIMESTEP)
         || controllerUpdatePeriod > SIMULATION_MAX_TIMESTEP)
         {
-            std::cout << "Error - EngineMultiRobot::setOptions - Cannot simulate a discrete system with update period smaller than"
-                      << SIMULATION_MIN_TIMESTEP << "s or larger than" << SIMULATION_MAX_TIMESTEP << "s. "
+            std::cout << "Error - EngineMultiRobot::setOptions - Cannot simulate a discrete system with update period smaller than "
+                      << SIMULATION_MIN_TIMESTEP << "s or larger than " << SIMULATION_MAX_TIMESTEP << "s. "
                       << "Increase period or switch to continuous mode by setting period to zero." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }

core/src/robot/Model.cc

@@ -226,13 +226,6 @@ namespace jiminy
             return hresult_t::ERROR_INIT_FAILED;
         }
 
-        // Make sure that the body list is not empty
-        if (bodyNames.empty())
-        {
-            std::cout << "Error - Model::addCollisionBodies - The list of bodies must not be empty." << std::endl;
-            return hresult_t::ERROR_BAD_INPUT;
-        }
-
         // Make sure that no body are duplicates
         if (checkDuplicates(bodyNames))
         {
@@ -257,51 +250,27 @@ namespace jiminy
             }
         }
 
-        // Make sure that one and only one geometry is associated with each body
-        for (std::string const & name : bodyNames)
-        {
-            int32_t nChildGeom = 0;
-            for (pinocchio::GeometryObject const & geom : pncGeometryModel_.geometryObjects)
-            {
-                if (pncModel_.frames[geom.parentFrame].name == name)
-                {
-                    nChildGeom++;
-                }
-            }
-            if (nChildGeom != 1)
-            {
-                std::cout << "Error - Model::addCollisionBodies - Collision is only supported for bodies associated with one and only one geometry." << std::endl;
-                return hresult_t::ERROR_BAD_INPUT;
-            }
-        }
-
         // Add the list of bodies to the set of collision bodies
         collisionBodiesNames_.insert(collisionBodiesNames_.end(), bodyNames.begin(), bodyNames.end());
 
         // Create the collision pairs and add them to the geometry model of the robot
         pinocchio::GeomIndex const & groundId = pncGeometryModel_.getGeometryId("ground");
         for (std::string const & name : bodyNames)
         {
-            // Find the body id by looking at the first geometry having it for parent
-            pinocchio::GeomIndex bodyId;
+            // Find the geometries having the body for parent, and add a collision pair for each of them
             for (uint32_t i=0; i<pncGeometryModel_.geometryObjects.size(); ++i)
             {
                 pinocchio::GeometryObject const & geom = pncGeometryModel_.geometryObjects[i];
                 if (pncModel_.frames[geom.parentFrame].name == name)
                 {
-                    bodyId = i;
-                    break;
+                    /* Create and add the collision pair with the ground.
+                       Note that the ground always comes second for the normal to be
+                       consistently compute wrt the ground instead of the body. */
+                    pinocchio::CollisionPair const collisionPair(i, groundId);
+                    pncGeometryModel_.addCollisionPair(collisionPair);
                 }
             }
 
-            /* Create and add the collision pair with the ground.
-               Note that the ground must come first for the normal to be properly computed
-               since the contact information only reports the normal of the second geometry
-               wrt the world, which is the only one that is really interesting since the
-               ground normal never changes for flat ground, as it is the case now. */
-            pinocchio::CollisionPair const collisionPair(bodyId, groundId);
-            pncGeometryModel_.addCollisionPair(collisionPair);
-
             // Refresh proxies associated with the collisions only
             refreshCollisionsProxies();
         }
@@ -345,22 +314,18 @@ namespace jiminy
         pinocchio::GeomIndex const & groundId = pncGeometryModel_.getGeometryId("ground");
         for (std::string const & name : bodyNames)
         {
-            // Find the body id by looking at the first geometry having it for parent
-            pinocchio::GeomIndex bodyId;
+            // Find the geometries having the body for parent, and remove the collision pair for each of them
             for (uint32_t i=0; i<pncGeometryModel_.geometryObjects.size(); ++i)
             {
                 pinocchio::GeometryObject const & geom = pncGeometryModel_.geometryObjects[i];
                 if (pncModel_.frames[geom.parentFrame].name == name)
                 {
-                    bodyId = i;
-                    break;
+                    // Create and remove the collision pair with the ground
+                    pinocchio::CollisionPair const collisionPair(i, groundId);
+                    pncGeometryModel_.removeCollisionPair(collisionPair);
                 }
             }
 
-            // Create and remove the collision pair with the ground
-            pinocchio::CollisionPair const collisionPair(groundId, bodyId);
-            pncGeometryModel_.removeCollisionPair(collisionPair);
-
             // Refresh proxies associated with the collisions only
             refreshCollisionsProxies();
         }
@@ -376,13 +341,6 @@ namespace jiminy
             return hresult_t::ERROR_INIT_FAILED;
         }
 
-        // Make sure that the frame list is not empty
-        if (frameNames.empty())
-        {
-            std::cout << "Error - Model::addContactPoints - The list of frames must not be empty." << std::endl;
-            return hresult_t::ERROR_BAD_INPUT;
-        }
-
         // Make sure that no frame are duplicates
         if (checkDuplicates(frameNames))
         {
@@ -972,9 +930,9 @@ namespace jiminy
         meshPackageDirs_ = meshPackageDirs;
         hasFreeflyer_ = hasFreeflyer;
 
-        // Build the robot model
         try
         {
+            // Build robot physics model
             if (hasFreeflyer)
             {
                 pinocchio::urdf::buildModel(urdfPath,
@@ -985,16 +943,16 @@ namespace jiminy
             {
                 pinocchio::urdf::buildModel(urdfPath, pncModel_);
             }
+
+            // Build robot geometry model
+            pinocchio::urdf::buildGeom(pncModel_, urdfPath, pinocchio::COLLISION, pncGeometryModel_, meshPackageDirs);
         }
         catch (std::exception& e)
         {
             std::cout << "Error - Model::loadUrdfModel - Something is wrong with the URDF. Impossible to build a model from it." << std::endl;
             return hresult_t::ERROR_BAD_INPUT;
         }
 
-        // Build the robot geometry model.
-        pinocchio::urdf::buildGeom(pncModel_, urdfPath, pinocchio::COLLISION, pncGeometryModel_, meshPackageDirs);
-
         // Replace the mesh geometry object by its convex representation for efficiency
         #if PINOCCHIO_MINOR_VERSION >= 4 || PINOCCHIO_PATCH_VERSION >= 4
         for (uint32_t i=0; i<pncGeometryModel_.geometryObjects.size(); ++i)

gym_jiminy/gym_jiminy/__init__.py

@@ -1,18 +1,16 @@
+import jiminy_py # Preload jiminy
 from gym.envs.registration import register
 
 
 register(
     id='jiminy-cartpole-v0',
-    entry_point='gym_jiminy.envs:JiminyCartPoleEnv',
+    entry_point='gym_jiminy.envs:CartPoleJiminyEnv',
     reward_threshold=10000.0
 )
-
 register(
     id='jiminy-acrobot-v0',
-    entry_point='gym_jiminy.envs:JiminyAcrobotEnv',
+    entry_point='gym_jiminy.envs:AcrobotJiminyEnv',
     max_episode_steps=12000,
     reward_threshold=-3000.0
 )
 
-
-import jiminy_py # Preload jiminy to force import of eigenpy and pinocchio