diff --git a/CMakeLists.txt b/CMakeLists.txt
index 04f304b83..9f4e3f6dd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.6.14)
+set(BUILD_VERSION 1.6.15)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")
diff --git a/core/include/jiminy/core/Macros.h b/core/include/jiminy/core/Macros.h
index 89060b5b2..1f28249e3 100644
--- a/core/include/jiminy/core/Macros.h
+++ b/core/include/jiminy/core/Macros.h
@@ -30,21 +30,8 @@ namespace jiminy
 {
     // **************** Generic template utilities ******************
 
-    // https://stackoverflow.com/a/34672753/4820605
-    template<template<typename...> class base, typename derived>
-    struct is_base_of_template_impl
-    {
-        template<typename... Ts>
-        static constexpr std::true_type test(base<Ts...> const *);
-        static constexpr std::false_type test(...);
-        using type = decltype(test(std::declval<derived *>()));
-    };
-
-    template<template <typename...> class base, typename derived>
-    using is_base_of_template = typename is_base_of_template_impl<base, derived>::type;
-
     // https://stackoverflow.com/a/37227316/4820605
-    template <class F, class... Args>
+    template<class F, class... Args>
     void do_for(F f, Args... args)
     {
         (f(args), ...);
@@ -83,6 +70,21 @@ namespace jiminy
         return std::static_pointer_cast<T>(shared_from_base(derived));
     }
 
+    // ======================== is_base_of_template ===========================
+
+    // https://stackoverflow.com/a/34672753/4820605
+    template<template<typename...> class base, typename derived>
+    struct is_base_of_template_impl
+    {
+        template<typename... Ts>
+        static constexpr std::true_type test(base<Ts...> const *);
+        static constexpr std::false_type test(...);
+        using type = decltype(test(std::declval<derived *>()));
+    };
+
+    template<template<typename...> class base, typename derived>
+    using is_base_of_template = typename is_base_of_template_impl<base, derived>::type;
+
     // ======================== is_vector ===========================
 
     template<typename T>
@@ -112,36 +114,11 @@ namespace jiminy
     struct is_map : std::false_type {};
 
     template<typename T>
-    struct is_map<T, typename std::enable_if<isMap<T>::value>::type> : std::true_type {};
+    struct is_map<T, typename std::enable_if_t<isMap<T>::value> > : std::true_type {};
 
     template<typename T>
     constexpr bool is_map_v = is_map<T>::value;
 
-    // ========================= is_eigen ===========================
-
-    namespace isEigenObjectDetail
-    {
-        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
-        std::true_type test(Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const *);
-        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
-        std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> > const *);
-        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
-        std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const> const *);
-        std::false_type test(...);
-    }
-
-    template<typename T>
-    struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval<std::add_pointer_t<T> >())) {};
-
-    template<typename T, typename Enable = void>
-    struct is_eigen : public std::false_type {};
-
-    template<typename T>
-    struct is_eigen<T, typename std::enable_if_t<isEigenObject<T>::value> > : std::true_type {};
-
-    template<typename T>
-    constexpr bool is_eigen_v = is_eigen<T>::value;
-
     // ====================== is_not_eigen_expr =======================
 
     // Check it is eigen object has its own storage, otherwise it is an expression
@@ -176,6 +153,52 @@ namespace jiminy
     template<typename T>
     constexpr bool is_eigen_vector_v = is_eigen_vector<T>::value;
 
+    // ====================== is_eigen_ref =======================
+
+    namespace isEigenRefDetail
+    {
+        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
+        std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> > const *);
+        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
+        std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const> const *);
+        std::false_type test(...);
+    }
+
+    template<typename T>
+    struct isEigenRef : public decltype(isEigenRefDetail::test(std::declval<std::add_pointer_t<T> >())) {};
+
+    template<typename T, typename Enable = void>
+    struct is_eigen_ref : std::false_type {};
+
+    template<typename T>
+    struct is_eigen_ref<T, typename std::enable_if_t<isEigenRef<T>::value> > : std::true_type {};
+
+    template<typename T>
+    constexpr bool is_eigen_ref_v = is_eigen_ref<T>::value;
+
+    // ========================= is_eigen ===========================
+
+    namespace isEigenObjectDetail
+    {
+        template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
+        std::true_type test(Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const *);
+        template<typename T>
+        std::enable_if_t<is_eigen_ref_v<T>, std::true_type> test(T const *);
+        std::false_type test(...);
+    }
+
+    template<typename T>
+    struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval<std::add_pointer_t<T> >())) {};
+
+    template<typename T, typename Enable = void>
+    struct is_eigen : public std::false_type {};
+
+    template<typename T>
+    struct is_eigen<T, typename std::enable_if_t<isEigenObject<T>::value> > : std::true_type {};
+
+    template<typename T>
+    constexpr bool is_eigen_v = is_eigen<T>::value;
+
     // =================== is_pinocchio_joint_* ===================
 
     #define IS_PINOCCHIO_JOINT_ENABLE_IF(type, name) \
diff --git a/core/include/jiminy/core/robot/AbstractSensor.h b/core/include/jiminy/core/robot/AbstractSensor.h
index 0c9fc735d..8923594d9 100644
--- a/core/include/jiminy/core/robot/AbstractSensor.h
+++ b/core/include/jiminy/core/robot/AbstractSensor.h
@@ -215,31 +215,8 @@ namespace jiminy
         ///////////////////////////////////////////////////////////////////////////////////////////////
         configHolder_t getOptions(void) const;
 
-        ///////////////////////////////////////////////////////////////////////////////////////////////
-        ///
-        /// \brief      Request every sensors of the same type than the current one to record data
-        ///             based of the input data.
-        ///
-        /// \details    It assumes that the internal state of the robot is consistent with the
-        ///             input arguments.
-        ///
-        /// \remark     This method is not intended to be called manually. The Robot to which the
-        ///             sensor is added is taking care of it while updating the state of the sensors.
-        ///
-        /// \param[in]  t       Current time.
-        /// \param[in]  q       Current configuration vector of the motor.
-        /// \param[in]  v       Current velocity vector of the motor.
-        /// \param[in]  a       Current acceleration vector of the motor.
-        /// \param[in]  uMotor  Current motor effort vector of the motor.
-        ///
-        /// \return     Return code to determine whether the execution of the method was successful.
-        ///
-        ///////////////////////////////////////////////////////////////////////////////////////////////
-        virtual hresult_t setAll(float64_t const & t,
-                                 vectorN_t const & q,
-                                 vectorN_t const & v,
-                                 vectorN_t const & a,
-                                 vectorN_t const & uMotor) = 0;
+        template<typename DerivedType>
+        hresult_t set(Eigen::MatrixBase<DerivedType> const & value);
 
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///
@@ -331,6 +308,32 @@ namespace jiminy
         virtual std::vector<std::string> const & getFieldnames(void) const = 0;
 
     protected:
+        ///////////////////////////////////////////////////////////////////////////////////////////////
+        ///
+        /// \brief      Request every sensors of the same type than the current one to record data
+        ///             based of the input data.
+        ///
+        /// \details    It assumes that the internal state of the robot is consistent with the
+        ///             input arguments.
+        ///
+        /// \remark     This method is not intended to be called manually. The Robot to which the
+        ///             sensor is added is taking care of it while updating the state of the sensors.
+        ///
+        /// \param[in]  t       Current time.
+        /// \param[in]  q       Current configuration vector of the motor.
+        /// \param[in]  v       Current velocity vector of the motor.
+        /// \param[in]  a       Current acceleration vector of the motor.
+        /// \param[in]  uMotor  Current motor effort vector of the motor.
+        ///
+        /// \return     Return code to determine whether the execution of the method was successful.
+        ///
+        ///////////////////////////////////////////////////////////////////////////////////////////////
+        virtual hresult_t setAll(float64_t const & t,
+                                 vectorN_t const & q,
+                                 vectorN_t const & v,
+                                 vectorN_t const & a,
+                                 vectorN_t const & uMotor) = 0;
+
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///
         /// \brief      Request the sensor to record data based of the input data.
@@ -453,13 +456,13 @@ namespace jiminy
         virtual uint64_t getSize(void) const override final;
 
         virtual Eigen::Ref<vectorN_t const> get(void) const override final;
+
+    protected:
         virtual hresult_t setAll(float64_t const & t,
                                  vectorN_t const & q,
                                  vectorN_t const & v,
                                  vectorN_t const & a,
                                  vectorN_t const & uMotor) override final;
-
-    protected:
         virtual Eigen::Ref<vectorN_t> get(void) override final;
         virtual Eigen::Ref<vectorN_t> data(void) override final;
 
diff --git a/core/include/jiminy/core/robot/AbstractSensor.tpp b/core/include/jiminy/core/robot/AbstractSensor.tpp
index 22b3e1ab3..7877bcf0b 100644
--- a/core/include/jiminy/core/robot/AbstractSensor.tpp
+++ b/core/include/jiminy/core/robot/AbstractSensor.tpp
@@ -6,7 +6,32 @@
 
 namespace jiminy
 {
-    template <typename T>
+    // ========================== AbstractSensorBase ==============================
+
+    template<typename DerivedType>
+    hresult_t AbstractSensorBase::set(Eigen::MatrixBase<DerivedType> const & value)
+    {
+        if (!isAttached_)
+        {
+            PRINT_ERROR("Sensor not attached to any robot.");
+            return hresult_t::ERROR_GENERIC;
+        }
+
+        auto robot = robot_.lock();
+        if (!robot || robot->getIsLocked())
+        {
+            PRINT_ERROR("Robot is locked, probably because a simulation is running. "
+                        "Please stop it before setting sensor value manually.");
+            return hresult_t::ERROR_GENERIC;
+        }
+
+        get() = value;
+        return hresult_t::SUCCESS;
+    }
+
+    // ========================== AbstractSensorTpl ===============================
+
+    template<typename T>
     AbstractSensorTpl<T>::AbstractSensorTpl(std::string const & name) :
     AbstractSensorBase(name),
     sensorIdx_(-1),
@@ -15,7 +40,7 @@ namespace jiminy
         // Empty
     }
 
-    template <typename T>
+    template<typename T>
     AbstractSensorTpl<T>::~AbstractSensorTpl(void)
     {
         // Detach the sensor before deleting it if necessary
@@ -25,7 +50,7 @@ namespace jiminy
         }
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::attach(std::weak_ptr<Robot const> robot,
                                            SensorSharedDataHolder_t * sharedHolder)
     {
@@ -69,7 +94,7 @@ namespace jiminy
         return hresult_t::SUCCESS;
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::detach(void)
     {
         // Delete the part of the shared memory associated with the sensor
@@ -134,7 +159,7 @@ namespace jiminy
         return hresult_t::SUCCESS;
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::resetAll(void)
     {
         // Make sure the sensor is attached to a robot
@@ -183,7 +208,7 @@ namespace jiminy
         return hresult_t::SUCCESS;
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::setOptionsAll(configHolder_t const & sensorOptions)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
@@ -205,30 +230,30 @@ namespace jiminy
         return returnCode;
     }
 
-    template <typename T>
+    template<typename T>
     int32_t const & AbstractSensorTpl<T>::getIdx(void) const
     {
         return sensorIdx_;
     }
 
-    template <typename T>
+    template<typename T>
     std::string const & AbstractSensorTpl<T>::getType(void) const
     {
         return type_;
     }
 
-    template <typename T>
+    template<typename T>
     std::vector<std::string> const & AbstractSensorTpl<T>::getFieldnames(void) const
     {
         return fieldNames_;
     }
 
-    template <typename T>
+    template<typename T>
     uint64_t AbstractSensorTpl<T>::getSize(void) const
     {
         return fieldNames_.size();
     }
-    template <typename T>
+    template<typename T>
     std::string AbstractSensorTpl<T>::getTelemetryName(void) const
     {
         if (areFieldnamesGrouped_)
@@ -241,11 +266,11 @@ namespace jiminy
         }
     }
 
-    template <typename T>
-    inline Eigen::Ref<vectorN_t const> AbstractSensorTpl<T>::get(void) const
+    template<typename T>
+    Eigen::Ref<vectorN_t const> AbstractSensorTpl<T>::get(void) const
     {
         static vectorN_t dataDummy = vectorN_t::Zero(fieldNames_.size());
-        if (sharedHolder_)
+        if (isAttached_)
         {
             return sharedHolder_->dataMeasured_.col(sensorIdx_);
         }
@@ -259,14 +284,14 @@ namespace jiminy
         return sharedHolder_->dataMeasured_.col(sensorIdx_);
     }
 
-    template <typename T>
+    template<typename T>
     inline Eigen::Ref<vectorN_t> AbstractSensorTpl<T>::data(void)
     {
         // No guard, since this method is not public
         return sharedHolder_->data_.back().col(sensorIdx_);
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::interpolateData(void)
     {
         assert(sharedHolder_->time_.size() > 0 && "Do data to interpolate.");
@@ -373,7 +398,7 @@ namespace jiminy
         return hresult_t::SUCCESS;
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::generateMeasurementAll(void)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
@@ -396,7 +421,7 @@ namespace jiminy
         return returnCode;
     }
 
-    template <typename T>
+    template<typename T>
     hresult_t AbstractSensorTpl<T>::setAll(float64_t const & t,
                                            vectorN_t const & q,
                                            vectorN_t const & v,
@@ -486,7 +511,7 @@ namespace jiminy
         return returnCode;
     }
 
-    template <typename T>
+    template<typename T>
     void AbstractSensorTpl<T>::updateTelemetryAll(void)
     {
         for (AbstractSensorBase * sensor : sharedHolder_->sensors_)
diff --git a/core/include/jiminy/core/stepper/LieGroup.h b/core/include/jiminy/core/stepper/LieGroup.h
index 9600db1fe..46fe10930 100644
--- a/core/include/jiminy/core/stepper/LieGroup.h
+++ b/core/include/jiminy/core/stepper/LieGroup.h
@@ -885,7 +885,7 @@ namespace Eigen
             // Empty on purpose
         }
 
-        template <typename ... Args>
+        template<typename ... Args>
         explicit VectorContainer(std::vector<Robot const *> const & robots,
                                  std::vector<Args> const & ... args) :
         vector_()
@@ -900,7 +900,7 @@ namespace Eigen
             }
         }
 
