[core] Add stairs terrain

core/include/jiminy/core/utilities/geometry.h

@@ -25,6 +25,29 @@ namespace jiminy
         double yMax,
         double yUnit,
         bool mustSimplify = false);
+
+    /// \brief Unidirectional periodic stairs ground of even parity, consisting of alternating
+    ///        ascending and descending staircases.
+    ///                               _______                         _______
+    ///     __                _______|H      |_______         _______|H      |_______
+    ///       |______________|H                     H|_______|H                     H|_ . . .
+    ///       .       .      .       .       .       .       .       .       .       .
+    ///       .   W   .  W   .   W   .   W   .   W   .   W   .   W   .   W   .   W   .
+    ///       .  i=0  . i=0  .  i=1  .  i=N  . i=N+1 .  i=0  .  i=1  .  i=N  . i=N+1 .
+    ///               |------>
+    ///             x = 0
+    ///
+    /// \details The stairs have identical height and width, and each staircase has an identical
+    ///          step number. This number corresponds to the amount of steps to climb in order to
+    ///          reach the highest steps from the lowest ones. The above ASCII art shows staircases
+    ///          with a step number of two.
+    ///
+    /// \param[in] stepWidth   Width of the steps.
+    /// \param[in] stepHeight  Heigh of the steps.
+    /// \param[in] stepNumber  Number of steps in the ascending or descending direction.
+    /// \param[in] orientation Orientation of the staircases in the XY plane.
+    HeightmapFunction JIMINY_DLLAPI stairs(
+        double stepWidth, double stepHeight, uint32_t stepNumber, double orientation);
 }
 
 #endif  // JIMINY_GEOMETRY_H

core/src/utilities/geometry.cc

@@ -749,4 +749,50 @@ namespace jiminy
             }
         };
     }
+
+    HeightmapFunction stairs(
+        double stepWidth, double stepHeight, uint32_t stepNumber, double orientation)
+    {
+        const double interpDelta = 0.01;
+        const Eigen::Rotation2D<double> rot_mat(orientation);
+
+        return [stepWidth, stepHeight, stepNumber, rot_mat, interpDelta](
+                   const Eigen::Vector2d & pos, double & height, Eigen::Vector3d & normal) -> void
+        {
+            // Compute position in stairs reference frame
+            Eigen::Vector2d posRel = rot_mat.inverse() * pos;
+            const double modPos = std::fmod(std::abs(posRel[0]), stepWidth * stepNumber * 2);
+
+            // Compute the default height and normal
+            uint32_t stairIndex = static_cast<uint32_t>(modPos / stepWidth);
+            int8_t staircaseSlopeSign = 1;
+            if (stairIndex >= stepNumber)
+            {
+                stairIndex = 2 * stepNumber - stairIndex;
+                staircaseSlopeSign = -1;
+            }
+            height = stairIndex * stepHeight;
+            normal = Eigen::Vector3d::UnitZ();
+
+            // Avoid unsupported vertical edge
+            const double posRelOnStep = std::fmod(modPos, stepWidth) / stepWidth;
+            if (1.0 - posRelOnStep < interpDelta)
+            {
+                const double slope = staircaseSlopeSign * stepHeight / interpDelta;
+                // Update height
+                height += slope * (posRelOnStep - (1.0 - interpDelta));
+
+                // Compute the inverse of the normal's Euclidean norm
+                const double normInv = 1.0 / std::sqrt(1.0 + std::pow(slope, 2));
+
+                // Update normal vector
+                // step 1. compute normal in stairs reference frame:
+                // normal << -slope * normInv, 0.0, normInv;
+                // step 2. Rotate normal vector in world plane reference frame:
+                // normal.head<2>() = rot_mat * normal.head<2>();
+                // Or simply in a single operation:
+                normal << -slope * normInv * rot_mat.toRotationMatrix().col(0), normInv;
+            }
+        };
+    }
 }

python/jiminy_py/setup.py

@@ -144,7 +144,7 @@ def finalize_options(self) -> None:
             # Used internally by Viewer to enable recording video
             # programmatically with Meshcat as backend.
             # - 1.43 is broken
-            "playwright!=1.43"
+            "playwright<1.43"
         ],
         "dev": [
             # Generate Python type hints files (aka. stubs) for C extensions.

python/jiminy_pywrap/src/generators.cc

@@ -193,6 +193,7 @@ namespace jiminy::python
         bp::def("random_tile_ground", &tiles,
                                       (bp::arg("size"), "height_max", "interp_delta",
                                        "sparsity", "orientation", "seed"));
+        bp::def("stairs_ground", &stairs, (bp::arg("step_width"), "step_height", "step_number", "orientation"));
         bp::def("sum_heightmaps", &sumHeightmaps, (bp::arg("heightmaps")));
         bp::def("merge_heightmaps", &mergeHeightmaps, (bp::arg("heightmaps")));