multibody: allow displaying external forces (applied to a frame)

CHANGELOG.md

@@ -7,10 +7,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
-- core : Add `LambertMaterial`
-- multibody/Visualizer : remove default world triad
-  * user must add it explicitly
-- multibody/Visualizer : pass scale to addFrameViz()
+- multibody : enable displaying forces applies to frame (with lifetime) (https://github.com/Simple-Robotics/candlewick/pull/93)
+- core/DebugScene : add function to add arrow entities (https://github.com/Simple-Robotics/candlewick/pull/93)
+- core : Add `LambertMaterial` (https://github.com/Simple-Robotics/candlewick/pull/92)
+- multibody/Visualizer : remove default world triad (https://github.com/Simple-Robotics/candlewick/pull/92)
+  * (user must add it explicitly)
+- multibody/Visualizer : pass scale to addFrameViz() (https://github.com/Simple-Robotics/candlewick/pull/92)
+- core/DebugScene : ensure we re-use `Mesh` objects effectively (https://github.com/Simple-Robotics/candlewick/pull/92)
 
 ## [0.8.0] - 2025-07-25
 

bindings/python/src/expose-visualizer.cpp

@@ -29,6 +29,7 @@ void exposeVisualizer() {
 
   eigenpy::OptionalConverter<ConstVectorRef, std::optional>::registration();
   eigenpy::OptionalConverter<Vector3, std::optional>::registration();
+  eigenpy::OptionalConverter<float, std::optional>::registration();
   eigenpy::detail::NoneToPython<std::nullopt_t>::registration();
 
   bp::class_<Visualizer::Config>("VisualizerConfig", bp::init<>("self"_a))
@@ -79,6 +80,9 @@ void exposeVisualizer() {
             "scale"_a = std::nullopt, "vel_scale"_a = std::nullopt),
            "Add visualization (triad and frame velocity) for the given frame "
            "by ID.")
+      .def("setFrameExternalForce", &Visualizer::setFrameExternalForce,
+           ("self"_a, "frame_id", "force", "lifetime"_a = 5u),
+           "Add visualization of the body forces at a frame.")
       .def("removeFramesViz", &Visualizer::removeFramesViz, ("self"_a),
            "Remove visualization for all frames.")
       .def("getDebugFrames", &visualizer_get_frame_debugs, ("self"_a),

examples/python/ur10_loop_force.py

@@ -0,0 +1,69 @@
+import pinocchio as pin
+import time
+import numpy as np
+from utils import add_floor_geom
+from candlewick import Visualizer, VisualizerConfig
+
+try:
+    import example_robot_data as erd
+except ImportError as import_error:
+    raise ImportError(
+        "example-robot-data package not found. Please install "
+        "it (from e.g. pip or conda)."
+    ) from import_error
+
+robot = erd.load("ur10")
+model: pin.Model = robot.model
+data: pin.Data = robot.data
+visual_model: pin.GeometryModel = robot.visual_model
+add_floor_geom(visual_model)
+visual_data = visual_model.createData()
+
+config = VisualizerConfig()
+config.width = 1920
+config.height = 1080
+viz = Visualizer(config, model, visual_model, data=data, visual_data=visual_data)
+
+q0 = pin.neutral(model)
+v0 = np.zeros(model.nv)
+
+t = 0.0
+dt = 0.02
+frame_id = model.getFrameId("elbow_joint")
+joint_id = model.getJointId("elbow_joint")
+viz.addFrameViz(model.getFrameId("ee_link"), show_velocity=True)
+q = q0.copy()
+v = v0.copy()
+
+i = 0
+
+
+def get_force(t: float):
+    ef = np.zeros(3)
+    ef[1] = 1.2 * np.cos(2 * t)
+    return ef
+
+
+while not viz.shouldExit and i < 1000:
+    alpha = np.sin(t)
+    ext_forces = [pin.Force.Zero() for _ in range(model.njoints)]
+    dw = np.random.randn(3)
+    f = ext_forces[joint_id]
+    f.linear[:] = get_force(t)
+    if i == 0:
+        viz.setFrameExternalForce(frame_id, force=f, lifetime=100)
+    elif i > 100:
+        viz.setFrameExternalForce(frame_id, f)
+
+    a_des = np.zeros(model.nv)
+    u0 = pin.rnea(model, data, q, v, a_des)
+    a = pin.aba(model, data, q, v, u0, ext_forces)
+
+    pin.forwardKinematics(model, data, q, v)
+    viz.display()
+
+    time.sleep(dt)
+    t += dt
+    v += dt * a
+    q = pin.integrate(model, q, v * dt)
+    i += 1

src/candlewick/core/DebugScene.cpp

@@ -65,6 +65,16 @@ DebugScene::addLineGrid(const Float4 &color) {
   return {entity, item};
 }
 
+entt::entity DebugScene::addArrow(const Float4 &color) {
+  auto entity = m_registry.create();
+  auto &dmc = m_registry.emplace<DebugMeshComponent>(
+      entity, DebugPipelines::TRIANGLE_FILL, DebugMeshType::ARROW,
+      std::vector{color}, true);
+  dmc.scale << 0.333f, 0.333f, 1.0f;
+  m_registry.emplace<TransformComponent>(entity, Mat4f::Identity());
+  return entity;
+}
+
 void DebugScene::setupPipelines(const MeshLayout &layout) {
   if (m_linePipeline && m_trianglePipeline)
     return;
@@ -165,5 +175,6 @@ void DebugScene::release() {
   // clean up all DebugMeshComponent objects.
   auto view = m_registry.view<DebugMeshComponent>();
   m_registry.destroy(view.begin(), view.end());
+  m_sharedMeshes.clear();
 }
 } // namespace candlewick

src/candlewick/core/DebugScene.h

@@ -100,6 +100,9 @@ class DebugScene {
   std::tuple<entt::entity, DebugMeshComponent &>
   addLineGrid(const Float4 &color = Float4::Ones());
 
+  /// \brief Add an arrow debug entity.
+  entt::entity addArrow(const Float4 &color = 0xf351ff_rgbaf);
+
   void update() {
     for (auto &system : m_subsystems) {
       system->update();

src/candlewick/core/RenderContext.cpp

@@ -21,6 +21,18 @@ RenderContext::RenderContext(Device &&device_, Window &&window_,
   createDepthTexture(suggested_depth_format);
 }
 
+bool RenderContext::waitAndAcquireSwapchain(CommandBuffer &command_buffer) {
+  assert(SDL_IsMainThread());
+  return SDL_WaitAndAcquireGPUSwapchainTexture(command_buffer, window,
+                                               &swapchain, NULL, NULL);
+}
+
+bool RenderContext::acquireSwapchain(CommandBuffer &command_buffer) {
+  assert(SDL_IsMainThread());
+  return SDL_AcquireGPUSwapchainTexture(command_buffer, window, &swapchain,
+                                        NULL, NULL);
+}
+
 void RenderContext::createDepthTexture(
     SDL_GPUTextureFormat suggested_depth_format) {
   auto [width, height] = window.size();

src/candlewick/core/RenderContext.h

@@ -42,20 +42,12 @@ struct RenderContext {
 
   /// \brief Wait until swapchain is available, then acquire it.
   /// \sa acquireSwapchain()
-  bool waitAndAcquireSwapchain(CommandBuffer &command_buffer) {
-    assert(SDL_IsMainThread());
-    return SDL_WaitAndAcquireGPUSwapchainTexture(command_buffer, window,
-                                                 &swapchain, NULL, NULL);
-  }
+  bool waitAndAcquireSwapchain(CommandBuffer &command_buffer);
 
   /// \brief Acquire GPU swapchain.
   /// \warning This can only be called from the main thread (see SDL docs for
   /// the meaning of "main thread").
-  bool acquireSwapchain(CommandBuffer &command_buffer) {
-    assert(SDL_IsMainThread());
-    return SDL_AcquireGPUSwapchainTexture(command_buffer, window, &swapchain,
-                                          NULL, NULL);
-  }
+  bool acquireSwapchain(CommandBuffer &command_buffer);
 
   bool waitForSwapchain() { return SDL_WaitForGPUSwapchain(device, window); }
 

src/candlewick/multibody/RobotDebug.cpp

@@ -35,9 +35,9 @@ void RobotDebugSystem::update() {
     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>()};
+      const SE3f pose = m_robotData->oMf[frame_id].cast<float>();
       auto D = dmc.scale.homogeneous().asDiagonal();
-      tr.noalias() = pose * D;
+      tr.noalias() = pose.toHomogeneousMatrix() * D;
     }
   }
 
@@ -63,6 +63,41 @@ void RobotDebugSystem::update() {
       R.applyOnTheRight(R2);
     }
   }
+
+  {
+    auto view = reg.view<const ExternalForceComponent, const DebugMeshComponent,
+                         TransformComponent>();
+    for (auto &&[ent, arrow, dmc, tr] : view.each()) {
+      pin::FrameIndex frame_id = arrow.frame_id;
+      const SE3f pose = m_robotData->oMf[frame_id].cast<float>();
+      tr = pose.toHomogeneousMatrix();
+
+      const auto &f = arrow.force;
+
+      // base scale
+      Float3 scale = dmc.scale;
+      scale.z() *= std::tanh(f.linear().norm());
+
+      auto quat = Quaternionf::FromTwoVectors(Float3::UnitZ(), f.linear());
+      Eigen::Ref<Mat3f> R = tr.topLeftCorner<3, 3>();
+      Mat3f R2 = quat.toRotationMatrix() * scale.asDiagonal();
+      R.applyOnTheRight(R2);
+    }
+  }
+
+  // cleanup expired force arrows
+  {
+    auto view = reg.group<ExternalForceComponent>();
+    for (auto [ent, arrow] : view.each()) {
+      if (--arrow.lifetime <= 0) {
+#ifndef NDEBUG
+        SDL_Log("Force arrow for frame %zu has expired... destroy.",
+                arrow.frame_id);
+#endif
+        reg.destroy(ent);
+      }
+    }
+  }
 }
 
 template <class... Ts>
@@ -76,7 +111,8 @@ void destroy_debug_entities_from_types(entt::registry &reg) {
 
 void RobotDebugSystem::destroyEntities() {
   destroy_debug_entities_from_types<const PinFrameComponent,
-                                    const PinFrameVelocityComponent>(
+                                    const PinFrameVelocityComponent,
+                                    const ExternalForceComponent>(
       m_scene.registry());
 }
 

src/candlewick/multibody/RobotDebug.h

@@ -2,10 +2,19 @@
 
 #include "Multibody.h"
 #include "../core/DebugScene.h"
+#include <pinocchio/spatial/force.hpp>
 
 namespace candlewick::multibody {
 
-/// \brief A debug system for use with Pinocchio geometries.
+/// \brief Display an external force using an arrow.
+/// This is a transient component which has its own lifetime.
+struct ExternalForceComponent {
+  pin::FrameIndex frame_id;     //< Frame at which the force applies
+  Forcef force{Forcef::Zero()}; //< Force value
+  int lifetime = 1;             //< Arrow lifetime
+};
+
+/// \brief A debug system for use with Pinocchio models and geometries.
 ///
 /// Supports drawing a triad for a frame. Member \ref pinData must
 /// refer to an existing pinocchio::Data object at all times.

src/candlewick/multibody/Visualizer.cpp

@@ -273,6 +273,29 @@ void Visualizer::addFrameViz(pin::FrameIndex id, bool show_velocity,
         id, vel_scale.value_or(RobotDebugSystem::DEFAULT_VEL_SCALE));
 }
 
+void Visualizer::setFrameExternalForce(pin::FrameIndex frame_id,
+                                       const pin::Force &force_,
+                                       Uint32 initial_lifetime) {
+  Forcef force = force_.cast<float>();
+  auto view = registry.group<ExternalForceComponent>();
+  for (auto &&[ent, arrow] : view.each()) {
+    if (arrow.frame_id == frame_id) {
+      arrow.force = force;
+      arrow.lifetime++;
+      return;
+    }
+  }
+
+  // otherwise, create the arrow
+#ifndef NDEBUG
+  SDL_Log("Force arrow not found for frame %zu, adding arrow with lifetime %u",
+          frame_id, initial_lifetime);
+#endif
+  auto ent = debugScene.addArrow();
+  registry.emplace<ExternalForceComponent>(ent, frame_id, force,
+                                           initial_lifetime);
+}
+
 void Visualizer::removeFramesViz() {
   assert(m_robotDebug);
   m_robotDebug->destroyEntities();

src/candlewick/multibody/Visualizer.h

@@ -157,6 +157,14 @@ class Visualizer final : public BaseVisualizer {
                    std::optional<Vector3> scale = std::nullopt,
                    std::optional<float> vel_scale = std::nullopt);
 
+  /// \brief Add visualization for the body forces expressed at the local frame
+  /// of a joint.
+  /// \param frame_id Model frame index.
+  /// \param force Force value.
+  /// \param lifetime Force arrow lifetime.
+  void setFrameExternalForce(pin::FrameIndex frame_id, const pin::Force &force,
+                             Uint32 lifetime = 5u);
+
   /// \brief Remove all frame visualizations.
   void removeFramesViz();