-        template <typename ... Args>
+        template<typename ... Args>
         explicit VectorContainer(std::vector<Robot const *> const & robots,
                                  std::vector<Args> && ... args) :
         vector_()
diff --git a/core/include/jiminy/core/telemetry/TelemetryData.h b/core/include/jiminy/core/telemetry/TelemetryData.h
index 859dd8665..39b1bd63a 100644
--- a/core/include/jiminy/core/telemetry/TelemetryData.h
+++ b/core/include/jiminy/core/telemetry/TelemetryData.h
@@ -145,7 +145,7 @@ namespace jiminy
         ///
         /// \return S_OK if successful, the corresponding telemetry error otherwise.
         ////////////////////////////////////////////////////////////////////////
-        template <typename T>
+        template<typename T>
         hresult_t registerVariable(std::string const   & variableNameIn,
                                    T                 * & positionInBufferOut);
 
@@ -191,7 +191,7 @@ namespace jiminy
         ///
         /// \return S_OK if successful, the corresponding telemetry error otherwise.
         ////////////////////////////////////////////////////////////////////////
-        template <typename T>
+        template<typename T>
         hresult_t internalRegisterVariable(struct memHeader       *   header,
                                            std::string      const   & variableNameIn,
                                            T                      * & positionInBufferOut);
