[python/viewer] Add support of user-specified extra cameras (rgb and depth).

core/include/jiminy/core/utilities/random.h

@@ -451,7 +451,8 @@ namespace jiminy
 
         VectorN<double> shift_{};
 
-        mutable VectorN<int32_t> cellIndex_{};
+        mutable VectorN<int32_t> cellIndex_ =
+            VectorN<int32_t>::Constant(std::numeric_limits<int32_t>::max());
         mutable std::array<VectorN<double>, (1U << N)> gradKnots_{};
     };
 

core/include/jiminy/core/utilities/random.hxx

@@ -239,6 +239,9 @@ namespace jiminy
     {
         // Sample random cell shift
         shift_ = uniform(N, 1, g).cast<double>();
+
+        // Clear cache index
+        cellIndex_.setConstant(std::numeric_limits<int32_t>::max());
     }
 
     template<template<unsigned int> class DerivedPerlinNoiseOctave, unsigned int N>

core/unit/miscellaneous.cc

@@ -1,4 +1,3 @@
-#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
 #include "jiminy/core/fwd.h"
@@ -71,66 +70,3 @@ TEST(Miscellaneous, swapMatrixRows)
                      matrixOut);
     }
 }
-
-
-TEST(Miscellaneous, MatrixRandom)
-{
-    using generator_t =
-        std::independent_bits_engine<std::mt19937, std::numeric_limits<uint32_t>::digits, uint32_t>;
-
-    generator_t gen32{0};
-    uniform_random_bit_generator_ref<uint32_t> gen32_ref = gen32;
-
-    float mean = 5.0;
-    float stddev = 2.0;
-
-    auto mean_vec = Eigen::MatrixXf::Constant(1, 2, mean);
-    auto stddev_vec = Eigen::MatrixXf::Constant(1, 2, stddev);
-    float value1 = normal(gen32, mean, stddev);
-    float value2 = normal(gen32, mean, stddev);
-
-    {
-        gen32.seed(0);
-        scalar_random_op<float(generator_t &, float, float)> op{
-            [](auto & g, float _mean, float _stddev) -> float
-            { return normal(g, _mean, _stddev); },
-            gen32,
-            mean_vec,
-            stddev_vec};
-        ASSERT_FLOAT_EQ(op(0, 0), value1);
-        ASSERT_FLOAT_EQ(op(0, 1), value2);
-        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
-    }
-    {
-        gen32.seed(0);
-        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
-                         uniform_random_bit_generator_ref<uint32_t>,
-                         float,
-                         float>
-            op{normal, gen32, mean, stddev};
-        ASSERT_FLOAT_EQ(op(0, 0), value1);
-        ASSERT_FLOAT_EQ(op(10, 10), value2);
-        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
-    }
-    {
-        gen32.seed(0);
-        auto mat_expr = normal(gen32_ref, mean_vec, stddev_vec);
-        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
-        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
-        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
-    }
-    {
-        gen32.seed(0);
-        auto mat_expr = normal(1, 2, gen32, mean, stddev);
-        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
-        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
-        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
-    }
-    {
-        gen32.seed(0);
-        auto mat_expr = normal(gen32_ref, mean, stddev_vec.transpose());
-        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
-        ASSERT_FLOAT_EQ(mat_expr(1, 0), value2);
-        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
-    }
-}

core/unit/random_test.cc

@@ -1,3 +1,4 @@
+#include <gmock/gmock.h>  // `testing::*`
 #include <gtest/gtest.h>
 
 #include "jiminy/core/utilities/random.h"
@@ -6,32 +7,98 @@
 using namespace jiminy;
 
 static inline constexpr double DELTA{1e-6};
