diff --git a/CHANGELOG.md b/CHANGELOG.md index 17e9ad9a..32834f2a 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/bindings/python/src/expose-visualizer.cpp b/bindings/python/src/expose-visualizer.cpp index 90bb9aa4..366b201d 100644 --- a/bindings/python/src/expose-visualizer.cpp +++ b/bindings/python/src/expose-visualizer.cpp @@ -29,6 +29,7 @@ void exposeVisualizer() { eigenpy::OptionalConverter::registration(); eigenpy::OptionalConverter::registration(); + eigenpy::OptionalConverter::registration(); eigenpy::detail::NoneToPython::registration(); bp::class_("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), diff --git a/examples/python/ur10_loop_force.py b/examples/python/ur10_loop_force.py new file mode 100644 index 00000000..e6bdd437 --- /dev/null +++ b/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 diff --git a/src/candlewick/core/DebugScene.cpp b/src/candlewick/core/DebugScene.cpp index 5b234ebb..1a26cba7 100644 --- a/src/candlewick/core/DebugScene.cpp +++ b/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( + entity, DebugPipelines::TRIANGLE_FILL, DebugMeshType::ARROW, + std::vector{color}, true); + dmc.scale << 0.333f, 0.333f, 1.0f; + m_registry.emplace(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(); m_registry.destroy(view.begin(), view.end()); + m_sharedMeshes.clear(); } } // namespace candlewick diff --git a/src/candlewick/core/DebugScene.h b/src/candlewick/core/DebugScene.h index 01fc841d..e713224e 100644 --- a/src/candlewick/core/DebugScene.h +++ b/src/candlewick/core/DebugScene.h @@ -100,6 +100,9 @@ class DebugScene { std::tuple 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(); diff --git a/src/candlewick/core/RenderContext.cpp b/src/candlewick/core/RenderContext.cpp index e06a96cb..e697d282 100644 --- a/src/candlewick/core/RenderContext.cpp +++ b/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(); diff --git a/src/candlewick/core/RenderContext.h b/src/candlewick/core/RenderContext.h index 445f92eb..e7f9adf0 100644 --- a/src/candlewick/core/RenderContext.h +++ b/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); } diff --git a/src/candlewick/multibody/RobotDebug.cpp b/src/candlewick/multibody/RobotDebug.cpp index 7089808b..1db0c949 100644 --- a/src/candlewick/multibody/RobotDebug.cpp +++ b/src/candlewick/multibody/RobotDebug.cpp @@ -35,9 +35,9 @@ void RobotDebugSystem::update() { auto view = reg.view(); for (auto &&[ent, frame_id, dmc, tr] : view.each()) { - Mat4f pose{m_robotData->oMf[frame_id].cast()}; + const SE3f pose = m_robotData->oMf[frame_id].cast(); 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(); + for (auto &&[ent, arrow, dmc, tr] : view.each()) { + pin::FrameIndex frame_id = arrow.frame_id; + const SE3f pose = m_robotData->oMf[frame_id].cast(); + 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 R = tr.topLeftCorner<3, 3>(); + Mat3f R2 = quat.toRotationMatrix() * scale.asDiagonal(); + R.applyOnTheRight(R2); + } + } + + // cleanup expired force arrows + { + auto view = reg.group(); + 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 @@ -76,7 +111,8 @@ void destroy_debug_entities_from_types(entt::registry ®) { void RobotDebugSystem::destroyEntities() { destroy_debug_entities_from_types( + const PinFrameVelocityComponent, + const ExternalForceComponent>( m_scene.registry()); } diff --git a/src/candlewick/multibody/RobotDebug.h b/src/candlewick/multibody/RobotDebug.h index 2064b4f9..cfc04322 100644 --- a/src/candlewick/multibody/RobotDebug.h +++ b/src/candlewick/multibody/RobotDebug.h @@ -2,10 +2,19 @@ #include "Multibody.h" #include "../core/DebugScene.h" +#include 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. diff --git a/src/candlewick/multibody/Visualizer.cpp b/src/candlewick/multibody/Visualizer.cpp index 66c5ec49..f308849d 100644 --- a/src/candlewick/multibody/Visualizer.cpp +++ b/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(); + auto view = registry.group(); + 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(ent, frame_id, force, + initial_lifetime); +} + void Visualizer::removeFramesViz() { assert(m_robotDebug); m_robotDebug->destroyEntities(); diff --git a/src/candlewick/multibody/Visualizer.h b/src/candlewick/multibody/Visualizer.h index bd9f2fda..71051f10 100644 --- a/src/candlewick/multibody/Visualizer.h +++ b/src/candlewick/multibody/Visualizer.h @@ -157,6 +157,14 @@ class Visualizer final : public BaseVisualizer { std::optional scale = std::nullopt, std::optional 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();