diff --git a/core/include/jiminy/core/telemetry/TelemetryData.tpp b/core/include/jiminy/core/telemetry/TelemetryData.tpp
index 155c46417..e86ec61de 100644
--- a/core/include/jiminy/core/telemetry/TelemetryData.tpp
+++ b/core/include/jiminy/core/telemetry/TelemetryData.tpp
@@ -13,7 +13,7 @@
 
 namespace jiminy
 {
-    template <typename T>
+    template<typename T>
     hresult_t TelemetryData::internalRegisterVariable(struct memHeader       *   header,
                                                       std::string      const   & variableName,
                                                       T                      * & positionInBufferOut)
diff --git a/core/include/jiminy/core/telemetry/TelemetrySender.h b/core/include/jiminy/core/telemetry/TelemetrySender.h
index 6eecba47b..d3c70e2f5 100644
--- a/core/include/jiminy/core/telemetry/TelemetrySender.h
+++ b/core/include/jiminy/core/telemetry/TelemetrySender.h
@@ -44,7 +44,7 @@ namespace jiminy
         hresult_t registerVariable(std::string const & fieldname,
                                    T           const & initialValue);
 
-        template <typename Derived>
+        template<typename Derived>
         hresult_t registerVariable(std::vector<std::string>   const & fieldnames,
                                    Eigen::MatrixBase<Derived> const & values);
 
@@ -54,11 +54,11 @@ namespace jiminy
         /// \param[in]  fieldname  Name of the value to update.
         /// \param[in]  value      Updated value of the variable.
         ////////////////////////////////////////////////////////////////////////
-        template <typename T>
+        template<typename T>
         void updateValue(std::string const & fieldname,
                          T           const & value);
 
-        template <typename Derived>
+        template<typename Derived>
         void updateValue(std::vector<std::string>   const & fieldnames,
                          Eigen::MatrixBase<Derived> const & values);
 
diff --git a/core/include/jiminy/core/telemetry/TelemetrySender.tpp b/core/include/jiminy/core/telemetry/TelemetrySender.tpp
index b42fd29e0..e57b5ec9b 100644
--- a/core/include/jiminy/core/telemetry/TelemetrySender.tpp
+++ b/core/include/jiminy/core/telemetry/TelemetrySender.tpp
@@ -7,7 +7,7 @@
 
 namespace jiminy
 {
-    template <typename Derived>
+    template<typename Derived>
     hresult_t TelemetrySender::registerVariable(std::vector<std::string>   const & fieldnames,
                                                 Eigen::MatrixBase<Derived> const & initialValues)
     {
@@ -22,7 +22,7 @@ namespace jiminy
         return returnCode;
     }
 
-    template <typename Derived>
+    template<typename Derived>
     void TelemetrySender::updateValue(std::vector<std::string>   const & fieldnames,
                                       Eigen::MatrixBase<Derived> const & values)
     {
diff --git a/core/include/jiminy/core/utilities/Helpers.h b/core/include/jiminy/core/utilities/Helpers.h
index 19cd69138..bca986550 100644
--- a/core/include/jiminy/core/utilities/Helpers.h
+++ b/core/include/jiminy/core/utilities/Helpers.h
@@ -105,7 +105,7 @@ namespace jiminy
     // ********************** Math utilities *************************
 
     template<typename T0, typename T1, typename... Ts>
-    typename std::common_type<T0, T1, Ts...>::type min(T0 && val1, T1 && val2, Ts &&... vs);
+    typename std::common_type_t<T0, T1, Ts...> min(T0 && val1, T1 && val2, Ts &&... vs);
 
     template<typename DerivedType>
     auto clamp(Eigen::MatrixBase<DerivedType> const & data,
diff --git a/core/include/jiminy/core/utilities/Helpers.tpp b/core/include/jiminy/core/utilities/Helpers.tpp
index b4946770d..60d9ca71a 100644
--- a/core/include/jiminy/core/utilities/Helpers.tpp
+++ b/core/include/jiminy/core/utilities/Helpers.tpp
@@ -13,7 +13,7 @@ namespace jiminy
     }
 
     template<typename T0, typename T1, typename... Ts>
-    typename std::common_type<T0, T1, Ts...>::type min(T0 && val1, T1 && val2, Ts &&... vs)
+    typename std::common_type_t<T0, T1, Ts...> min(T0 && val1, T1 && val2, Ts &&... vs)
     {
         return min(std::min(val1, val2), std::forward<Ts>(vs)...);
     }
diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc
index d6dbee33d..411593eec 100644
--- a/core/src/robot/AbstractMotor.cc
+++ b/core/src/robot/AbstractMotor.cc
@@ -271,7 +271,7 @@ namespace jiminy
     float64_t const & AbstractMotorBase::get(void) const
     {
         static float64_t dataEmpty;
-        if (sharedHolder_)
+        if (isAttached_)
         {
             return sharedHolder_->data_[motorIdx_];
         }
diff --git a/core/src/telemetry/TelemetrySender.cc b/core/src/telemetry/TelemetrySender.cc
index ecc8f509d..0797145c9 100644
--- a/core/src/telemetry/TelemetrySender.cc
+++ b/core/src/telemetry/TelemetrySender.cc
@@ -23,7 +23,7 @@ namespace jiminy
         // Empty.
     }
 
-    template <>
+    template<>
     void TelemetrySender::updateValue<int64_t>(std::string const & fieldNameIn,
                                                int64_t     const & value)
     {
@@ -38,7 +38,7 @@ namespace jiminy
         *(it->second) = value;
     }
 
-    template <>
+    template<>
     void TelemetrySender::updateValue<float64_t>(std::string const & fieldNameIn,
                                                  float64_t   const & value)
     {
diff --git a/core/src/utilities/Json.cc b/core/src/utilities/Json.cc
index 2fd5eb472..03d63487c 100644
--- a/core/src/utilities/Json.cc
+++ b/core/src/utilities/Json.cc
@@ -75,7 +75,7 @@ namespace jiminy
 
         ~AppendBoostVariantToJson(void) = default;
 
-        template <typename T>
+        template<typename T>
         void operator()(T const & value)
         {
             root_[field_] = convertToJson(value);
diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
index 6186cdc8b..d096458ce 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
@@ -811,6 +811,13 @@ def replay(self, enable_travelling: bool = True, **kwargs: Any) -> None:
             kwargs['mode'] = 'rgb_array'
             kwargs['close_backend'] = not self.simulator.is_viewer_available
 
+        # Stop any running simulation before replay if `is_done` is True.
+        # It will enable to display contact forces.
+        if self.simulator.is_simulation_running:
+            if self.is_done():
+                if not self.viewer or self.viewer._display_contacts:
+                    self.simulator.stop()
+
         # Call render before replay in order to take into account custom
         # backend viewer instantiation options, such as initial camera pose.
         self.render(**kwargs)
diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py
index ed91ae359..062adfdf4 100644
--- a/python/gym_jiminy/unit_py/test_pipeline_control.py
+++ b/python/gym_jiminy/unit_py/test_pipeline_control.py
@@ -50,7 +50,8 @@ def configure_telemetry() -> None:
             self.env.step(action_init)
 
         # Get the final posture of the robot as an RGB array
-        rgb_array = self.env.render(mode='rgb_array')
+        rgb_array = self.env.render(
+            mode='rgb_array', display_com=False, display_contacts=False)
 
         # Check that the final posture matches the expected one
         data_dir = os.path.join(os.path.dirname(__file__), "data")
diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py
index 0e482f506..228c00057 100644
--- a/python/jiminy_py/src/jiminy_py/log.py
+++ b/python/jiminy_py/src/jiminy_py/log.py
@@ -6,15 +6,27 @@
 from csv import DictReader
 from textwrap import dedent
 from itertools import cycle
+from bisect import bisect_right
 from collections import OrderedDict
-from typing import Tuple, Dict, Optional, Any
+from typing import Callable, Tuple, Dict, Optional, Any, Sequence
 
 import h5py
 import numpy as np
 import matplotlib.pyplot as plt
 from matplotlib.lines import Line2D
+from typing_extensions import TypedDict
 
-from .core import Engine, Robot, load_config_json_string
+from . import core as jiminy
+from .state import State
+
+
+class TrajectoryDataType(TypedDict, total=False):
+    # List of State objects of increasing time.
+    evolution_robot: Sequence[State]
+    # Jiminy robot. None if omitted.
+    robot: Optional[jiminy.Robot]
+    # Whether to use theoretical or actual model
+    use_theoretical_model: bool
 
 
 def _is_log_binary(fullpath: str) -> bool:
@@ -69,7 +81,7 @@ def read_log(fullpath: str,
 
     if file_format == 'binary':
         # Read binary file using C++ parser.
-        data_dict, const_dict = Engine.read_log_binary(fullpath)
+        data_dict, const_dict = jiminy.Engine.read_log_binary(fullpath)
     elif file_format == 'csv':
         # Read text csv file.
         const_dict = {}
@@ -126,7 +138,56 @@ def parse_constant(key: str, value: str) -> Any:
     return data_dict, const_dict
 
 
-def build_robot_from_log(log_file: str) -> Robot:
+def extract_viewer_data_from_log(log_data: Dict[str, np.ndarray],
+                                 robot: jiminy.Model) -> TrajectoryDataType:
+    """Extract the minimal required information from raw log data in order to
+    replay the simulation in a viewer.
+
+    It extracts only the required data for replay, namely the evolution over
+    time of the joints positions.
+
+    :param log_data: Data from the log file, in a dictionnary.
+    :param robot: Jiminy robot.
+
+    :returns: Trajectory dictionary. The actual trajectory corresponds to the
+              field "evolution_robot" and it is a list of State object. The
+              other fields are additional information.
+    """
+    t = log_data["Global.Time"]
+    try:
+        # Extract the joint positions evolution over time
+        qe = np.stack([log_data[".".join(("HighLevelController", s))]
+                       for s in robot.logfile_position_headers], axis=-1)
+
+        # Determine whether to use the theoretical or flexible model
+        use_theoretical_model = not robot.is_flexible
+
+        # Create state sequence
+        evolution_robot = []
+        for t_i, q_i in zip(t, qe):
+            evolution_robot.append(State(t=t_i, q=q_i))
+
+        traj_data = {'evolution_robot': evolution_robot,
+                     'robot': robot,
+                     'use_theoretical_model': use_theoretical_model}
+    except KeyError:  # The current options are inconsistent with log data
+        # Toggle flexibilities
+        model_options = robot.get_model_options()
+        dyn_options = model_options['dynamics']
+        dyn_options['enableFlexibleModel'] = not robot.is_flexible
+        robot.set_model_options(model_options)
+
+        # Get viewer data
+        traj_data = extract_viewer_data_from_log(log_data, robot)
+
+        # Restore back flexibilities
+        dyn_options['enableFlexibleModel'] = not robot.is_flexible
+        robot.set_model_options(model_options)
+
+    return traj_data
+
+
+def build_robot_from_log(log_file: str) -> jiminy.Robot:
     """Extract log data and build robot from it.
 
     .. note::
@@ -161,7 +222,7 @@ def build_robot_from_log(log_file: str) -> Robot:
             "HighLevelController.mesh_package_dirs"].split(";")
     else:
         mesh_package_dirs = []
-    all_options = load_config_json_string(
+    all_options = jiminy.load_config_json_string(
         log_constants["HighLevelController.options"])
 
     # Create temporary URDF file
@@ -171,7 +232,7 @@ def build_robot_from_log(log_file: str) -> Robot:
     os.close(fd)
 
     # Build robot
-    robot = Robot()
+    robot = jiminy.Robot()
     robot.initialize(urdf_path, has_freeflyer, mesh_package_dirs)
     robot.set_options(all_options["system"]["robot"])
     robot.pinocchio_model.loadFromString(pinocchio_model_str)
@@ -179,6 +240,53 @@ def build_robot_from_log(log_file: str) -> Robot:
     return robot, (log_data, log_constants)
 
 
+def emulate_sensors_data_from_log(log_data: Dict[str, np.ndarray],
+                                  robot: jiminy.Model
+                                  ) -> Callable[[float], None]:
+    """Helper to make it easy to emulate sensor data update based on log data.
+
+    .. note::
+        It returns an update_hook that can forwarding the `Viewer.replay` to
+        display sensor information such as contact forces for instance.
+
+    :param log_data: Data from the log file, in a dictionnary.
+    :param robot: Jiminy robot.
+
+    :returns: Callable taking update time in argument and returning nothing.
+              Note that it does not through an exception if out-of-range, but
+              rather clip to desired time to the available data range.
+    """
+    # Filter sensors whose data is available
+    sensors_set, sensors_log = [], []
+    for sensor_type, sensors_names in robot.sensors_names.items():
+        sensor_fieldnames = getattr(jiminy, sensor_type).fieldnames
+        for name in sensors_names:
+            sensor = robot.get_sensor(sensor_type, name)
+            if any(field.startswith(sensor.name) for field in log_data.keys()):
+                sensor_log = np.stack([
+                    log_data['.'.join((sensor.name, field))]
+                    for field in sensor_fieldnames], axis=-1)
+                sensors_set.append(sensor)
+                sensors_log.append(sensor_log)
+
+    def update_hook(t: float) -> None:
+        nonlocal sensors_set, sensors_log
+
+        # Get current time ratio and surrounding indices in log data
+        times = log_data['Global.Time']
+        i = bisect_right(times, t)
+        i_prev, i_next = max(i - 1, 0), min(i, len(times) - 1)
+        ratio = (t - times[i_prev]) / (times[i_next] - times[i_prev])
+
+        # Update sensors data
+        for sensor, sensor_log, in zip(sensors_set, sensors_log):
+            value_prev, value_next = sensor_log[i_prev], sensor_log[i_next]
+            value = value_prev + (value_next - value_prev) * ratio
+            sensor.data = value
+
+    return update_hook
+
+
 def plot_log():
     parser = argparse.ArgumentParser(
         formatter_class=argparse.RawTextHelpFormatter, description=dedent("""
diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py
index d1bfae587..9f4733ae9 100644
--- a/python/jiminy_py/src/jiminy_py/simulator.py
+++ b/python/jiminy_py/src/jiminy_py/simulator.py
@@ -459,7 +459,8 @@ def render(self,
                               Yaw] corresponding to the absolute pose of the
                               camera. None to disable.
                               Optional:None by default.
-        :param kwargs: Used argument to allow chaining renderining methods.
+        :param kwargs: Extra keyword arguments to forward at `Viewer`
+                       initialization.
 
         :returns: Rendering as an RGB array (3D numpy array), if enabled, None
                   otherwise.
@@ -483,6 +484,8 @@ def render(self,
                                  **{'scene_name': scene_name,
                                     'robot_name': robot_name,
                                     'backend': self.viewer_backend,
+                                    'display_com': True,
+                                    'display_contacts': True,
                                     'delete_robot_on_close': True,
                                     **kwargs})
             self.viewer_backend = Viewer.backend  # Just in case it was `None`
@@ -517,10 +520,16 @@ def replay(self, **kwargs: Any) -> None:
                 "`replay` method.")
         self.render(**{
             'return_rgb_array': kwargs.get(
-                'record_video_path', None) is not None, **kwargs})
-        play_logs_data(
-            [self.robot], [self.log_data], viewers=[self.viewer],
-            **{'verbose': True, 'backend': self.viewer_backend, **kwargs})
+                'record_video_path', None) is not None,
+            **kwargs})
+        play_logs_data([self.robot],
+                       [self.log_data],
+                       viewers=[self.viewer],
+                       **{'verbose': True,
+                          'backend': self.viewer_backend,
+                          'display_com': True,
+                          'display_contacts': True,
+                          **kwargs})
 
     def close(self) -> None:
         """Close the connection with the renderer.
diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
index 5cd289213..81893e131 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
@@ -22,7 +22,9 @@
     OmniBoundingVolume, CompassEffect, BillboardEffect, Filename, TextNode,
     Texture, TextureStage, PNMImageHeader, PGTop, Camera, PerspectiveLens,
     TransparencyAttrib, OrthographicLens, ClockObject, GraphicsPipe,
-    WindowProperties, FrameBufferProperties, loadPrcFileData, AntialiasAttrib)
+    WindowProperties, FrameBufferProperties, loadPrcFileData, AntialiasAttrib,
+    CollisionNode, CollisionRay, CollisionTraverser, CollisionHandlerQueue,
+    RenderModeAttrib)
 from direct.showbase.ShowBase import ShowBase
 from direct.gui.OnscreenImage import OnscreenImage
 from direct.gui.OnscreenText import OnscreenText
@@ -181,11 +183,11 @@ def make_cone(num_sides: int = 16) -> Geom:
     # https://discourse.panda3d.org/t/procedurally-generated-geometry-and-the-default-normals/24986/2
     prim = GeomTriangles(Geom.UH_static)
     for i in range(num_sides - 1):
-        prim.addVertices(i, i + 1, num_sides)
-        prim.addVertices(i + 1, i, num_sides + 1)
+        prim.add_vertices(i, i + 1, num_sides)
+        prim.add_vertices(i + 1, i, num_sides + 1)
 
     geom = Geom(vdata)
-    geom.addPrimitive(prim)
+    geom.add_primitive(prim)
     return geom
 
 
@@ -223,14 +225,15 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
 
         # Configure rendering
         self.render.set_shader_auto()
-        self.render.set_antialias(AntialiasAttrib.MAuto)
+        self.render.set_antialias(
+            AntialiasAttrib.M_auto | AntialiasAttrib.M_faster)
 
         # Define default camera pos
         self._camera_defaults = CAMERA_POS_DEFAULT
         self.reset_camera(*self._camera_defaults)
 
         # Define clock. It will be used later to limit framerate.
-        self.clock = ClockObject.getGlobalClock()
+        self.clock = ClockObject.get_global_clock()
         self.framerate = None
 
         # Configure lighting and shadows
@@ -280,20 +283,20 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
 
         # Create shared 2D renderer to allow display selectively gui elements
         # on offscreen and onscreen window used for capturing frames.
-        self.sharedRender2d = NodePath('sharedRender2d')
-        self.sharedRender2d.setDepthTest(False)
-        self.sharedRender2d.setDepthWrite(False)
+        self.shared_render_2d = NodePath('shared_render_2d')
+        self.shared_render_2d.set_depth_test(False)
+        self.shared_render_2d.set_depth_write(False)
 
         # Create dedicated camera 2D for offscreen rendering
-        self.offCamera2d = NodePath(Camera('off_camera2d'))
+        self.offscreen_camera_2d = NodePath(Camera('offscreen_camera2d'))
         lens = OrthographicLens()
-        lens.setFilmSize(2, 2)
-        lens.setNearFar(-1000, 1000)
-        self.offCamera2d.node().setLens(lens)
-        self.offCamera2d.reparentTo(self.sharedRender2d)
+        lens.set_film_size(2, 2)
+        lens.set_near_far(-1000, 1000)
+        self.offscreen_camera_2d.node().set_lens(lens)
+        self.offscreen_camera_2d.reparent_to(self.shared_render_2d)
 
         # Create dedicated aspect2d for offscreen rendering
-        self.offAspect2d = self.sharedRender2d.attach_new_node(
+        self.offAspect2d = self.shared_render_2d.attach_new_node(
             PGTop("offAspect2d"))
         self.offA2dTopLeft = self.offAspect2d.attach_new_node(
             "offA2dTopLeft")
@@ -308,18 +311,31 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         self.offA2dBottomLeft.set_pos(self.a2dLeft, 0, self.a2dBottom)
         self.offA2dBottomRight.set_pos(self.a2dRight, 0, self.a2dBottom)
 
-        # Initialize onscreen display and controls internal state
+        # Define widget overlay
+        self.offscreen_graphics_lens = None
+        self.offscreen_display_region = None
         self._help_label = None
         self._watermark = None
         self._legend = None
         self._clock = None
-        self.offGraphicsLens = None
-        self.offDisplayRegion = None
+
+        # Define input control
+        self.key_map = {"mouse1": 0, "mouse2": 0, "mouse3": 0}
+
+        # Define camera control
         self.zoom_rate = 1.03
         self.camera_lookat = np.zeros(3)
-        self.key_map = {"mouse1": 0, "mouse2": 0, "mouse3": 0}
-        self.longitudeDeg = 0.0
-        self.latitudeDeg = 0.0
+        self.longitude_deg = 0.0
+        self.latitude_deg = 0.0
+        self.last_mouse_x = 0.0
+        self.last_mouse_y = 0.0
+
+        # Define object/highlighting selector
+        self.picker_ray = None
+        self.picker_node = None
+        self.picked_object = None
+        self.click_mouse_x = 0.0
+        self.click_mouse_y = 0.0
 
         # Create resizeable offscreen buffer
         self._open_offscreen_window(WINDOW_SIZE_DEFAULT)
@@ -334,6 +350,9 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         self.show_floor(True)
 
     def open_window(self) -> None:
+        """Open a graphical window, with offscreen buffer attached on it to
+        allow for arbitrary size screenshot.
+        """
         # Make sure a graphical window is not already open
         if any(isinstance(win, GraphicsWindow) for win in self.winList):
             raise RuntimeError("Only one graphical window can be opened.")
@@ -361,6 +380,17 @@ def open_window(self) -> None:
             self.taskMgr.add(
                 self.move_orbital_camera_task, "move_camera_task", sort=2)
 
+            # Setup object pickler
+            self.picker_ray = CollisionRay()
+            self.picker_node = CollisionNode('mouse_ray')
+            self.picker_node.set_from_collide_mask(
+                GeomNode.get_default_collide_mask())
+            self.picker_node.addSolid(self.picker_ray)
+            self.picker_traverser = CollisionTraverser('traverser')
+            picker_np = self.camera.attachNewNode(self.picker_node)
+            self.picker_queue = CollisionHandlerQueue()
+            self.picker_traverser.addCollider(picker_np, self.picker_queue)
+
             # Limit framerate to reduce computation cost
             self.set_framerate(PANDA3D_FRAMERATE_MAX)
 
@@ -386,7 +416,7 @@ def _open_offscreen_window(self,
         # Note that one must remove display region associated with shared 2D
         # renderer, otherwise it will be altered when closing current window.
         if self.buff is not None:
-            self.buff.remove_display_region(self.offDisplayRegion)
+            self.buff.remove_display_region(self.offscreen_display_region)
             self.close_window(self.buff, keepCamera=False)
 
         # Set offscreen buffer frame properties
@@ -408,7 +438,7 @@ def _open_offscreen_window(self,
         # Note that it is impossible to create resizeable buffer without an
         # already existing host for some reason...
         win = self.graphicsEngine.make_output(
-            self.pipe, "off_buffer", 0, fbprops, winprops, flags,
+            self.pipe, "offscreen_buffer", 0, fbprops, winprops, flags,
             self.win.get_gsg(), self.win)
 
         # Append buffer to the list of windows managed by the ShowBase
@@ -417,24 +447,30 @@ def _open_offscreen_window(self,
 
         # Create 3D camera region for the scene.
         # Set near distance of camera lens to allow seeing model from close.
-        self.offGraphicsLens = PerspectiveLens()
-        self.offGraphicsLens.set_near(0.1)
-        self.make_camera(win, camName='off_camera', lens=self.offGraphicsLens)
+        self.offscreen_graphics_lens = PerspectiveLens()
+        self.offscreen_graphics_lens.set_near(0.1)
+        self.make_camera(
+            win, camName='offscreen_camera', lens=self.offscreen_graphics_lens)
 
         # Create 2D display region for widgets
-        self.offDisplayRegion = win.makeMonoDisplayRegion()
-        self.offDisplayRegion.set_sort(5)
-        self.offDisplayRegion.set_camera(self.offCamera2d)
+        self.offscreen_display_region = win.makeMonoDisplayRegion()
+        self.offscreen_display_region.set_sort(5)
+        self.offscreen_display_region.set_camera(self.offscreen_camera_2d)
 
         # # Adjust aspect ratio
         self._adjust_offscreen_window_aspect_ratio()
 
     def _adjust_offscreen_window_aspect_ratio(self) -> None:
+        """Adjust aspect ratio of offscreen window.
+
+        .. note::
+            This method is called automatically after resize.
+        """
         # Get aspect ratio
         aspect_ratio = self.get_aspect_ratio(self.buff)
 
         # Adjust 3D rendering aspect ratio
-        self.offGraphicsLens.set_aspect_ratio(aspect_ratio)
+        self.offscreen_graphics_lens.set_aspect_ratio(aspect_ratio)
 
         # Adjust existing anchors for offscreen 2D rendering
         if aspect_ratio < 1:
@@ -446,7 +482,7 @@ def _adjust_offscreen_window_aspect_ratio(self) -> None:
             a2dRight = 1.0
         else:
             # If the window is WIDE, lets expand the left and right
-            self.offAspect2d.setScale(1.0 / aspect_ratio, 1.0, 1.0)
+            self.offAspect2d.set_scale(1.0/aspect_ratio, 1.0, 1.0)
             a2dTop = 1.0
             a2dBottom = -1.0
             a2dLeft = -aspect_ratio
@@ -466,12 +502,26 @@ def getSize(self, win: Optional[Any] = None) -> Tuple[int, int]:
         return super().getSize(win)
 
     def getMousePos(self) -> Tuple[int, int]:
+        """Get current mouse position.
+
+        .. note::
+            Can be overloaded to allow for emulated mouse click.
+        """
         md = self.win.getPointer(0)
         return md.getX(), md.getY()
 
     def handle_key(self, key: str, value: bool) -> None:
+        """Input controller handler.
+        """
         if key in ["mouse1", "mouse2", "mouse3"]:
-            self.lastMouseX, self.lastMouseY = self.getMousePos()
+            mouseX, mouseY = self.getMousePos()
+            if key == "mouse1":
+                if value:
+                    self.click_mouse_x, self.click_mouse_y = mouseX, mouseY
+                elif (self.click_mouse_x == mouseX and
+                        self.click_mouse_y == mouseY):
+                    self.click_on_node()
+            self.last_mouse_x, self.last_mouse_y = mouseX, mouseY
             self.key_map[key] = value
         elif key in ["wheelup", "wheeldown"]:
             cam_dir = self.camera_lookat - np.asarray(self.camera.get_pos())
@@ -481,66 +531,105 @@ def handle_key(self, key: str, value: bool) -> None:
                 cam_pos = self.camera_lookat - cam_dir * self.zoom_rate
             self.camera.set_pos(*cam_pos)
 
+    def click_on_node(self) -> None:
+        """Object selector handler.
+        """
+        # Remove focus of currently selected object
+        picked_object_prev = self.picked_object
+        if self.picked_object is not None:
+            self.highlight_node(*self.picked_object, False)
+            self.picked_object = None
+
+        # Select new object if the user actually clicked on a selectable node
+        object_found = False
+        mpos = self.mouseWatcherNode.getMouse()
+        self.picker_ray.set_from_lens(self.camNode, mpos.getX(), mpos.getY())
+        self.picker_traverser.traverse(self.render)
+        for i in range(self.picker_queue.getNumEntries()):
+            self.picker_queue.sortEntries()
+            picked_node = self.picker_queue.getEntry(i).getIntoNodePath()
+            # Do not allow selecting hidden node
+            if not picked_node.isHidden():
+                node_path = str(picked_node)
+                for group_name in self._groups.keys():
+                    group_path = f"render/scene_root/{group_name}/"
+                    # Only nodes part of user groups can be selected
+                    if node_path.startswith(group_path):
+                        name = node_path[len(group_path):].split("/", 1)[0]
+                        if (group_name, name) != picked_object_prev:
+                            self.picked_object = (group_name, name)
+                        object_found = True
+                        break
+                if object_found:
+                    break
+
+        # Focus on newly selected node
+        if self.picked_object is not None:
+            self.highlight_node(*self.picked_object, True)
+
     def move_orbital_camera_task(self, task: Any) -> None:
+        """Custom control of the camera to be more suitable for robotic
+        application than the default one.
+        """
         # Get mouse position
         x, y = self.getMousePos()
 
         # Ensure consistent camera pose and lookat
-        self.longitudeDeg, self.latitudeDeg, _ = self.camera.get_hpr()
+        self.longitude_deg, self.latitude_deg, _ = self.camera.get_hpr()
         cam_pos = np.asarray(self.camera.get_pos())
         cam_dir = self.camera_lookat - cam_pos
         cam_dist = np.linalg.norm(cam_dir)
-        longitudeRad = self.longitudeDeg * np.pi / 180.0
-        latitudeRad = self.latitudeDeg * np.pi / 180.0
-        cam_dir_n = np.array([-np.sin(longitudeRad)*np.cos(latitudeRad),
-                              np.cos(longitudeRad)*np.cos(latitudeRad),
-                              np.sin(latitudeRad)])
+        longitude = self.longitude_deg * np.pi / 180.0
+        latitude = self.latitude_deg * np.pi / 180.0
+        cam_dir_n = np.array([-np.sin(longitude)*np.cos(latitude),
+                              np.cos(longitude)*np.cos(latitude),
+                              np.sin(latitude)])
         self.camera_lookat = cam_pos + cam_dist * cam_dir_n
 
         if self.key_map["mouse1"]:
             # Update camera rotation
-            self.longitudeDeg -= (x - self.lastMouseX) * 0.2
-            self.latitudeDeg -= (y - self.lastMouseY) * 0.2
+            self.longitude_deg -= (x - self.last_mouse_x) * 0.2
+            self.latitude_deg -= (y - self.last_mouse_y) * 0.2
 
             # Limit angles to [-180;+180] x [-90;+90]
-            if (self.longitudeDeg > 180.0):
-                self.longitudeDeg = self.longitudeDeg - 360.0
-            if (self.longitudeDeg < -180.0):
-                self.longitudeDeg = self.longitudeDeg + 360.0
-            if (self.latitudeDeg > (90.0 - 0.001)):
-                self.latitudeDeg = 90.0 - 0.001
-            if (self.latitudeDeg < (-90.0 + 0.001)):
-                self.latitudeDeg = -90.0 + 0.001
-
-            longitudeRad = self.longitudeDeg * np.pi / 180.0
-            latitudeRad = self.latitudeDeg * np.pi / 180.0
-            cam_dir_n = np.array([-np.sin(longitudeRad) * np.cos(latitudeRad),
-                                  np.cos(longitudeRad) * np.cos(latitudeRad),
-                                  np.sin(latitudeRad)])
+            if (self.longitude_deg > 180.0):
+                self.longitude_deg = self.longitude_deg - 360.0
+            if (self.longitude_deg < -180.0):
+                self.longitude_deg = self.longitude_deg + 360.0
+            if (self.latitude_deg > (90.0 - 0.001)):
+                self.latitude_deg = 90.0 - 0.001
+            if (self.latitude_deg < (-90.0 + 0.001)):
+                self.latitude_deg = -90.0 + 0.001
+
+            longitude = self.longitude_deg * np.pi / 180.0
+            latitude = self.latitude_deg * np.pi / 180.0
+            cam_dir_n = np.array([-np.sin(longitude) * np.cos(latitude),
+                                  np.cos(longitude) * np.cos(latitude),
+                                  np.sin(latitude)])
             cam_pos = self.camera_lookat - cam_dist * cam_dir_n
             self.camera.set_pos(*cam_pos)
-            self.camera.set_hpr(self.longitudeDeg, self.latitudeDeg, 0)
+            self.camera.set_hpr(self.longitude_deg, self.latitude_deg, 0)
         if self.key_map["mouse2"]:
-            cam_delta = (y - self.lastMouseY) * 0.02 * cam_dir_n
+            cam_delta = (y - self.last_mouse_y) * 0.02 * cam_dir_n
             self.camera_lookat -= cam_delta
             cam_pos -= cam_delta
             self.camera.set_pos(*cam_pos)
         elif self.key_map["mouse3"]:
-            cam_n1 = np.array([np.cos(longitudeRad),
-                               np.sin(longitudeRad),
+            cam_n1 = np.array([np.cos(longitude),
+                               np.sin(longitude),
                                0.0])
-            cam_n2 = np.array([-np.sin(longitudeRad) * np.sin(latitudeRad),
-                               np.cos(longitudeRad) * np.sin(latitudeRad),
-                               -np.cos(latitudeRad)])
-            pos_shift = ((x - self.lastMouseX) * cam_n1 +
-                         (y - self.lastMouseY) * cam_n2) * 0.01
+            cam_n2 = np.array([-np.sin(longitude) * np.sin(latitude),
+                               np.cos(longitude) * np.sin(latitude),
+                               -np.cos(latitude)])
+            pos_shift = ((x - self.last_mouse_x) * cam_n1 +
+                         (y - self.last_mouse_y) * cam_n2) * 0.01
             cam_pos -= pos_shift
             self.camera_lookat -= pos_shift
             self.camera.set_pos(*cam_pos)
 
         # Store latest mouse position for the next frame
-        self.lastMouseX = x
-        self.lastMouseY = y
+        self.last_mouse_x = x
+        self.last_mouse_y = y
 
         # End task
         return task.cont
@@ -566,7 +655,7 @@ def _make_light_direct(self,
 
     def _make_axes(self) -> NodePath:
         node = super()._make_axes()
-        node.set_scale(0.33)
+        node.set_scale(0.3)
         return node
 
     def _make_floor(self) -> NodePath:
@@ -585,6 +674,49 @@ def _make_floor(self) -> NodePath:
         node.set_two_sided(True)
         return node
 
+    def append_group(self,
+                     root_path: str,
+                     remove_if_exists: bool = True,
+                     scale: float = 1.0) -> None:
+        """Must be patched to avoid adding new group if 'remove_if_exists' is
+        false, otherwise it will be impossible to access to old ones.
+        """
+        if not remove_if_exists and root_path in self._groups:
+            return
+        super().append_group(root_path, remove_if_exists, scale)
+
+    def append_node(self,
+                    root_path: str,
+                    name: str,
+                    node: NodePath,
+                    frame: Optional[FrameType] = None) -> None:
+        """Must be patched to make sure node's name is valid.
+        """
+        assert name.isidentifier(), (
+            "Node's name is restricted to case-insensitive ASCII alphanumeric "
+            "string (including underscores).")
+        super().append_node(root_path, name, node, frame)
+
+    def highlight_node(self, root_path: str, name: str, enable: bool) -> None:
+        node = self._groups[root_path].find(name)
+        if node:
+            render_mode = node.get_render_mode()
+            if enable:
+                if render_mode == RenderModeAttrib.M_filled_wireframe:
+                    return
+                render_attrib = node.get_state().get_attrib_def(
+                    RenderModeAttrib.get_class_slot())
+                node.set_attrib(RenderModeAttrib.make(
+                    RenderModeAttrib.M_filled_wireframe,
+                    1.5,  # thickness (1.0 by default)
+                    render_attrib.perspective,
+                    (2.0, 2.0, 2.0, 1.0)  # wireframe_color
+                ))
+            else:
+                if render_mode == RenderModeAttrib.M_off:
+                    return
+                node.clear_render_mode()
+
     def append_cone(self,
                     root_path: str,
                     name: str,
@@ -600,6 +732,24 @@ def append_cone(self,
         node.set_scale(radius, radius, length)
         self.append_node(root_path, name, node, frame)
 
+    def append_cylinder(self,
+                        root_path: str,
+                        name: str,
+                        radius: float,
+                        length: float,
+                        anchor_bottom: bool = False,
+                        frame: Optional[FrameType] = None) -> None:
+        """Must be patched to add optional to place anchor at the bottom of the
+        cylinder instead of the middle.
+        """
+        geom_node = GeomNode('cylinder')
+        geom_node.add_geom(geometry.make_cylinder())
+        node = NodePath(geom_node)
+        node.set_scale(Vec3(radius, radius, length))
+        if anchor_bottom:
+            node.set_pos(0.0, 0.0, -length/2)
+        self.append_node(root_path, name, node, frame)
+
     def append_arrow(self,
                      root_path: str,
                      name: str,
@@ -748,7 +898,7 @@ def set_legend(self,
         fig.gca().set_axis_off()
 
         # Render the legend
-        fig.canvas.draw()
+        fig.draw(renderer=fig.canvas.get_renderer())
 
         # Compute bbox size to be power of 2 for software rendering.
         bbox = legend.get_window_extent().padded(2)
@@ -868,27 +1018,23 @@ def set_material(self,
             if color is None:
                 node.clear_color()
 
-    def move_node(self,
-                  root_path: str,
-                  name: str,
-                  frame: FrameType) -> None:
-        """Set pose of a single node.
-        """
-        node = self._groups[root_path].find(name).children[0]
-        if isinstance(frame, np.ndarray):
-            node.set_mat(Mat4(*frame.T.flat))
-        else:
-            pos, quat = frame
-            node.set_pos_quat(Vec3(*pos), Quat(*quat))
-
     def set_scale(self,
                   root_path: str,
                   name: str,
                   scale: Optional[Tuple3FType] = None) -> None:
         """Override scale of node of a node.
         """
-        node = self._groups[root_path].find(name).children[0]
-        node.set_scale(*scale)
+        node = self._groups[root_path].find(name)
+        if node:
+            if any(abs(s) < 1e-3 for s in scale):
+                if not node.is_hidden():
+                    node.set_tag("status", "disable")
+                    node.hide()
+            else:
+                node.set_scale(*scale)
+                if node.get_tag("status") == "disable":
+                    node.set_tag("status", "auto")
+                    node.show()
 
     def set_scales(self, root_path, name_scales_dict):
         """Override scale of nodes within a group.
@@ -896,13 +1042,49 @@ def set_scales(self, root_path, name_scales_dict):
         for name, scale in name_scales_dict.items():
             self.set_scale(root_path, name, scale)
 
+    def move_node(self,
+                  root_path: str,
+                  name: str,
+                  frame: FrameType) -> None:
+        """Set pose of a single node.
+        """
+        node = self._groups[root_path].find(name)
+        if isinstance(frame, np.ndarray):
+            node.set_mat(Mat4(*frame.T.flat))
+        else:
+            pos, quat = frame
+            node.set_pos_quat(Vec3(*pos), Quat(*quat))
+
     def remove_node(self, root_path: str, name: str) -> None:
-        """Remove a signle node from the scene.
+        """Remove a single node from the scene.
         """
         node = self._groups[root_path].find(name)
         if node:
             node.remove_node()
 
+    def show_node(self,
+                  root_path: str,
+                  name: str,
+                  show: bool,
+                  always_foreground: bool = False) -> None:
+        """Turn rendering on or off for a single node.
+        """
+        node = self._groups[root_path].find(name)
+        if node:
+            if show:
+                if node.get_tag("status") in ("hidden", ""):
+                    node.set_tag("status", "auto")
+                    node.show()
+            else:
+                node.set_tag("status", "hidden")
+                node.hide()
+            if always_foreground:
+                node.set_bin("fixed", 0)
+            else:
+                node.clear_bin()
+            node.set_depth_test(not always_foreground)
+            node.set_depth_write(not always_foreground)
+
     def set_camera_transform(self,
                              pos: Tuple3FType,
                              quat: np.ndarray) -> None:
@@ -1029,6 +1211,7 @@ def delegate(self, name: str) -> Any:
 
 Panda3dViewer.__getattr__ = delegate
 delattr(Panda3dViewer, 'set_material')
+delattr(Panda3dViewer, 'append_cylinder')
 
 
 class Panda3dVisualizer(BaseVisualizer):
@@ -1203,7 +1386,7 @@ def move(group, model, data):
             name_pose_dict = {}
             for obj in model.geometryObjects:
                 oMg = data.oMg[model.getGeometryId(obj.name)]
-                x, y, z, qx, qy, qz, qw = pin.SE3ToXYZQUATtuple(oMg)
+                x, y, z, qx, qy, qz, qw = pin.SE3ToXYZQUAT(oMg)
                 name_pose_dict[obj.name] = ((x, y, z), (qw, qx, qy, qz))
             self.viewer.move_nodes(group, name_pose_dict)
 
diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py
index c1ca58564..73debe65e 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/replay.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py
@@ -5,18 +5,20 @@
 import tempfile
 import argparse
 from bisect import bisect_right
+from functools import partial
 from threading import Thread, Lock
 from itertools import cycle, islice
-from typing import Optional, Union, Sequence, Tuple, Dict, Any
+from typing import Optional, Union, Sequence, Tuple, Dict, Any, Callable
 
 import av
 import numpy as np
 from tqdm import tqdm
-from typing_extensions import TypedDict
 
 from .. import core as jiminy
-from ..state import State
-from ..log import build_robot_from_log
+from ..log import (TrajectoryDataType,
+                   build_robot_from_log,
+                   extract_viewer_data_from_log,
+                   emulate_sensors_data_from_log)
 from .viewer import (
     COLORS, Viewer, Tuple3FType, Tuple4FType, CameraPoseType, CameraMotionType)
 from .meshcat.utilities import interactive_mode
@@ -30,69 +32,14 @@
 logger = logging.getLogger(__name__)
 
 
-class TrajectoryDataType(TypedDict, total=False):
-    # List of State objects of increasing time.
-    evolution_robot: Sequence[State]
-    # Jiminy robot. None if omitted.
-    robot: Optional[jiminy.Robot]
-    # Whether to use theoretical or actual model
-    use_theoretical_model: bool
-
-
 ColorType = Union[Tuple4FType, str]
 
 
-def extract_viewer_data_from_log(log_data: Dict[str, np.ndarray],
-                                 robot: jiminy.Model) -> TrajectoryDataType:
-    """Extract the minimal required information from raw log data in order to
-    replay the simulation in a viewer.
-
-    It extracts only the required data for replay, namely the evolution over
-    time of the joints positions.
-
-    :param log_data: Data from the log file, in a dictionnary.
-    :param robot: Jiminy robot.
-
-    :returns: Trajectory dictionary. The actual trajectory corresponds to the
-              field "evolution_robot" and it is a list of State object. The
-              other fields are additional information.
-    """
-    t = log_data["Global.Time"]
-    try:
-        # Extract the joint positions evolution over time
-        qe = np.stack([log_data[".".join(("HighLevelController", s))]
-                       for s in robot.logfile_position_headers], axis=-1)
-
-        # Determine whether to use the theoretical or flexible model
-        use_theoretical_model = not robot.is_flexible
-
-        # Create state sequence
-        evolution_robot = []
-        for t_i, q_i in zip(t, qe):
-            evolution_robot.append(State(t=t_i, q=q_i))
-
-        viewer_data = {'evolution_robot': evolution_robot,
-                       'robot': robot,
-                       'use_theoretical_model': use_theoretical_model}
-    except KeyError:  # The current options are inconsistent with log data
-        # Toggle flexibilities
-        model_options = robot.get_model_options()
-        dyn_options = model_options['dynamics']
-        dyn_options['enableFlexibleModel'] = not robot.is_flexible
-        robot.set_model_options(model_options)
-
-        # Get viewer data
-        viewer_data = extract_viewer_data_from_log(log_data, robot)
-
-        # Restore back flexibilities
-        dyn_options['enableFlexibleModel'] = not robot.is_flexible
-        robot.set_model_options(model_options)
-
-    return viewer_data
-
-
-def play_trajectories(trajectory_data: Union[
+def play_trajectories(trajs_data: Union[
                           TrajectoryDataType, Sequence[TrajectoryDataType]],
+                      update_hooks: Optional[Union[
+                          Callable[[float], None],
+                          Sequence[Callable[[float], None]]]] = None,
                       time_interval: Optional[Union[
                           np.ndarray, Tuple[float, float]]] = (0.0, np.inf),
                       speed_ratio: float = 1.0,
@@ -106,6 +53,8 @@ def play_trajectories(trajectory_data: Union[
                       watermark_fullpath: Optional[str] = None,
                       legend: Optional[Union[str, Sequence[str]]] = None,
                       enable_clock: bool = False,
+                      display_com: bool = None,
+                      display_contacts: Optional[bool] = None,
                       scene_name: str = 'world',
                       record_video_path: Optional[str] = None,
                       start_paused: bool = False,
@@ -126,7 +75,13 @@ def play_trajectories(trajectory_data: Union[
         Replay speed is independent of the platform (windows, linux...) and
         available CPU power.
 
-    :param trajectory_data: List of `TrajectoryDataType` dicts.
+    :param trajs_data: List of `TrajectoryDataType` dicts.
+    :param update_hooks: Callables associated with each robot that can be used
+                         to update non-kinematic robot data, for instance to
+                         emulate sensors data from log using the hook provided
+                         by `emulate_sensors_data_from_log` method. None` to
+                         disable.
+                         Optional: None by default.
     :param time_interval: Replay only timesteps in this interval of time.
                           It does not have to be finite.
                           Optional: [0, inf] by default.
@@ -141,9 +96,9 @@ def play_trajectories(trajectory_data: Union[
                           Optional: Original color if single robot, default
                           color cycle otherwise.
     :param travelling_frame: Name of the frame of the robot associated with the
-                             first trajectory_data. The camera will
-                             automatically follow it. None to disable.
-                             Optional: None by default.
+                             first `trajs_data`. The camera will utomatically
+                             follow it. `None` to disable.
+                             Optional: Disable by default.
     :param camera_xyzrpy: Tuple position [X, Y, Z], rotation [Roll, Pitch, Yaw]
                           corresponding to the absolute pose of the camera
                           during replay, if travelling is disable, or the
@@ -166,6 +121,16 @@ def play_trajectories(trajectory_data: Union[
     :param enable_clock: Add clock on bottom right corner of the viewer.
                          Only available with panda3d rendering backend.
                          Optional: Disable by default.
+    :param display_com: Whether or not to display the center of mass. `None`
+                        to keep current viewers' settings, if any.
+                        Optional: Enable by default iif `viewers` is `None`.
+    :param display_contacts: Whether or not to display the contact forces.
+                             Note that the user is responsible for updating
+                             sensors data since `Viewer.display` is only
+                             computing kinematic quantities. `None` to keep
+                             current viewers' settings, if any.
+                             Optional: Enable by default iif `update_hooks` is
+                             specified and `viewers` is `None`.
     :param scene_name: Name of viewer's scene in which to display the robot.
                        Optional: Common default name if omitted.
     :param record_video_path: Fullpath location where to save generated video.
@@ -196,12 +161,18 @@ def play_trajectories(trajectory_data: Union[
                     Optional: None by default.
     :param verbose: Add information to keep track of the process.
                     Optional: True by default.
-    :param kwargs: Used argument to allow chaining renderining methods.
+    :param kwargs: Unused keyword arguments to allow chaining renderining
+                   methods with ease.
 
     :returns: List of viewers used to play the trajectories.
     """
-    if not isinstance(trajectory_data, (list, tuple)):
-        trajectory_data = [trajectory_data]
+    # Make sure trajectory data and update hook are list or tuple
+    if not isinstance(trajs_data, (list, tuple)):
+        trajs_data = [trajs_data]
+    if update_hooks is None:
+        update_hooks = [None] * len(trajs_data)
+    if not isinstance(update_hooks, (list, tuple)):
+        update_hooks = [update_hooks]
 
     # Sanitize user-specified viewers
     if viewers is not None:
@@ -218,23 +189,38 @@ def play_trajectories(trajectory_data: Union[
                     viewers = None
                     break
 
+    # Handling of default display of CoM and contact forces
+    if viewers is None:
+        if display_com is None:
+            display_com = True
+        if display_contacts is None:
+            display_contacts = all(func is not None for func in update_hooks)
+
+    # Make sure it is possible to display contacts if requested
+    if display_contacts is not False:
+        if any(traj['robot'].is_locked for traj in trajs_data):
+            logger.debug(
+                "`display_contacts` is not available if robot is locked. "
+                "Please stop any running simulation before replay.")
+            display_contacts = False
+
     # Sanitize user-specified robot offsets
     if xyz_offsets is None:
-        xyz_offsets = len(trajectory_data) * [None]
-    elif len(xyz_offsets) != len(trajectory_data):
-        xyz_offsets = np.tile(xyz_offsets, (len(trajectory_data), 1))
+        xyz_offsets = len(trajs_data) * [None]
+    elif len(xyz_offsets) != len(trajs_data):
+        xyz_offsets = np.tile(xyz_offsets, (len(trajs_data), 1))
 
     # Sanitize user-specified robot colors
     if robots_colors is None:
-        if len(trajectory_data) == 1:
+        if len(trajs_data) == 1:
             robots_colors = [None]
         else:
             robots_colors = list(islice(
-                cycle(COLORS.values()), len(trajectory_data)))
+                cycle(COLORS.values()), len(trajs_data)))
     elif not isinstance(robots_colors, (list, tuple)) or \
             isinstance(robots_colors[0], float):
         robots_colors = [robots_colors]
-    assert len(robots_colors) == len(trajectory_data)
+    assert len(robots_colors) == len(trajs_data)
 
     # Sanitize user-specified legend
     if legend is not None and not isinstance(legend, (list, tuple)):
@@ -250,7 +236,7 @@ def play_trajectories(trajectory_data: Union[
         viewers = []
         lock = Lock()
         uniq_id = next(tempfile._get_candidate_names())
-        for i, (traj, color) in enumerate(zip(trajectory_data, robots_colors)):
+        for i, (traj, color) in enumerate(zip(trajs_data, robots_colors)):
             # Create a new viewer instance, and load the robot in it
             robot = traj['robot']
             robot_name = f"{uniq_id}_robot_{i}"
@@ -269,10 +255,10 @@ def play_trajectories(trajectory_data: Union[
     else:
         # Reset robot model in viewer if requested color has changed
         for viewer, traj, color in zip(
-                viewers, trajectory_data, robots_colors):
+                viewers, trajs_data, robots_colors):
             if color != viewer.robot_color:
                 viewer._setup(traj['robot'], color)
-    assert len(viewers) == len(trajectory_data)
+    assert len(viewers) == len(trajs_data)
 
     # Add default legend with robots names if replaying multiple trajectories
     if all(color is not None for color in robots_colors) and legend is None:
@@ -288,7 +274,7 @@ def play_trajectories(trajectory_data: Union[
         enable_clock = False
 
     # Early return if nothing to replay
-    if all(not len(traj['evolution_robot']) for traj in trajectory_data):
+    if all(not len(traj['evolution_robot']) for traj in trajs_data):
         return viewers
 
     # Set camera pose or activate camera travelling if requested
@@ -310,11 +296,15 @@ def play_trajectories(trajectory_data: Union[
         Viewer.set_watermark(watermark_fullpath)
 
     # Initialize robot configuration is viewer before any further processing
-    for viewer_i, traj, offset in zip(viewers, trajectory_data, xyz_offsets):
+    for viewer_i, traj, offset in zip(viewers, trajs_data, xyz_offsets):
         evolution_robot = traj['evolution_robot']
-        if len(evolution_robot):
+        if evolution_robot:
             i = bisect_right([s.t for s in evolution_robot], time_interval[0])
             viewer_i.display(evolution_robot[i].q, offset)
+        if display_com is not None:
+            viewer_i.display_center_of_mass(display_com)
+        if display_contacts is not None:
+            viewer_i.display_contact_forces(display_contacts)
 
     # Wait for the meshes to finish loading if video recording is disable
     if record_video_path is None:
@@ -334,7 +324,7 @@ def play_trajectories(trajectory_data: Union[
     if record_video_path is not None:
         # Extract and resample trajectory data at fixed framerate
         time_max = time_interval[0]
-        for traj in trajectory_data:
+        for traj in trajs_data:
             if len(traj['evolution_robot']):
                 time_max = max([time_max, traj['evolution_robot'][-1].t])
         time_max = min(time_max, time_interval[1])
@@ -342,7 +332,7 @@ def play_trajectories(trajectory_data: Union[
         time_global = np.arange(
             time_interval[0], time_max, speed_ratio / VIDEO_FRAMERATE)
         position_evolutions = []
-        for traj in trajectory_data:
+        for traj in trajs_data:
             if len(traj['evolution_robot']):
                 data_orig = traj['evolution_robot']
                 if traj['use_theoretical_model']:
@@ -387,10 +377,14 @@ def play_trajectories(trajectory_data: Union[
         for i, t_cur in enumerate(tqdm(
                 time_global, desc="Rendering frames", disable=(not verbose))):
             # Update the configurations of the robots
-            for viewer, positions, offset in zip(
-                    viewers, position_evolutions, xyz_offsets):
+            for viewer, positions, xyz_offset, update_hook in zip(
+                    viewers, position_evolutions, xyz_offsets, update_hooks):
                 if positions is not None:
-                    viewer.display(positions[i], xyz_offset=offset)
+                    if update_hook is not None:
+                        update_hook_t = partial(update_hook, t_cur)
+                    else:
+                        update_hook_t = None
+                    viewer.display(positions[i], xyz_offset, update_hook_t)
 
             # Update clock if enabled
             if enable_clock:
@@ -429,14 +423,16 @@ def replay_thread(viewer, *args):
 
         # Play trajectories with multithreading
         threads = []
-        for viewer, traj, offset in zip(viewers, trajectory_data, xyz_offsets):
+        for viewer, traj, xyz_offset, update_hook in zip(
+                viewers, trajs_data, xyz_offsets, update_hooks):
             threads.append(Thread(
                 target=replay_thread,
                 args=(viewer,
                       traj['evolution_robot'],
                       time_interval,
                       speed_ratio,
-                      offset,
+                      xyz_offset,
+                      update_hook,
                       enable_clock)))
         for thread in threads:
             thread.daemon = True
@@ -494,8 +490,15 @@ def play_logs_data(robots: Union[Sequence[jiminy.Robot], jiminy.Robot],
     trajectories = [extract_viewer_data_from_log(log, robot)
                     for log, robot in zip(logs_data, robots)]
 
+    # Define `update_hook` to emulate sensor update
+    if not any(robot.is_locked for robot in robots):
+        update_hooks = [emulate_sensors_data_from_log(log, robot)
+                        for log, robot in zip(logs_data, robots)]
+    else:
+        update_hooks = None
+
     # Finally, play the trajectories
-    return play_trajectories(trajectories, **kwargs)
+    return play_trajectories(trajectories, update_hooks, **kwargs)
 
 
 def play_logs_files(logs_files: Union[str, Sequence[str]],
@@ -520,8 +523,8 @@ def play_logs_files(logs_files: Union[str, Sequence[str]],
         logs_data.append(log_data)
         robots.append(robot)
 
-    # Default legend
-    if "legend" not in kwargs:
+    # Default legend if several log files are provided
+    if "legend" not in kwargs and len(logs_files) > 1:
         kwargs["legend"] = [pathlib.Path(log_file).stem.split('_')[-1]
                             for log_file in logs_files]
 
diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py
index f928b3680..5d4f28c35 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py
@@ -14,7 +14,7 @@
 import multiprocessing
 import xml.etree.ElementTree as ET
 from copy import deepcopy
-from functools import wraps
+from functools import wraps, partial
 from bisect import bisect_right
 from threading import Lock
 from typing import Optional, Union, Sequence, Tuple, Dict, Callable, Any
@@ -30,11 +30,12 @@
 from panda3d_viewer.viewer_errors import ViewerClosedError
 
 import pinocchio as pin
-from pinocchio import SE3, SE3ToXYZQUAT
+from pinocchio import SE3, SE3ToXYZQUAT, SE3ToXYZQUATtuple
 from pinocchio.rpy import rpyToMatrix, matrixToRpy
 from pinocchio.visualize import GepettoVisualizer
 
 from .. import core as jiminy
+from ..core import ContactSensor as contact
 from ..state import State
 from .meshcat.utilities import interactive_mode
 from .meshcat.wrapper import MeshcatWrapper
@@ -53,6 +54,8 @@
 
 DEFAULT_WATERMARK_MAXSIZE = (150, 150)
 
+# Fz force value corresponding to capsule's length of 1cm
+CONTACT_FORCE_SCALE = 100.0  # [N]
 
 COLORS = {'red': (0.9, 0.15, 0.15, 1.0),
           'blue': (0.3, 0.3, 1.0, 1.0),
@@ -289,6 +292,8 @@ def __init__(self,
                  delete_robot_on_close: bool = False,
                  robot_name: Optional[str] = None,
                  scene_name: str = 'world',
+                 display_com: bool = False,
+                 display_contacts: bool = False,
                  **kwargs: Any):
         """
         :param robot: Jiminy.Model to display.
@@ -329,6 +334,13 @@ def __init__(self,
         :param scene_name: Scene name, used only with 'gepetto-gui' backend. It
                            must differ from the scene name.
                            Optional: 'world' by default.
+        :param display_com: Whether or not to display the center of mass.
+                            Optional: Disable by default.
+        :param display_contacts: Whether or not to display the contact forces.
+                                 Note that the user is responsible for updating
+                                 sensors data since `Viewer.display` is only
+                                 computing kinematic quantities.
+                                 Optional: Disable by default.
         :param kwargs: Unused extra keyword arguments to enable forwarding.
         """
         # Handling of default arguments
@@ -341,10 +353,12 @@ def __init__(self,
         self.robot_name = robot_name
         self.scene_name = scene_name
         self.use_theoretical_model = use_theoretical_model
-        self._lock = lock if lock is not None else Viewer._lock
         self.delete_robot_on_close = delete_robot_on_close
+        self._lock = lock if lock is not None else Viewer._lock
+        self._display_com = display_com
+        self._display_contacts = display_contacts
 
-        # Initialize marker register.
+        # Initialize marker register
         self.markers: Dict[str, MarkerDataType] = {}
         self._markers_group = '/'.join((
             self.scene_name, self.robot_name, "markers"))
@@ -503,6 +517,12 @@ def _setup(self,
                             ranging from 0.0 and 1.0, and a few named colors.
                             Optional: Disable by default.
         """
+        # Delete existing robot, if any
+        robot_node_path = '/'.join((self.scene_name, self.robot_name))
+        Viewer._delete_nodes_viewer([
+            '/'.join((robot_node_path, "visuals")),
+            '/'.join((robot_node_path, "collisions"))])
+
         # Backup desired color
         self.robot_color = get_color_code(robot_color)
 
@@ -539,10 +559,6 @@ def _setup(self,
         self._client.data = pinocchio_data
         self._client.collision_data = robot.collision_data
 
-        # Delete existing robot, if any
-        robot_node_path = '/'.join((self.scene_name, self.robot_name))
-        Viewer._delete_nodes_viewer([robot_node_path])
-
         # Create the scene and load robot
         if Viewer.backend == 'gepetto-gui':
             # Initialize the viewer
@@ -574,9 +590,55 @@ def _setup(self,
             self._client.loadViewerModel(
                 rootNodeName=robot_node_path, color=self.robot_color)
 
-        # Add 'markers' group
         if Viewer.backend.startswith('panda3d'):
-            self._gui.append_group(self._markers_group)
+            # Add markers' display groups
+            self._gui.append_group(self._markers_group, remove_if_exists=False)
+
+            # Add center of mass
+            def get_scale() -> Tuple3FType:
+                return (1.0, 1.0, self._client.data.com[0][2])
+
+            self.add_marker(name="com_0_sphere",
+                            shape="sphere",
+                            pose=[self._client.data.com[0], None],
+                            remove_if_exists=True,
+                            radius=0.03)
+
+            self.add_marker(name="com_0_cylinder",
+                            shape="cylinder",
+                            pose=[self._client.data.com[0], None],
+                            scale=get_scale,
+                            remove_if_exists=True,
+                            radius=0.004,
+                            length=1.0,
+                            anchor_bottom=True)
+
+            # Add contact sensor markers
+            def get_pose(sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]:
+                oMf = self._client.data.oMf[sensor.frame_idx]
+                frame_position = oMf.translation
+                frame_rotation = pin.Quaternion(oMf.rotation).coeffs()
+                return (frame_position, frame_rotation)
+
+            def get_scale(sensor: contact) -> Tuple3FType:
+                length = min(abs(sensor.data[2]) / CONTACT_FORCE_SCALE, 100.0)
+                return (1.0, 1.0, - np.sign(sensor.data[2]) * length)
+
+            if contact.type in robot.sensors_names.keys():
+                for name in robot.sensors_names[contact.type]:
+                    sensor = robot.get_sensor(contact.type, name)
+                    self.add_marker(name='_'.join((contact.type, name)),
+                                    shape="cylinder",
+                                    pose=partial(get_pose, sensor),
+                                    scale=partial(get_scale, sensor),
+                                    remove_if_exists=True,
+                                    radius=0.02,
+                                    length=0.01,
+                                    anchor_bottom=True)
+
+            # Refresh CoM and contacts visibility
+            self.display_center_of_mass(self._display_com)
+            self.display_contact_forces(self._display_contacts)
 
     @staticmethod
     def open_gui(start_if_needed: bool = False) -> bool:
@@ -1475,28 +1537,39 @@ def display_collisions(self, visibility: bool) -> None:
     def add_marker(self,
                    name: str,
                    shape: str,
-                   pose: Optional[np.ndarray] = None,
-                   scale: Union[float, Tuple3FType] = 1.0,
-                   color: Optional[Union[str, Tuple4FType]] = 'red',
+                   pose: Union[Sequence[Optional[np.ndarray]],
+                               Callable[[], Tuple[Tuple3FType, Tuple4FType]]
+                               ] = (None, None),
+                   scale: Union[Union[float, Tuple3FType],
+                                Callable[[], np.ndarray]] = 1.0,
+                   color: Union[Optional[Union[str, Tuple4FType]],
+                                Callable[[], Tuple4FType]] = None,
+                   remove_if_exists: bool = False,
+                   auto_refresh: bool = True,
                    **shape_kwargs: Any) -> MarkerDataType:
         """Add marker on the scene.
 
-        .. note::
-            This method is only supported by Panda3d for now.
+        .. warning::
+            This method is only supported by Panda3d.
 
-        :param name: Unique identifier name.
+        :param name: Unique name. It must be a valid string identifier.
         :param shape: Desired shape, as a string, i.e. 'cone', 'box', 'sphere',
                       'capsule', 'cylinder', or 'arrow'.
-        :param pose: Pose of the geometry on the scene, as a single vector
-                     (position [X, Y, Z] + quaternion [X,  Y, Z, W]). `None`
-                     corresponds to world frame.
+        :param pose: Pose of the geometry on the scene, as a list of vectors
+                     (position [X, Y, Z], quaternion [X,  Y, Z, W]). `None`
+                     corresponds to world frame position and/or orientation.
                      Optional: World frame by default.
         :param scale: Size of the marker. Each principal axis of the geometry
                       are scaled separately.
         :param color: Color of the marker. It supports both RGBA codes as a
                       list of 4 floating-point values ranging from 0.0 and 1.0,
                       and a few named colors.
-                      Optional: 'red' by default.
+                      Optional: robot's color by default if overridden,
+                      'red' otherwise.
+        :param auto_refresh: Whether or not to refresh the scene after adding
+                             the marker. Useful for adding a bunch of markers
+                             and only refresh once. Note that the marker will
+                             not display properly until then.
         :param shape_kwargs: Any additional keyword arguments to forward for
                              shape instantiation.
 
@@ -1507,35 +1580,105 @@ def add_marker(self,
         """
         # Handling of user arguments
         if pose is None:
-            pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
+            pose = [None, None]
+        if not callable(pose) and any(value is None for value in pose):
+            pose = list(pose)
+            if pose[0] is None:
+                pose[0] = np.zeros(3)
+            if pose[1] is None:
+                pose[1] = np.array([0.0, 0.0, 0.0, 1.0])
+        if np.isscalar(scale):
+            scale = np.full((3,), fill_value=float(scale))
+        if color is None:
+            color = self.robot_color
+        if color is None:
+            color = 'white'
         color = np.asarray(get_color_code(color))
-        if isinstance(scale, float):
-            scale = np.full((3,), fill_value=scale)
 
         # Make sure no marker with this name already exists
         if name in self.markers.keys():
-            raise ValueError(f"marker's name '{name}' already exists.")
+            if not remove_if_exists:
+                raise ValueError(f"marker's name '{name}' already exists.")
+            self.remove_marker(name)
 
         if Viewer.backend.startswith('panda3d'):
             create_shape = getattr(self._gui, f"append_{shape}")
-            create_shape(
-                self._markers_group, name, frame=(pose[:3], pose[3:]),
-                **shape_kwargs)
-            self._gui.set_material(self._markers_group, name, color)
-            self._gui.set_scale(self._markers_group, name, scale)
+            create_shape(self._markers_group, name, **shape_kwargs)
             marker_data = {"pose": pose, "scale": scale, "color": color}
             self.markers[name] = marker_data
-            return marker_data
+        else:
+            raise NotImplementedError(
+                "This method is only supported by Panda3d.")
+
+        # Refresh the scene if desired
+        if auto_refresh:
+            self.refresh()
+
+        return marker_data
+
+    @__must_be_open
+    def display_center_of_mass(self, visibility: bool) -> None:
+        """Display the position of the center of mass as a sphere.
+
+        .. note::
+            It corresponds to the attribute `com[0]` of the provided
+            `robot.pinocchio_model`. Calling `Viewer.display` will update it
+            automatically, while `Viewer.refresh` will not.
+
+        :param visibility: Whether to enable or disable display of the center
+                           of mass.
+        """
+        if Viewer.backend.startswith('panda3d'):
+            for name in self.markers:
+                if name.startswith("com_0"):
+                    self._gui.show_node(
+                        self._markers_group, name, visibility,
+                        always_foreground=True)
+            self._display_com = visibility
+        else:
+            raise NotImplementedError(
+                "This method is only supported by Panda3d.")
+
+    @__must_be_open
+    def display_contact_forces(self, visibility: bool) -> None:
+        """Display forces associated with the contact sensors attached to the
+        robot, as a capsule of variable length depending of Fz.
+
+        .. note::
+            Fz can be signed. It will affect the orientation of the capsule.
+
+        .. warning::
+            It corresponds to the attribute `data` of `jiminy.ContactSensor`.
+            Calling `Viewer.display` will NOT update its value automatically.
+            It is up to the user to keep it up-to-date. It will always be the
+            case during a simulation, but not when replaying a log file
+            a-posteriori. In such a case, the user is responsible of specifying
+            a custom `update_hook` to `Viewer.display` and `Viewer.replay`
+            methods to emulate sensor update based on log data.
+
+        .. warning::
+            This method is only supported by Panda3d.
+        """
+        if Viewer.backend.startswith('panda3d'):
+            for name in self.markers:
+                if name.startswith(contact.type):
+                    self._gui.show_node(
+                        self._markers_group, name, visibility,
+                        always_foreground=True)
+            self._display_contacts = visibility
         else:
             raise NotImplementedError(
                 "This method is only supported by Panda3d.")
 
     @__must_be_open
     def remove_marker(self, name: str) -> None:
-        try:
-            self.markers.pop(name)
-        except KeyError as e:
-            raise ValueError(f"marker's name '{name}' does not exists.") from e
+        """Remove a marker, based on its name.
+
+        :param identifier: Name of the marker to remove.
+        """
+        if name not in self.markers.keys():
+            raise ValueError(f"Marker's name '{name}' does not exists.")
+        self.markers.pop(name)
         self._gui.remove_node(self._markers_group, name)
 
     @__must_be_open
@@ -1581,7 +1724,7 @@ def refresh(self,
                     self._gui.applyConfigurations(
                         [self._client.getViewerNodeName(geom, model_type)
                          for geom in geom_model.geometryObjects],
-                        [tuple(SE3ToXYZQUAT(geom_data.oMg[i]))
+                        [SE3ToXYZQUATtuple(geom_data.oMg[i])
                          for i in range(len(geom_model.geometryObjects))])
             elif Viewer.backend.startswith('panda3d'):
                 for geom_model, geom_data, model_type in zip(
@@ -1592,7 +1735,7 @@ def refresh(self,
                         x, y, z, qx, qy, qz, qw = SE3ToXYZQUAT(oMg)
                         group, nodeName = self._client.getViewerNodeName(
                             geom, model_type)
-                        pose_dict[nodeName] = (x, y, z), (qw, qx, qy, qz)
+                        pose_dict[nodeName] = ((x, y, z), (qw, qx, qy, qz))
                     self._gui.move_nodes(group, pose_dict)
             else:
                 for geom_model, geom_data, model_type in zip(
@@ -1614,19 +1757,22 @@ def refresh(self,
             elif Viewer._camera_motion is not None:
                 self.set_camera_transform()
 
-            # Update markers placements.
+            # Update pose, color and scale of the markers, if any
             if Viewer.backend.startswith('panda3d'):
                 pose_dict, material_dict, scale_dict = {}, {}, {}
                 for marker_name, marker_data in self.markers.items():
-                    x, y, z, qx, qy, qz, qw = marker_data["pose"]
-                    pose_dict[marker_name] = (x, y, z), (qw, qx, qy, qz)
-                    material_dict[marker_name] = marker_data["color"]
+                    marker_data = {key: value() if callable(value) else value
+                                   for key, value in marker_data.items()}
+                    (x, y, z), (qx, qy, qz, qw) = marker_data["pose"]
+                    pose_dict[marker_name] = ((x, y, z), (qw, qx, qy, qz))
+                    r, g, b, a = marker_data["color"]
+                    material_dict[marker_name] = (2.0 * r, 2.0 * g, 2.0 * b, a)
                     scale_dict[marker_name] = marker_data["scale"]
                 self._gui.move_nodes(self._markers_group, pose_dict)
                 self._gui.set_materials(self._markers_group, material_dict)
                 self._gui.set_scales(self._markers_group, scale_dict)
 
-            # Refreshing viewer backend manually is necessary for gepetto-gui
+            # Refreshing viewer backend manually if necessary
             if Viewer.backend == 'gepetto-gui':
                 self._gui.refresh()
             elif Viewer.backend.startswith('panda3d'):
@@ -1640,6 +1786,7 @@ def refresh(self,
     def display(self,
                 q: np.ndarray,
                 xyz_offset: Optional[np.ndarray] = None,
+                update_hook: Optional[Callable[[], None]] = None,
                 wait: bool = False) -> None:
         """Update the configuration of the robot.
 
@@ -1650,6 +1797,9 @@ def display(self,
         :param q: Configuration of the robot.
         :param xyz_offset: Freeflyer position offset. Note that it does not
                            check for the robot actually have a freeflyer.
+        :param update_hook: Callable that will be called right after updating
+                            kinematics data. `None` to disable.
+                            Optional: None by default.
         :param wait: Whether or not to wait for rendering to finish.
         """
         assert self._client.model.nq == q.shape[0], (
@@ -1662,8 +1812,12 @@ def display(self,
             q[:3] += xyz_offset
 
         # Update pinocchio data
-        pin.forwardKinematics(self._client.model, self._client.data, q)
         pin.framesForwardKinematics(self._client.model, self._client.data, q)
+        pin.centerOfMass(self._client.model, self._client.data, False)
+
+        # Call custom update hook
+        if update_hook is not None:
+            update_hook()
 
         # Refresh the viewer
         self.refresh(wait)
@@ -1675,10 +1829,16 @@ def replay(self,
                    np.ndarray, Tuple[float, float]]] = (0.0, np.inf),
                speed_ratio: float = 1.0,
                xyz_offset: Optional[np.ndarray] = None,
+               update_hook: Optional[Callable[[float], None]] = None,
                enable_clock: bool = False,
                wait: bool = False) -> None:
         """Replay a complete robot trajectory at a given real-time ratio.
 
+        .. note::
+            Specifying 'udpate_hook' is necessary to be able to display sensor
+            information usch as contact forces. It will be automatically
+            disable otherwise.
+
         .. warning::
             It will alter original robot data if viewer attribute
             `use_theoretical_model` is false.
@@ -1691,26 +1851,39 @@ def replay(self,
         :param xyz_offset: Freeflyer position offset. Note that it does not
                            check for the robot actually have a freeflyer.
                            OPtional: None by default.
+        :param update_hook: Callable that will be called periodically between
+                            every state update. `None` to disable.
+                            Optional: None by default.
         :param wait: Whether or not to wait for rendering to finish.
         """
-        t = [s.t for s in evolution_robot]
+        # Disable display of sensor data if no update hook is provided
+        disable_display_contacts = False
+        if update_hook is None and self._display_contacts:
+            disable_display_contacts = True
+            self.display_contact_forces(False)
+
+        # Replay the whole trajectory at constant speed ratio
+        update_hook_t = None
+        times = [s.t for s in evolution_robot]
         t_simu = time_interval[0]
-        i = bisect_right(t, t_simu)
+        i = bisect_right(times, t_simu)
         init_time = time.time()
         while i < len(evolution_robot):
             try:
                 if enable_clock:
                     Viewer.set_clock(t_simu)
-                s = evolution_robot[i]
-                s_next = evolution_robot[min(i, len(evolution_robot)) - 1]
+                s_next = evolution_robot[min(i, len(times) - 1)]
+                s = evolution_robot[max(i - 1, 0)]
                 ratio = (t_simu - s.t) / (s_next.t - s.t)
                 q = pin.interpolate(self._client.model, s.q, s_next.q, ratio)
                 if Viewer._camera_motion is not None:
                     Viewer._camera_xyzrpy = Viewer._camera_motion(t_simu)
-                self.display(q, xyz_offset, wait)
+                if update_hook is not None:
+                    update_hook_t = partial(update_hook, t_simu)
+                self.display(q, xyz_offset, update_hook_t, wait)
                 t_simu = time_interval[0] + speed_ratio * (
                     time.time() - init_time)
-                i = bisect_right(t, t_simu)
+                i = bisect_right(times, t_simu)
                 wait = False  # Waiting for the first timestep is enough
                 if t_simu > time_interval[1]:
                     break
@@ -1720,7 +1893,14 @@ def replay(self,
                 Viewer.close()
                 return
 
-        # Disable clock after replay if enable and alive
-        if enable_clock and Viewer.is_alive():
-            with self._lock:
-                Viewer.set_clock()
+        # Restore Viewer's state if it has been altered
+        if Viewer.is_alive():
+            # Disable clock after replay if enabled
+            if enable_clock:
+                with self._lock:
+                    Viewer.set_clock()
+
+            # Restore display of sensor data
+            if disable_display_contacts:
+                with self._lock:
+                    self.display_contact_forces(True)
diff --git a/python/jiminy_pywrap/include/jiminy/python/Functors.h b/python/jiminy_pywrap/include/jiminy/python/Functors.h
index f11ca27d4..91a637a31 100644
--- a/python/jiminy_pywrap/include/jiminy/python/Functors.h
+++ b/python/jiminy_pywrap/include/jiminy/python/Functors.h
@@ -19,7 +19,7 @@ namespace python
     template<typename T>
     struct DataInternalBufferType
     {
-        using type = typename std::add_lvalue_reference<T>::type;
+        using type = typename std::add_lvalue_reference_t<T>;
     };
 
     template<>
@@ -55,14 +55,14 @@ namespace python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value, bp::handle<> >
+    std::enable_if_t<is_eigen_v<T>, bp::handle<> >
     FctPyWrapperArgToPython(T & arg)
     {
         return bp::handle<>(getNumpyReference(arg));
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value, bp::handle<> >
+    std::enable_if_t<is_eigen_v<T>, bp::handle<> >
     FctPyWrapperArgToPython(T const & arg)
     {
         return bp::handle<>(getNumpyReference(arg));
@@ -303,8 +303,6 @@ namespace python
 
         std::pair<float64_t, vector3_t> operator() (vector3_t const & posFrame)
         {
-            // Pass the arguments by reference (be careful const qualifiers are lost)
-
             if (heatMapType_ == heatMapType_t::STAIRS)
             {
                 bp::handle<> out1Py(bp::borrowed(out1PyPtr_));
diff --git a/python/jiminy_pywrap/include/jiminy/python/Utilities.h b/python/jiminy_pywrap/include/jiminy/python/Utilities.h
index 6d1650dd3..a0e3bc3b2 100644
--- a/python/jiminy_pywrap/include/jiminy/python/Utilities.h
+++ b/python/jiminy_pywrap/include/jiminy/python/Utilities.h
@@ -82,18 +82,18 @@ namespace python
     }
 
     // Forward declaration
-    template <class Container, bool NoProxy, class DerivedPolicies>
+    template<class Container, bool NoProxy, class DerivedPolicies>
     class vector_indexing_suite_no_contains;
 
     namespace detail
     {
-        template <class Container, bool NoProxy>
+        template<class Container, bool NoProxy>
         class final_vector_derived_policies
             : public vector_indexing_suite_no_contains<Container,
                 NoProxy, bp::detail::final_vector_derived_policies<Container, NoProxy> > {};
     }
 
-    template <class Container,
+    template<class Container,
               bool NoProxy = false,
               class DerivedPolicies = detail::final_vector_derived_policies<Container, NoProxy> >
     class vector_indexing_suite_no_contains : public bp::vector_indexing_suite<Container, NoProxy, DerivedPolicies>
@@ -195,22 +195,21 @@ namespace python
     /// Generic converter from Eigen Matrix to Numpy array by reference
 
     template<typename T>
-    std::enable_if_t<!is_eigen<T>::value, PyObject *>
+    std::enable_if_t<!is_eigen_v<T>, PyObject *>
     getNumpyReference(T & value)
     {
         return getNumpyReferenceFromScalar(value);
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_vector<T>::value, PyObject *>
+    std::enable_if_t<is_eigen_vector_v<T>, PyObject *>
     getNumpyReference(T & value)
     {
         return getNumpyReferenceFromEigenVector(value);
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value
-                 && !is_eigen_vector<T>::value, PyObject *>
+    std::enable_if_t<is_eigen_v<T> && !is_eigen_vector_v<T>, PyObject *>
     getNumpyReference(T & value)
     {
         return getNumpyReferenceFromEigenMatrix(value);
@@ -298,8 +297,7 @@ namespace python
     ///////////////////////////////////////////////////////////////////////////////
 
     template<typename T>
-    std::enable_if_t<!is_vector_v<T>
-                  && !is_eigen<T>::value, bp::object>
+    std::enable_if_t<!is_vector_v<T> && !is_eigen_v<T>, bp::object>
     convertToPython(T const & data, bool const & /* copy */ = true)
     {
         return bp::object(data);
@@ -318,7 +316,7 @@ namespace python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value, bp::object>
+    std::enable_if_t<is_eigen_v<T>, bp::object>
     convertToPython(T & data, bool const & copy = true)
     {
         PyObject * vecPyPtr = getNumpyReference(data);
@@ -330,7 +328,7 @@ namespace python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value, bp::object>
+    std::enable_if_t<is_eigen_v<T>, bp::object>
     convertToPython(T const & data, bool const & copy = true)
     {
         PyObject * vecPyPtr = getNumpyReference(data);
@@ -374,7 +372,7 @@ namespace python
             // Empty on purpose
         }
 
-        template <typename T>
+        template<typename T>
         auto operator()(T const & value) const
         {
             return convertToPython(value, copy_);
@@ -427,16 +425,17 @@ namespace python
     template<bool copy>
     struct result_converter
     {
-        template<typename T>
+        template<typename T, typename = typename std::enable_if_t<
+            copy || std::is_reference<T>::value || is_eigen_ref_v<T> > >
         struct apply
         {
             struct type
             {
-                typedef typename std::remove_reference<T>::type value_type;
+                typedef typename std::remove_reference_t<T> value_type;
 
-                PyObject * operator()(T const & x) const
+                PyObject * operator()(T x) const
                 {
-                    return converterToPython<value_type, copy>::convert(x);
+                    return bp::incref(convertToPython<value_type>(x, copy).ptr());
                 }
 
                 PyTypeObject const * get_pytype(void) const
@@ -489,8 +488,8 @@ namespace python
 
     template<typename T>
     std::enable_if_t<!is_vector_v<T>
-                  && !is_map<T>::value
-                  && !is_eigen<T>::value
+                  && !is_map_v<T>
+                  && !is_eigen_v<T>
                   && !(std::is_integral<T>::value && !std::is_same<T, bool_t>::value)
                   && !std::is_same<T, sensorsDataMap_t>::value, T>
     convertFromPython(bp::object const & dataPy)
@@ -524,14 +523,14 @@ namespace python
             }
             if (std::is_unsigned<T>::value)
             {
-                return bp::extract<typename std::make_signed<T>::type>(dataPy);
+                return bp::extract<typename std::make_signed_t<T> >(dataPy);
             }
-            return bp::extract<typename std::make_unsigned<T>::type>(dataPy);
+            return bp::extract<typename std::make_unsigned_t<T> >(dataPy);
         }
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen<T>::value, T>
+    std::enable_if_t<is_eigen_v<T>, T>
     convertFromPython(bp::object const & dataPy)
     {
         using Scalar = typename T::Scalar;
@@ -547,7 +546,7 @@ namespace python
             }
             Scalar * dataPtr = reinterpret_cast<Scalar *>(dataNumpy.get_data());
             Py_intptr_t const * dataShape = dataNumpy.get_shape();
-            if (is_eigen_vector<T>::value)
+            if (is_eigen_vector_v<T>)
             {
                 return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, 1> >(
                     dataPtr, dataShape[0]);
@@ -567,7 +566,7 @@ namespace python
             }
             Scalar * dataPtr = reinterpret_cast<Scalar *>(dataMatrix.get_data());
             Py_intptr_t const * dataShape = dataMatrix.get_shape();
-            if (is_eigen_vector<T>::value)
+            if (is_eigen_vector_v<T>)
             {
                 return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, 1> >(
                     dataPtr, dataShape[0]);
@@ -580,7 +579,7 @@ namespace python
         }
         else
         {
-            if (is_eigen_vector<T>::value)
+            if (is_eigen_vector_v<T>)
             {
                 return listPyToEigenVector(bp::extract<bp::list>(dataPy));
             }
@@ -679,14 +678,14 @@ namespace python
 
         ~AppendPythonToBoostVariant(void) = default;
 
-        template <typename T>
+        template<typename T>
         std::enable_if_t<!std::is_same<T, configHolder_t>::value, void>
         operator()(T & value)
         {
             value = convertFromPython<T>(*objPy_);
         }
 
-        template <typename T>
+        template<typename T>
         std::enable_if_t<std::is_same<T, configHolder_t>::value, void>
         operator()(T & value)
         {
diff --git a/python/jiminy_pywrap/src/Compatibility.cc b/python/jiminy_pywrap/src/Compatibility.cc
index fc592a3bd..7a38144e5 100644
--- a/python/jiminy_pywrap/src/Compatibility.cc
+++ b/python/jiminy_pywrap/src/Compatibility.cc
@@ -12,7 +12,7 @@ namespace python
 {
     namespace bp = boost::python;
 
-    template <typename T>
+    template<typename T>
     class arrayScalarFromPython
     {
     public:
@@ -76,7 +76,7 @@ namespace python
         static PyTypeObject const* get_pytype() { return &PyUnicode_Type; }
     };
 
-    template <class T, class SlotPolicy>
+    template<class T, class SlotPolicy>
     struct nativeFromPython
     {
     public:
diff --git a/python/jiminy_pywrap/src/Robot.cc b/python/jiminy_pywrap/src/Robot.cc
index c5db117fc..913dc4487 100644
--- a/python/jiminy_pywrap/src/Robot.cc
+++ b/python/jiminy_pywrap/src/Robot.cc
@@ -271,6 +271,9 @@ namespace python
                                     bp::arg("has_freeflyer") = false,
                                     bp::arg("mesh_package_dirs") = bp::list()))
 
+                .add_property("is_locked", bp::make_function(&Robot::getIsLocked,
+                                           bp::return_value_policy<bp::copy_const_reference>()))
+
                 .def("dump_options", &Robot::dumpOptions,
                                      (bp::arg("self"), "json_filename"))
                 .def("load_options", &Robot::loadOptions,
diff --git a/python/jiminy_pywrap/src/Sensors.cc b/python/jiminy_pywrap/src/Sensors.cc
index a6502b029..34dd04098 100644
--- a/python/jiminy_pywrap/src/Sensors.cc
+++ b/python/jiminy_pywrap/src/Sensors.cc
@@ -2,6 +2,7 @@
 #include "jiminy/core/robot/BasicSensors.h"
 #include "jiminy/core/Types.h"
 
+#include <eigenpy/eigenpy.hpp>
 #include <boost/python.hpp>
 
 #include "jiminy/python/Utilities.h"
@@ -75,8 +76,7 @@ namespace python
                 auto & sensorsDataTypeByName = self.at(sensorType).get<IndexByName>();
                 auto sensorDataIt = sensorsDataTypeByName.find(sensorName);
                 Eigen::Ref<vectorN_t const> const & sensorDataValue = sensorDataIt->value;
-                bp::handle<> valuePy(getNumpyReference(sensorDataValue));
-                return bp::object(valuePy);
+                return convertToPython(sensorDataValue, false);
             }
             catch (...)
             {
@@ -91,8 +91,7 @@ namespace python
             try
             {
                 auto & sensorsDataType = self.at(sensorType);
-                bp::handle<> valuePy(getNumpyReference(sensorsDataType.getAll()));
-                return bp::object(valuePy);
+                return convertToPython(sensorsDataType.getAll(), false);
             }
             catch (...)
             {
@@ -145,8 +144,7 @@ namespace python
             bp::list sensorsValue;
             for (auto const & sensorsDataType : self)
             {
-                bp::handle<> valuePy(getNumpyReference(sensorsDataType.second.getAll()));
-                sensorsValue.append(bp::object(valuePy));
+                sensorsValue.append(convertToPython(sensorsDataType.second.getAll(), false));
             }
             return sensorsValue;
         }
@@ -156,8 +154,9 @@ namespace python
             bp::list sensorsDataPy;
             for (auto const & sensorsDataType : self)
             {
-                bp::handle<> valuePy(getNumpyReference(sensorsDataType.second.getAll()));
-                sensorsDataPy.append(bp::make_tuple(sensorsDataType.first, bp::object(valuePy)));
+                sensorsDataPy.append(bp::make_tuple(
+                    sensorsDataType.first,
+                    convertToPython(sensorsDataType.second.getAll(), false)));
             }
             return sensorsDataPy;
         }
@@ -225,15 +224,24 @@ namespace python
             cl
                 .add_property("is_initialized", bp::make_function(&AbstractSensorBase::getIsInitialized,
                                                 bp::return_value_policy<bp::copy_const_reference>()))
-                .add_property("name", bp::make_function(&AbstractSensorBase::getName,
-                                      bp::return_value_policy<bp::copy_const_reference>()))
-                .add_property("idx", bp::make_function(&AbstractSensorBase::getIdx,
-                                     bp::return_value_policy<bp::copy_const_reference>()))
+
                 .add_property("type", bp::make_function(&AbstractSensorBase::getType,
                                       bp::return_value_policy<bp::copy_const_reference>()))
                 .add_property("fieldnames", bp::make_function(&AbstractSensorBase::getFieldnames,
                                             bp::return_value_policy<bp::return_by_value>()))
-                .add_property("data", &PyAbstractSensorVisitor::getData)
+
+                .add_property("name", bp::make_function(&AbstractSensorBase::getName,
+                                      bp::return_value_policy<bp::copy_const_reference>()))
+                .add_property("idx", bp::make_function(&AbstractSensorBase::getIdx,
+                                     bp::return_value_policy<bp::copy_const_reference>()))
+                .add_property("data", bp::make_function(
+                    static_cast<
+                        Eigen::Ref<vectorN_t const> (AbstractSensorBase::*)(void) const
+                    >(&AbstractSensorBase::get),
+                    bp::return_value_policy<result_converter<false> >()),
+                    static_cast<
+                        hresult_t (AbstractSensorBase::*)(Eigen::MatrixBase<vectorN_t> const &)
+                    >(&AbstractSensorBase::set))
 
                 .def("set_options", &PyAbstractSensorVisitor::setOptions)
                 .def("get_options", &AbstractSensorBase::getOptions)
@@ -243,15 +251,6 @@ namespace python
         }
 
     public:
-        static bp::object getData(AbstractSensorBase & self)
-        {
-            // Be careful, it removes the const qualifier, so that the data can be modified from Python
-            Eigen::Ref<vectorN_t const> const & sensorDataValue =
-                const_cast<AbstractSensorBase const &>(self).get();
-            bp::handle<> valuePy(getNumpyReference(sensorDataValue));
-            return bp::object(valuePy);
-        }
-
         static std::string repr(AbstractSensorBase & self)
         {
             std::stringstream s;