Merge pull request #89 from Simple-Robotics/next

ManifoldFR · web-flow · commit 7e9df0de7fed · 2025-07-03T17:28:48.000+02:00
Improvements to GUI reporting and interactivity for debug elements
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,9 +7,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
+- bindings/python : add expose-debug.cpp, expose DebugMeshComponent
+
 ### Changed
 
 - slightly rework file dialog GUI
+- core/DebugScene : make setupPipelines() private, do not use optional
+- multibody/Visualizer/GUI : display info for robot debug frames
 
 ### Fixed
 
diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
@@ -7,6 +7,7 @@ find_package(eigenpy 3.2.0 REQUIRED)
 
 set(
   pycandlewick_SOURCES
+  src/expose-debug.cpp
   src/expose-mesh-data.cpp
   src/expose-renderer.cpp
   src/module.cpp
diff --git a/bindings/python/src/expose-debug.cpp b/bindings/python/src/expose-debug.cpp
@@ -0,0 +1,23 @@
+#include "fwd.hpp"
+#include "candlewick/core/DebugScene.h"
+
+#include <eigenpy/std-vector.hpp>
+
+using namespace candlewick;
+
+void exposeDebug() {
+#define _c(name) value(#name, DebugPipelines::name)
+  bp::enum_<DebugPipelines>("DebugPipelines")
+      ._c(TRIANGLE_FILL)
+      ._c(TRIANGLE_LINE);
+#undef _c
+
+  bp::class_<DebugMeshComponent, boost::noncopyable>("DebugMeshComponent",
+                                                     bp::no_init)
+      .def_readwrite("pipeline_type", &DebugMeshComponent::pipeline_type)
+      .def_readonly("colors", &DebugMeshComponent::colors)
+      .def_readonly("enable", &DebugMeshComponent::enable)
+      .def_readwrite("scale", &DebugMeshComponent::scale, "Debug model scale.");
+
+  eigenpy::StdVectorPythonVisitor<std::vector<Float4>>::expose("StdVec_Vec4");
+}
diff --git a/bindings/python/src/expose-renderer.cpp b/bindings/python/src/expose-renderer.cpp
@@ -9,7 +9,7 @@ BOOST_PYTHON_FUNCTION_OVERLOADS(adfs_overloads,
 void exposeRenderer() {
   bp::class_<Device, boost::noncopyable>("Device", bp::no_init)
       .def("driverName", &Device::driverName, ("self"_a))
-      .def("shaderFormats", &Device::shaderFormats);
+      .def("shaderFormats", &Device::shaderFormats, ("self"_a));
 
   bp::def("get_num_gpu_drivers", SDL_GetNumGPUDrivers,
           "Get number of available GPU drivers.");
diff --git a/bindings/python/src/expose-visualizer.cpp b/bindings/python/src/expose-visualizer.cpp
@@ -8,13 +8,21 @@
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/multibody/geometry.hpp>
 
+using namespace candlewick;
 using namespace candlewick::multibody;
 
 #define DEF_PROP_PROXY(name)                                                   \
   add_property(#name, bp::make_function(                                       \
                           +[](Visualizer &v) -> auto & { return v.name(); },   \
                           bp::return_internal_reference<>()))
 
+static auto visualizer_get_frame_debugs(Visualizer &viz) {
+  auto view = viz.registry.view<DebugMeshComponent, const PinFrameComponent>();
+  bp::list out;
+  view.each([&](auto &dmc, auto) { out.append(boost::ref(dmc)); });
+  return out;
+}
+
 void exposeVisualizer() {
   using pinocchio::python::VisualizerPythonVisitor;
 
@@ -34,9 +42,7 @@ void exposeVisualizer() {
       .def(VisualizerPythonVisitor<Visualizer>{})
       .def_readonly("renderer", &Visualizer::renderer)
       .def_readwrite("worldSceneBounds", &Visualizer::worldSceneBounds)
-      .add_property("device",
-                    bp::make_function(&Visualizer::device,
-                                      bp::return_internal_reference<>()))
+      .DEF_PROP_PROXY(device)
       .def(
           "takeScreenshot",
           +[](Visualizer &viz, const std::string &filename) {
@@ -69,6 +75,9 @@ void exposeVisualizer() {
            "by ID.")
       .def("removeFramesViz", &Visualizer::removeFramesViz, ("self"_a),
            "Remove visualization for all frames.")
+      .def("getDebugFrames", &visualizer_get_frame_debugs, ("self"_a),
+           "Get the DebugMeshComponent objects associated with the current "
+           "debug frames.")
 // fix for Pinocchio 3.5.0
 #if PINOCCHIO_VERSION_AT_MOST(3, 5, 0)
       .DEF_PROP_PROXY(model)
diff --git a/bindings/python/src/module.cpp b/bindings/python/src/module.cpp
@@ -8,6 +8,7 @@ using namespace candlewick;
 
 void exposeMeshData();
 void exposeRenderer();
+void exposeDebug();
 #ifdef CANDLEWICK_WITH_FFMPEG_SUPPORT
 void exposeVideoRecorder();
 #endif
@@ -40,6 +41,7 @@ BOOST_PYTHON_MODULE(pycandlewick) {
 
   exposeMeshData();
   exposeRenderer();
+  exposeDebug();
 #ifdef CANDLEWICK_WITH_FFMPEG_SUPPORT
   exposeVideoRecorder();
 #endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -11,7 +11,7 @@ add_library(
   candlewick/core/Device.cpp
   candlewick/core/math_util.cpp
   candlewick/core/errors.cpp
-  candlewick/core/FileDialogGui.cpp
+  candlewick/core/file_dialog_gui.cpp
   candlewick/core/GuiSystem.cpp
   candlewick/core/Mesh.cpp
   candlewick/core/RenderContext.cpp
diff --git a/src/candlewick/core/DebugScene.cpp b/src/candlewick/core/DebugScene.cpp
@@ -27,30 +27,30 @@ DebugScene::DebugScene(DebugScene &&other)
 std::tuple<entt::entity, DebugMeshComponent &>
 DebugScene::addTriad(const Float3 &scale) {
   auto triad_datas = loadTriadSolid();
-  std::vector<GpuVec4> triad_colors(3);
+  std::vector<Float4> triad_colors(3);
   Mesh triad = createMeshFromBatch(device(), triad_datas, true);
   for (size_t i = 0; i < 3; i++) {
     triad_colors[i] = triad_datas[i].material.baseColor;
   }
   setupPipelines(triad.layout());
-  auto entity = m_registry.create();
+  entt::entity entity = m_registry.create();
   auto &item = m_registry.emplace<DebugMeshComponent>(
-      entity, DebugPipelines::TRIANGLE_FILL, std::move(triad), triad_colors);
-  item.scale = scale;
+      entity, DebugPipelines::TRIANGLE_FILL, std::move(triad), triad_colors,
+      true, scale);
   m_registry.emplace<TransformComponent>(entity, Mat4f::Identity());
   return {entity, item};
 }
 
 std::tuple<entt::entity, DebugMeshComponent &>
-DebugScene::addLineGrid(std::optional<Float4> color) {
+DebugScene::addLineGrid(const Float4 &color) {
   auto grid_data = loadGrid(20);
   Mesh grid = createMesh(device(), grid_data, true);
-  GpuVec4 grid_color = color.value_or(grid_data.material.baseColor);
 
   setupPipelines(grid.layout());
   auto entity = m_registry.create();
   auto &item = m_registry.emplace<DebugMeshComponent>(
-      entity, DebugPipelines::LINE, std::move(grid), std::vector{grid_color});
+      entity, DebugPipelines::TRIANGLE_LINE, std::move(grid),
+      std::vector{color});
   m_registry.emplace<TransformComponent>(entity, Mat4f::Identity());
   return {entity, item};
 }
@@ -124,7 +124,7 @@ void DebugScene::render(CommandBuffer &cmdBuf, const Camera &camera) const {
     case DebugPipelines::TRIANGLE_FILL:
       SDL_BindGPUGraphicsPipeline(render_pass, m_trianglePipeline);
       break;
-    case DebugPipelines::LINE:
+    case DebugPipelines::TRIANGLE_LINE:
       SDL_BindGPUGraphicsPipeline(render_pass, m_linePipeline);
       break;
     }
diff --git a/src/candlewick/core/DebugScene.h b/src/candlewick/core/DebugScene.h
@@ -5,15 +5,14 @@
 #include "RenderContext.h"
 #include "math_types.h"
 
-#include <optional>
 #include <entt/entity/registry.hpp>
 #include <entt/signal/sigh.hpp>
 
 namespace candlewick {
 
 enum class DebugPipelines {
   TRIANGLE_FILL,
-  LINE,
+  TRIANGLE_LINE,
 };
 
 class DebugScene;
@@ -33,21 +32,26 @@ struct IDebugSubSystem {
 struct DebugMeshComponent {
   DebugPipelines pipeline_type;
   Mesh mesh;
-  std::vector<GpuVec4> colors;
+  std::vector<Float4> colors;
   bool enable = true;
   Float3 scale = Float3::Ones();
 };
 
 /// \brief %Scene for organizing debug entities and render systems.
 ///
 /// This implements a basic render system for DebugMeshComponent.
+///
+/// This scene and all subsystems are assumed to use the same shaders and
+/// pipelines.
 class DebugScene {
   entt::registry &m_registry;
   const RenderContext &m_renderer;
   SDL_GPUGraphicsPipeline *m_trianglePipeline{nullptr};
   SDL_GPUGraphicsPipeline *m_linePipeline{nullptr};
   std::vector<std::unique_ptr<IDebugSubSystem>> m_subsystems;
 
+  void setupPipelines(const MeshLayout &layout);
+
 public:
   enum { TRANSFORM_SLOT = 0 };
   enum { COLOR_SLOT = 0 };
@@ -70,15 +74,14 @@ class DebugScene {
     return static_cast<System &>(*p);
   }
 
-  /// \brief Setup pipelines; this will only have an effect **ONCE**.
-  void setupPipelines(const MeshLayout &layout);
-
   /// \brief Just the basic 3D triad.
   std::tuple<entt::entity, DebugMeshComponent &>
   addTriad(const Float3 &scale = Float3::Ones());
+
   /// \brief Add a basic line grid.
+  /// \param color Grid color.
   std::tuple<entt::entity, DebugMeshComponent &>
-  addLineGrid(std::optional<Float4> color = std::nullopt);
+  addLineGrid(const Float4 &color = Float4::Ones());
 
   void update() {
     for (auto &system : m_subsystems) {
diff --git a/src/candlewick/core/DepthAndShadowPass.cpp b/src/candlewick/core/DepthAndShadowPass.cpp
@@ -153,9 +153,9 @@ ShadowMapPass::ShadowMapPass(const Device &device, const MeshLayout &layout,
       .props = 0,
   };
 
+  shadowMap = Texture(device, texInfo, "Shadow atlas");
   this->configureAtlasRegions(config);
 
-  shadowMap = Texture(device, texInfo, "Shadow atlas");
   pipeline = create_depth_pass_pipeline(device, layout, format,
                                         {
                                             SDL_GPU_CULLMODE_FRONT,
diff --git a/src/candlewick/core/file_dialog_gui.cpp b/src/candlewick/core/file_dialog_gui.cpp
diff --git a/src/candlewick/multibody/RobotDebug.cpp b/src/candlewick/multibody/RobotDebug.cpp
@@ -16,53 +16,54 @@ entt::entity RobotDebugSystem::addFrameTriad(DebugScene &scene,
 }
 
 entt::entity RobotDebugSystem::addFrameVelocityArrow(DebugScene &scene,
-                                                     pin::FrameIndex frame_id) {
+                                                     pin::FrameIndex frame_id,
+                                                     float scale) {
   entt::registry &reg = scene.registry();
   MeshData arrow_data = loadArrowSolid(false);
   Mesh mesh = createMesh(scene.device(), arrow_data, true);
-  GpuVec4 color = 0xFF217Eff_rgbaf;
+  Float4 color = 0xFF217Eff_rgbaf;
 
   auto entity = reg.create();
-  reg.emplace<DebugMeshComponent>(entity, DebugPipelines::TRIANGLE_FILL,
-                                  std::move(mesh), std::vector{color});
+  auto &dmc = reg.emplace<DebugMeshComponent>(
+      entity, DebugPipelines::TRIANGLE_FILL, std::move(mesh),
+      std::vector{color}, true);
+  dmc.scale << 0.333f, 0.333f, scale;
   reg.emplace<PinFrameVelocityComponent>(entity, frame_id);
   reg.emplace<TransformComponent>(entity, Mat4f::Identity());
   return entity;
 }
 
-void RobotDebugSystem::updateFrames(entt::registry &reg) {
-  auto view = reg.view<const PinFrameComponent, const DebugMeshComponent,
-                       TransformComponent>();
-  for (auto &&[ent, frame_id, dmc, tr] : view.each()) {
-    Mat4f pose{m_robotData.oMf[frame_id].cast<float>()};
-    auto D = dmc.scale.homogeneous().asDiagonal();
-    tr.noalias() = pose * D;
+void RobotDebugSystem::update(DebugScene &scene) {
+  auto &reg = scene.registry();
+  {
+    auto view = reg.view<const PinFrameComponent, const DebugMeshComponent,
+                         TransformComponent>();
+    for (auto &&[ent, frame_id, dmc, tr] : view.each()) {
+      Mat4f pose{m_robotData.oMf[frame_id].cast<float>()};
+      auto D = dmc.scale.homogeneous().asDiagonal();
+      tr.noalias() = pose * D;
+    }
   }
-}
-
-void RobotDebugSystem::updateFrameVelocities(entt::registry &reg) {
-  constexpr float vel_scale = 0.5f;
 
-  auto view = reg.view<const PinFrameVelocityComponent,
-                       const DebugMeshComponent, TransformComponent>();
-  for (auto &&[ent, fvc, dmc, tr] : view.each()) {
+  auto view_vel = reg.view<const PinFrameVelocityComponent,
+                           const DebugMeshComponent, TransformComponent>();
+  for (auto &&[ent, fvc, dmc, tr] : view_vel.each()) {
     Motionf vel =
         pin::getFrameVelocity(m_robotModel, m_robotData, fvc, pin::LOCAL)
             .cast<float>();
 
     const SE3f pose = m_robotData.oMf[fvc].cast<float>();
     Eigen::Quaternionf quatf;
     tr = pose.toHomogeneousMatrix();
-    auto v = vel.linear();
-    Eigen::DiagonalMatrix<float, 3> scaleMatrix(0.3f, 0.3f,
-                                                vel_scale * v.norm());
+    Float3 scale = dmc.scale;
+    scale.z() *= vel.linear().norm();
 
     // the arrow mesh is posed z-up by default.
     // we need to rotate towards where the velocity is pointing,
     // then transform to the frame space.
-    quatf.setFromTwoVectors(Float3::UnitZ(), v);
-    Mat3f R2 = quatf.toRotationMatrix() * scaleMatrix;
+    quatf.setFromTwoVectors(Float3::UnitZ(), vel.linear());
     auto R = tr.topLeftCorner<3, 3>();
+    Mat3f R2 = quatf.toRotationMatrix() * scale.asDiagonal();
     R.applyOnTheRight(R2);
   }
 }
diff --git a/src/candlewick/multibody/RobotDebug.h b/src/candlewick/multibody/RobotDebug.h
@@ -10,31 +10,25 @@ namespace candlewick::multibody {
 /// Supports drawing a triad for a frame. Member \ref pinData must
 /// refer to an existing pinocchio::Data object at all times.
 struct RobotDebugSystem final : IDebugSubSystem {
-  inline static const Float3 DEFAULT_TRIAD_SCALE = Float3::Constant(0.3333f);
 
   RobotDebugSystem(const pin::Model &model, const pin::Data &data)
       : IDebugSubSystem(), m_robotModel(model), m_robotData(data) {}
 
   entt::entity addFrameTriad(DebugScene &scene, pin::FrameIndex frame_id,
-                             const Float3 &scale = DEFAULT_TRIAD_SCALE);
+                             const Float3 &scale = Float3::Constant(0.3333f));
 
   entt::entity addFrameVelocityArrow(DebugScene &scene,
-                                     pin::FrameIndex frame_id);
+                                     pin::FrameIndex frame_id,
+                                     float scale = 0.5f);
 
   /// \brief Update the transform components for the debug visual entities,
   /// according to their frame placements and velocities.
   ///
   /// \warning We expect pinocchio::updateFramePlacements() or
   /// pinocchio::framesForwardKinematics() to be called first!
-  void update(DebugScene &scene) {
-    updateFrames(scene.registry());
-    updateFrameVelocities(scene.registry());
-  }
+  void update(DebugScene &scene);
 
 private:
-  void updateFrames(entt::registry &reg);
-  void updateFrameVelocities(entt::registry &reg);
-
   const pin::Model &m_robotModel;
   const pin::Data &m_robotData;
 };
diff --git a/src/candlewick/multibody/Visualizer.cpp b/src/candlewick/multibody/Visualizer.cpp
@@ -81,7 +81,7 @@ void Visualizer::initialize() {
   robotScene.setConfig(rconfig);
   robotScene.loadModels(visualModel(), visualData());
 
-  robotDebug = &debugScene.addSystem<RobotDebugSystem>(this->model(), data());
+  m_robotDebug = &debugScene.addSystem<RobotDebugSystem>(this->model(), data());
   std::tie(m_triad, std::ignore) = debugScene.addTriad();
   std::tie(m_grid, std::ignore) = debugScene.addLineGrid();
 
@@ -241,10 +241,10 @@ void Visualizer::stopRecording() {
 
 void Visualizer::addFrameViz(pin::FrameIndex id, bool show_velocity) {
   assert(robotDebug);
-  m_debug_frame_pos.push_back(robotDebug->addFrameTriad(debugScene, id));
+  m_debug_frame_pos.push_back(m_robotDebug->addFrameTriad(debugScene, id));
   if (show_velocity)
     m_debug_frame_vel.push_back(
-        robotDebug->addFrameVelocityArrow(debugScene, id));
+        m_robotDebug->addFrameVelocityArrow(debugScene, id));
 }
 
 } // namespace candlewick::multibody
diff --git a/src/candlewick/multibody/Visualizer.h b/src/candlewick/multibody/Visualizer.h
@@ -56,7 +56,7 @@ class Visualizer final : public BaseVisualizer {
   bool m_showGui = true;
   bool m_shouldExit = false;
   entt::entity m_grid, m_triad;
-  RobotDebugSystem *robotDebug = nullptr;
+  RobotDebugSystem *m_robotDebug = nullptr;
   std::vector<entt::entity> m_debug_frame_pos;
   std::vector<entt::entity> m_debug_frame_vel;
 
diff --git a/src/candlewick/multibody/pinocchio_info_gui.cpp b/src/candlewick/multibody/pinocchio_info_gui.cpp
diff --git a/src/candlewick/multibody/visualizer_gui.cpp b/src/candlewick/multibody/visualizer_gui.cpp
diff --git a/src/candlewick/utils/WriteTextureToImage.cpp b/src/candlewick/utils/WriteTextureToImage.cpp

Original file line number	Diff line number	Diff line change
`@@ -153,9 +153,9 @@ ShadowMapPass::ShadowMapPass(const Device &device, const MeshLayout &layout,`
`153`	`153`	`.props = 0,`
`154`	`154`	`};`
`155`	`155`
	`156`	`+ shadowMap = Texture(device, texInfo, "Shadow atlas");`
`156`	`157`	`this->configureAtlasRegions(config);`
`157`	`158`
`158`		`- shadowMap = Texture(device, texInfo, "Shadow atlas");`
`159`	`159`	`pipeline = create_depth_pass_pipeline(device, layout, format,`
`160`	`160`	`{`
`161`	`161`	`SDL_GPU_CULLMODE_FRONT,`