-static inline constexpr double TOL{1e-3};
+static inline constexpr double TOL{1e-4};
+
+
+TEST(Miscellaneous, MatrixRandom)
+{
+    using generator_t =
+        std::independent_bits_engine<std::mt19937, std::numeric_limits<uint32_t>::digits, uint32_t>;
+
+    generator_t gen32{0};
+    uniform_random_bit_generator_ref<uint32_t> gen32_ref = gen32;
+
+    float mean = 5.0;
+    float stddev = 2.0;
+
+    auto mean_vec = Eigen::MatrixXf::Constant(1, 2, mean);
+    auto stddev_vec = Eigen::MatrixXf::Constant(1, 2, stddev);
+    float value1 = normal(gen32, mean, stddev);
+    float value2 = normal(gen32, mean, stddev);
+
+    {
+        gen32.seed(0);
+        scalar_random_op<float(generator_t &, float, float)> op{
+            [](auto & g, float _mean, float _stddev) -> float
+            { return normal(g, _mean, _stddev); },
+            gen32,
+            mean_vec,
+            stddev_vec};
+        ASSERT_FLOAT_EQ(op(0, 0), value1);
+        ASSERT_FLOAT_EQ(op(0, 1), value2);
+        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         uniform_random_bit_generator_ref<uint32_t>,
+                         float,
+                         float>
+            op{normal, gen32, mean, stddev};
+        ASSERT_FLOAT_EQ(op(0, 0), value1);
+        ASSERT_FLOAT_EQ(op(10, 10), value2);
+        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(gen32_ref, mean_vec, stddev_vec);
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(1, 2, gen32, mean, stddev);
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(gen32_ref, mean, stddev_vec.transpose());
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(1, 0), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+}
 
 
 TEST(PerlinNoiseTest, RandomPerlinNoiseOctaveInitialization)
 {
     double wavelength = 10.0;
-    RandomPerlinNoiseOctave<1> randomOctave(wavelength);
+    RandomPerlinNoiseOctave<1> octave(wavelength);
+    octave.reset(PCG32{std::seed_seq{0}});
 
-    EXPECT_DOUBLE_EQ(randomOctave.getWavelength(), wavelength);
+    EXPECT_DOUBLE_EQ(octave.getWavelength(), wavelength);
 }
 
 TEST(PerlinNoiseTest, PeriodicPerlinNoiseOctaveInitialization)
 {
     double wavelength = 10.0;
     double period = 20.0;
-    PeriodicPerlinNoiseOctave<1> periodicOctave(wavelength, period);
+    PeriodicPerlinNoiseOctave<1> octave(wavelength, period);
+    octave.reset(PCG32{std::seed_seq{0}});
 
-    EXPECT_DOUBLE_EQ(periodicOctave.getWavelength(), wavelength);
-    EXPECT_DOUBLE_EQ(periodicOctave.getPeriod(), period);
+    EXPECT_DOUBLE_EQ(octave.getWavelength(), wavelength);
+    EXPECT_DOUBLE_EQ(octave.getPeriod(), period);
 }
 
 TEST(PerlinNoiseTest, RandomGradientCalculation1D)
 {
     {
         double wavelength = 10.0;
         RandomPerlinNoiseOctave<1> octave(wavelength);
+        octave.reset(PCG32{std::seed_seq{0}});
 
         Eigen::Array<double, 1, 1> t{5.43};
         Eigen::Matrix<double, 1, 1> finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) /
@@ -41,6 +108,7 @@ TEST(PerlinNoiseTest, RandomGradientCalculation1D)
     {
         double wavelength = 3.41;
         RandomPerlinNoiseOctave<1> octave(wavelength);
+        octave.reset(PCG32{std::seed_seq{0}});
 
         Eigen::Array<double, 1, 1> t{17.0};
         Eigen::Matrix<double, 1, 1> finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) /
@@ -54,6 +122,7 @@ TEST(PerlinNoiseTest, PeriodicGradientCalculation1D)
     double wavelength = 10.0;
     double period = 30.0;
     PeriodicPerlinNoiseOctave<1> octave(wavelength, period);
+    octave.reset(PCG32{std::seed_seq{0}});
 
     Eigen::Array<double, 1, 1> t{5.43};
     Eigen::Matrix<double, 1, 1> finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) /
@@ -66,6 +135,7 @@ TEST(PerlinNoiseTest, RandomGradientCalculation2D)
 {
     double wavelength = 10.0;
     RandomPerlinNoiseOctave<2> octave(wavelength);
+    octave.reset(PCG32{std::seed_seq{0}});
 
     Eigen::Vector2d pos{5.43, 7.12};
     Eigen::Vector2d finiteDiffGrad{(octave(pos + DELTA * Eigen::Vector2d::UnitX()) -
@@ -83,6 +153,7 @@ TEST(PerlinNoiseTest, PeriodicGradientCalculation2D)
     double wavelength = 10.0;
     double period = 30.0;
     PeriodicPerlinNoiseOctave<2> octave(wavelength, period);
+    octave.reset(PCG32{std::seed_seq{0}});
 
     Eigen::Vector2d pos{5.43, 7.12};
     Eigen::Vector2d finiteDiffGrad{(octave(pos + DELTA * Eigen::Vector2d::UnitX()) -

python/jiminy_py/examples/extra_cameras.py

@@ -0,0 +1,74 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+import gymnasium as gym
+
+from panda3d.core import VBase4, Point3, Vec3
+from jiminy_py.viewer import Viewer
+import pinocchio as pin
+
+Viewer.close()
+#Viewer.connect_backend("panda3d-sync")
+env = gym.make("gym_jiminy.envs:atlas-pid", viewer_kwargs={"backend": "panda3d-sync"})
+env.reset(seed=0)
+env.step(env.action)
+#env.render()
+env.simulator.render(return_rgb_array=True)
+
+env.viewer.add_marker("sphere",
+                      shape="sphere",
+                      pose=(np.array((1.7, 0.0, 1.5)), None),
+                      color="red",
+                      radius=0.1,
+                      always_foreground=False)
+
+Viewer.add_camera("rgb", height=200, width=200, is_depthmap=False)
+Viewer.add_camera("depth", height=128, width=128, is_depthmap=True)
+Viewer.set_camera_transform(
+    position=[2.5, -1.4, 1.6],  # [3.0, 0.0, 0.0],
+    rotation=[1.35, 0.0, 0.8],  # [np.pi/2, 0.0, np.pi/2]
+    camera_name="depth")
+
+frame_index = env.robot.pinocchio_model.getFrameId("head")
+frame_pose = env.robot.pinocchio_data.oMf[frame_index]
+# Viewer._backend_obj.gui.set_camera_transform(
+#     pos=frame_pose.translation + np.array([0.0, 0.0, 0.0]),
+#     quat=pin.Quaternion(frame_pose.rotation @ pin.rpy.rpyToMatrix(0.0, 0.0, -np.pi/2)).coeffs(),
+#     camera_name="rgb")
+Viewer.set_camera_transform(
+    position=frame_pose.translation + np.array([0.0, 0.0, 0.0]),
+    rotation=pin.rpy.matrixToRpy(frame_pose.rotation @ pin.rpy.rpyToMatrix(np.pi/2, 0.0, -np.pi/2)),
+    camera_name="rgb")
+
+lens = Viewer._backend_obj.render.find("user_camera_depth").node().get_lens()
+# proj = lens.get_projection_mat_inv()
+# buffer = Viewer._backend_obj._user_buffers["depth"]
+# buffer.trigger_copy()
+# Viewer._backend_obj.graphics_engine.render_frame()
+# texture = buffer.get_texture()
+# tex_peeker = texture.peek()
+# pixel = VBase4()
+# tex_peeker.lookup(pixel, 0.5, 0.5)  # (y, x normalized coordinates, from top-left to bottom-right)
+# depth_rel = 2.0 * pixel[0] - 1.0   # map range [0.0 (near), 1.0 (far)] to [-1.0, 1.0]
+# point = Point3()
+# #lens.extrude_depth(Point3(0.0, 0.0, depth_rel), point)
+# # proj.xform_point_general(Point3(0.0, 0.0, pixel[0]))
+# # depth = point[1]
+# depth = 1.0 / (proj[2][3] * depth_rel + proj[3][3])
+# print(depth)
+
+rgb_array = Viewer.capture_frame(camera_name="rgb")
+depth_array = Viewer.capture_frame(camera_name="depth")
+# depth_normalized_array = lens.near / (lens.far - (lens.far - lens.near) * depth_array)
+depth_true_array = lens.near / (1.0 - (1.0 - lens.near / lens.far) * depth_array)
+fig = plt.figure(figsize=(10, 5))
+ax1 = fig.add_subplot(121)
+ax1.imshow(rgb_array)
+ax2 = fig.add_subplot(122)
+ax2.imshow(depth_true_array, cmap=plt.cm.binary)
+for ax in (ax1, ax2):
+    ax.axis('off')
+    ax.xaxis.set_visible(False)
+    ax.yaxis.set_visible(False)
+fig.tight_layout(pad=1.0)
+plt.show(block=False)

python/jiminy_py/setup.py

@@ -136,7 +136,7 @@ def finalize_options(self) -> None:
             "meshcat>=0.3.2",
             # Used to detect running Meshcat servers and avoid orphan child
             # processes.
-            "psutil",
+            "psutil>=6.0",
             # Low-level backend for Ipython powering Jupyter notebooks
             "ipykernel>=5.0,<7.0",
             # Used internally by Viewer to read/write Meshcat snapshots