From e4bb1c3321f0d60915376a3d2cd44764ca234034 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Thu, 10 Apr 2025 22:30:45 +0200 Subject: [PATCH 01/10] Add some files --- src/CMakeLists.txt | 4 ++++ src/candlewick/multibody/VisualizerClient.h | 13 +++++++++++++ src/candlewick/runtime/main.cpp | 3 +++ 3 files changed, 20 insertions(+) create mode 100644 src/candlewick/multibody/VisualizerClient.h create mode 100644 src/candlewick/runtime/main.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7ade014..bce32d4 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,6 +17,7 @@ ADD_PROJECT_DEPENDENCY( avcodec swscale ) +find_package(ZeroMQ REQUIRED) add_library( candlewick_core @@ -129,6 +130,9 @@ if(BUILD_PINOCCHIO_VISUALIZER) INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ) + + add_executable(candlewick_visualizer candlewick/runtime/main.cpp) + target_link_libraries(candlewick_visualizer PRIVATE candlewick_multibody) endif() # install headers diff --git a/src/candlewick/multibody/VisualizerClient.h b/src/candlewick/multibody/VisualizerClient.h new file mode 100644 index 0000000..4b07e87 --- /dev/null +++ b/src/candlewick/multibody/VisualizerClient.h @@ -0,0 +1,13 @@ +#pragma once + +#include "Multibody.h" +#include +#include + +namespace candlewick { +namespace multibody { + class VisualizerClient final : public pin::visualizers::BaseVisualizer { + public: + }; +} // namespace multibody +} // namespace candlewick diff --git a/src/candlewick/runtime/main.cpp b/src/candlewick/runtime/main.cpp new file mode 100644 index 0000000..a8c1a42 --- /dev/null +++ b/src/candlewick/runtime/main.cpp @@ -0,0 +1,3 @@ +#include "candlewick/multibody/Multibody.h" + +int main() { return 0; } From b13e10c3c8fb7f1c1daf62f62066bbf15ef35034 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 11:51:22 +0200 Subject: [PATCH 02/10] src/CMakeLists.txt : use cppzmq --- src/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bce32d4..196f755 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -131,8 +131,12 @@ if(BUILD_PINOCCHIO_VISUALIZER) LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ) + find_package(cppzmq) add_executable(candlewick_visualizer candlewick/runtime/main.cpp) - target_link_libraries(candlewick_visualizer PRIVATE candlewick_multibody) + target_link_libraries( + candlewick_visualizer + PRIVATE cppzmq candlewick_multibody + ) endif() # install headers From de3a09bf53a868a29b27e338ffece2c330e7afbc Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 14:35:42 +0200 Subject: [PATCH 03/10] RobotScene.h : move invalid_enum() out to public header --- src/candlewick/multibody/RobotScene.cpp | 11 ----------- src/candlewick/multibody/RobotScene.h | 13 +++++++++++++ 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/candlewick/multibody/RobotScene.cpp b/src/candlewick/multibody/RobotScene.cpp index 30d6dab..ef9de9e 100644 --- a/src/candlewick/multibody/RobotScene.cpp +++ b/src/candlewick/multibody/RobotScene.cpp @@ -2,12 +2,10 @@ #include "Components.h" #include "LoadPinocchioGeometry.h" #include "../core/Components.h" -#include "../core/errors.h" #include "../core/Shader.h" #include "../core/Components.h" #include "../core/TransformUniforms.h" #include "../core/Camera.h" -#include "../core/errors.h" #include #include @@ -26,15 +24,6 @@ struct alignas(16) light_ubo_t { alignas(16) GpuMat4 projMat; }; -template - requires std::is_enum_v -[[noreturn]] void -invalid_enum(const char *msg, T type, - std::source_location location = std::source_location::current()) { - terminate_with_message( - std::format("{:s} - {:s}", msg, magic_enum::enum_name(type)), location); -} - void updateRobotTransforms(entt::registry ®istry, const pin::GeometryData &geom_data) { auto robot_view = diff --git a/src/candlewick/multibody/RobotScene.h b/src/candlewick/multibody/RobotScene.h index 25ca14d..a38cd2f 100644 --- a/src/candlewick/multibody/RobotScene.h +++ b/src/candlewick/multibody/RobotScene.h @@ -16,6 +16,19 @@ #include namespace candlewick { + +/// \brief Terminate the application after encountering an invalid enum value. +template + requires std::is_enum_v +[[noreturn]] void +invalid_enum(const char *msg, T type, + std::source_location location = std::source_location::current()) { + char out[64]; + SDL_snprintf(out, 64ul, "Invalid enum: %s - %s", msg, + magic_enum::enum_name(type).data()); + terminate_with_message(out, location); +} + namespace multibody { void updateRobotTransforms(entt::registry ®istry, From b9674ff50428afed7de6d8dddec9136a63de05bc Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 14:44:07 +0200 Subject: [PATCH 04/10] RobotScene : do not clear all MeshMaterialComponent... this is dumb --- src/candlewick/multibody/RobotScene.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/candlewick/multibody/RobotScene.cpp b/src/candlewick/multibody/RobotScene.cpp index ef9de9e..1687bb0 100644 --- a/src/candlewick/multibody/RobotScene.cpp +++ b/src/candlewick/multibody/RobotScene.cpp @@ -389,7 +389,8 @@ void RobotScene::release() { if (!device()) return; - m_registry.clear(); + clearEnvironment(); + clearRobotGeometries(); for (auto &pipeline : renderPipelines) { SDL_ReleaseGPUGraphicsPipeline(device(), pipeline); From 0992a0461b5286c0d40a6deedfaeb905d6a0286b Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 15:03:57 +0200 Subject: [PATCH 05/10] Ur5WithSystems.cpp : move about window button --- examples/Ur5WithSystems.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/examples/Ur5WithSystems.cpp b/examples/Ur5WithSystems.cpp index 9e29b96..f161b8d 100644 --- a/examples/Ur5WithSystems.cpp +++ b/examples/Ur5WithSystems.cpp @@ -273,17 +273,30 @@ int main(int argc, char **argv) { GuiSystem gui_system{ renderer, [&](const Renderer &r) { + IMGUI_CHECKVERSION(); + static bool demo_window_open = true; static bool show_about_window = false; static bool show_plane_vis = true; - ImGui::Begin("Renderer info & controls", nullptr, - ImGuiWindowFlags_AlwaysAutoResize); + if (show_about_window) + showCandlewickAboutWindow(&show_about_window); + + ImGuiWindowFlags window_flags = 0; + window_flags |= ImGuiWindowFlags_AlwaysAutoResize; + window_flags |= ImGuiWindowFlags_MenuBar; + ImGui::Begin("Renderer info & controls", nullptr, window_flags); + + if (ImGui::BeginMenuBar()) { + ImGui::MenuItem("About Candlewick", NULL, &show_about_window); + ImGui::EndMenuBar(); + } ImGui::Text("Video driver: %s", SDL_GetCurrentVideoDriver()); + ImGui::SameLine(); + ImGui::Text("Device driver: %s", r.device.driverName()); ImGui::Text("Display pixel density: %.2f / scale: %.2f", r.window.pixelDensity(), r.window.displayScale()); - ImGui::Text("Device driver: %s", r.device.driverName()); ImGui::SeparatorText("Camera"); bool ortho_change, persp_change; ortho_change = ImGui::RadioButton("Orthographic", (int *)&g_cameraType, @@ -348,9 +361,6 @@ int main(int argc, char **argv) { ImGui::ColorEdit4("plane color", plane_obj.materials[0].baseColor.data()); - if (ImGui::Button("About candlewick")) - show_about_window = true; - ImGui::End(); ImGui::SetNextWindowCollapsed(true, ImGuiCond_Once); From 873adcd51a88e6a937723d22abde8f1a8097974e Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 15:38:39 +0200 Subject: [PATCH 06/10] RobotScene : rename pipeline_tag_component => pipeline_tag + move up he pipeline creation --- src/candlewick/multibody/RobotScene.cpp | 45 +++++++++++-------------- src/candlewick/multibody/RobotScene.h | 2 +- 2 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/candlewick/multibody/RobotScene.cpp b/src/candlewick/multibody/RobotScene.cpp index 1687bb0..afc0338 100644 --- a/src/candlewick/multibody/RobotScene.cpp +++ b/src/candlewick/multibody/RobotScene.cpp @@ -66,9 +66,7 @@ auto RobotScene::pinGeomToPipeline(const coal::CollisionGeometry &geom) void add_pipeline_tag_component(entt::registry ®, entt::entity ent, RobotScene::PipelineType type) { magic_enum::enum_switch( - [®, ent](auto pt) { - reg.emplace>(ent); - }, + [®, ent](auto pt) { reg.emplace>(ent); }, type); } @@ -154,11 +152,18 @@ void RobotScene::loadModels(const pin::GeometryModel &geom_model, const auto &geom_obj = geom_model.geometryObjects[geom_id]; auto meshDatas = loadGeometryObject(geom_obj); PipelineType pipeline_type = pinGeomToPipeline(*geom_obj.geometry); - auto mesh = createMeshFromBatch(device(), meshDatas, true); + Mesh mesh = createMeshFromBatch(device(), meshDatas, true); SDL_assert(validateMesh(mesh)); - // local copy for use const auto layout = mesh.layout(); + if (!renderPipelines[pipeline_type]) { + SDL_Log("Building pipeline for type %s", + magic_enum::enum_name(pipeline_type).data()); + renderPipelines[pipeline_type] = + createPipeline(layout, m_renderer.getSwapchainTextureFormat(), + m_renderer.depthFormat(), pipeline_type); + SDL_assert(renderPipelines[pipeline_type]); + } // add entity for this geometry entt::entity entity = m_registry.create(); @@ -180,16 +185,6 @@ void RobotScene::loadModels(const pin::GeometryModel &geom_model, ShadowPassInfo::create(m_renderer, layout, m_config.shadow_config); } } - - if (!renderPipelines[pipeline_type]) { - SDL_Log("Building pipeline for type %s", - magic_enum::enum_name(pipeline_type).data()); - SDL_GPUGraphicsPipeline *pipeline = - createPipeline(layout, m_renderer.getSwapchainTextureFormat(), - m_renderer.depthFormat(), pipeline_type); - SDL_assert(pipeline); - renderPipelines[pipeline_type] = pipeline; - } } m_initialized = true; } @@ -199,11 +194,10 @@ void RobotScene::updateTransforms() { } void RobotScene::collectOpaqueCastables() { - auto all_view = - m_registry.view>( - entt::exclude); + auto all_view = m_registry.view>( + entt::exclude); m_castables.clear(); @@ -320,7 +314,7 @@ void RobotScene::renderPBRTriangleGeometry(CommandBuffer &command_buffer, auto all_view = m_registry.view>( + pipeline_tag>( entt::exclude); for (auto [ent, tr, obj] : all_view.each()) { const Mat4f modelView = camera.view * tr; @@ -367,12 +361,11 @@ void RobotScene::renderOtherGeometry(CommandBuffer &command_buffer, auto env_view = m_registry.view>( + pipeline_tag>( entt::exclude); - for (auto [entity, tr, obj] : env_view.each()) { - auto &modelMat = tr; + for (auto &&[entity, tr, obj] : env_view.each()) { const Mesh &mesh = obj.mesh; - const Mat4f mvp = viewProj * modelMat; + const Mat4f mvp = viewProj * tr; const auto &color = obj.materials[0].baseColor; command_buffer .pushVertexUniform(VertexUniformSlots::TRANSFORM, &mvp, sizeof(mvp)) @@ -392,7 +385,7 @@ void RobotScene::release() { clearEnvironment(); clearRobotGeometries(); - for (auto &pipeline : renderPipelines) { + for (auto *pipeline : renderPipelines) { SDL_ReleaseGPUGraphicsPipeline(device(), pipeline); pipeline = nullptr; } diff --git a/src/candlewick/multibody/RobotScene.h b/src/candlewick/multibody/RobotScene.h index a38cd2f..38605ca 100644 --- a/src/candlewick/multibody/RobotScene.h +++ b/src/candlewick/multibody/RobotScene.h @@ -76,7 +76,7 @@ namespace multibody { } } - template using pipeline_tag_component = entt::tag; + template using pipeline_tag = entt::tag; struct PipelineConfig { // shader set From 2044c801a809e1090967f34d1fdfab55cbf4be66 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 17:23:34 +0200 Subject: [PATCH 07/10] Dummy pull/push system --- src/candlewick/runtime/main.cpp | 31 ++++++++++++++++++++++++++++++- tests/test_client.py | 14 ++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) create mode 100644 tests/test_client.py diff --git a/src/candlewick/runtime/main.cpp b/src/candlewick/runtime/main.cpp index a8c1a42..788619e 100644 --- a/src/candlewick/runtime/main.cpp +++ b/src/candlewick/runtime/main.cpp @@ -1,3 +1,32 @@ +#include +#include #include "candlewick/multibody/Multibody.h" -int main() { return 0; } +#include +#include + +namespace pin = pinocchio; + +int main() { + zmq::context_t ctx; + zmq::socket_t sock{ctx, zmq::socket_type::pull}; + sock.bind("tcp://127.0.0.1:12000"); + const std::string endpoint = sock.get(zmq::sockopt::last_endpoint); + printf("ZMQ endpoint: %s\n", endpoint.c_str()); + + pin::Model model; + pin::GeometryModel geom_model; + + std::vector recv_models; + + printf("Receiving Model and GeometryModel..."); + + const auto ret = zmq::recv_multipart(sock, std::back_inserter(recv_models)); + assert(*ret == 2); + + model.loadFromString(recv_models[0].to_string()); + std::cout << model << std::endl; + geom_model.loadFromString(recv_models[1].to_string()); + + return 0; +} diff --git a/tests/test_client.py b/tests/test_client.py new file mode 100644 index 0000000..8701535 --- /dev/null +++ b/tests/test_client.py @@ -0,0 +1,14 @@ +import pinocchio as pin +import zmq + + +PORT = 12000 + +ctx = zmq.Context.instance() +sock: zmq.Socket = ctx.socket(zmq.SocketType.PUSH) +sock.connect(f"tcp://127.0.0.1:{PORT}") + +model = pin.buildSampleModelHumanoidRandom() +geom_model = pin.buildSampleGeometryModelHumanoid(model) + +sock.send_multipart([model.saveToString().encode(), geom_model.saveToString().encode()]) From 9cba5777cf79c732b9af6cfd8978b9235c2af344 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 18:00:17 +0200 Subject: [PATCH 08/10] Working transfer of pin models over zmq --- src/candlewick/runtime/main.cpp | 37 ++++++++++++++++++++++++++++++--- tests/test_client.py | 11 +++++++--- 2 files changed, 42 insertions(+), 6 deletions(-) diff --git a/src/candlewick/runtime/main.cpp b/src/candlewick/runtime/main.cpp index 788619e..12b13be 100644 --- a/src/candlewick/runtime/main.cpp +++ b/src/candlewick/runtime/main.cpp @@ -1,13 +1,33 @@ #include #include #include "candlewick/multibody/Multibody.h" +#include "candlewick/multibody/Visualizer.h" #include #include +#include +#include +#include + +namespace cdw = candlewick; namespace pin = pinocchio; +using cdw::multibody::Visualizer; + +int main(int argc, char **argv) { + CLI::App app{"Candlewick visualizer runtime"}; + argv = app.ensure_utf8(argv); + + std::vector viz_dims{1920u, 1080u}; + app.add_option("--dims", viz_dims, "Window dimensions.") + ->capture_default_str(); + + CLI11_PARSE(app, argc, argv); + + if (viz_dims.size() != 2) { + cdw::terminate_with_message("Expected only two values for argument --dims"); + } -int main() { zmq::context_t ctx; zmq::socket_t sock{ctx, zmq::socket_type::pull}; sock.bind("tcp://127.0.0.1:12000"); @@ -19,14 +39,25 @@ int main() { std::vector recv_models; - printf("Receiving Model and GeometryModel..."); + printf("Receiving Model and GeometryModel...\n"); const auto ret = zmq::recv_multipart(sock, std::back_inserter(recv_models)); assert(*ret == 2); + printf("Received %zu messages\n", ret.value()); model.loadFromString(recv_models[0].to_string()); - std::cout << model << std::endl; geom_model.loadFromString(recv_models[1].to_string()); + Visualizer::Config config; + config.width = viz_dims[0]; + config.height = viz_dims[1]; + Visualizer viz{config, model, geom_model}; + + Eigen::VectorXd q0 = pin::neutral(model); + + while (!viz.shouldExit()) { + viz.display(q0); + } + return 0; } diff --git a/tests/test_client.py b/tests/test_client.py index 8701535..3280621 100644 --- a/tests/test_client.py +++ b/tests/test_client.py @@ -1,4 +1,5 @@ import pinocchio as pin +import example_robot_data as erd import zmq @@ -8,7 +9,11 @@ sock: zmq.Socket = ctx.socket(zmq.SocketType.PUSH) sock.connect(f"tcp://127.0.0.1:{PORT}") -model = pin.buildSampleModelHumanoidRandom() -geom_model = pin.buildSampleGeometryModelHumanoid(model) +robot: pin.RobotWrapper = erd.load("solo12") +model = robot.model +geom_model = robot.visual_model -sock.send_multipart([model.saveToString().encode(), geom_model.saveToString().encode()]) +model_str = model.saveToString() +geom_str = geom_model.saveToString() +sock.send_multipart([model_str.encode(), geom_str.encode()]) +print("Sent") From 566942c1015fe5c164e09ee2582bb31b65466af7 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 11 Apr 2025 18:17:42 +0200 Subject: [PATCH 09/10] you can't call terminate() without an exception... --- examples/Visualizer.cpp | 14 +++++++++++--- src/candlewick/core/errors.h | 4 +--- src/candlewick/runtime/main.cpp | 10 +++++----- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/examples/Visualizer.cpp b/examples/Visualizer.cpp index 04ddfd9..436dc27 100644 --- a/examples/Visualizer.cpp +++ b/examples/Visualizer.cpp @@ -10,24 +10,32 @@ #include #include +namespace cdw = candlewick; using namespace candlewick::multibody; using std::chrono::steady_clock; int main(int argc, char **argv) { CLI::App app{"Visualizer example"}; argv = app.ensure_utf8(argv); - double fps; + std::vector window_dims{1920u, 1080u}; + double fps = 50.0; + app.add_option("--dims", window_dims, "Window dimensions.") + ->capture_default_str(); app.add_option("--fps", fps, "Framerate") - ->default_val(50); + ->default_str("50.0"); CLI11_PARSE(app, argc, argv); + if (window_dims.size() != 2) { + cdw::terminate_with_message("Expected only two values for argument --dims"); + } + pin::Model model; pin::GeometryModel geom_model; robot_descriptions::loadModelsFromToml("ur.toml", "ur5_gripper", model, &geom_model, NULL); - Visualizer visualizer{{1920, 1280}, model, geom_model}; + Visualizer visualizer{{window_dims[0], window_dims[1]}, model, geom_model}; assert(!visualizer.hasExternalData()); Eigen::VectorXd q0 = pin::neutral(model); diff --git a/src/candlewick/core/errors.h b/src/candlewick/core/errors.h index 4257b30..f9fdb60 100644 --- a/src/candlewick/core/errors.h +++ b/src/candlewick/core/errors.h @@ -38,11 +38,9 @@ namespace detail { inline void terminate_with_message( std::string_view msg, std::source_location location = std::source_location::current()) { - SDL_LogError( - SDL_LOG_CATEGORY_APPLICATION, "%s", + throw std::runtime_error( detail::error_message_format(location.function_name(), "{:s}", msg) .c_str()); - ::std::terminate(); } [[noreturn]] diff --git a/src/candlewick/runtime/main.cpp b/src/candlewick/runtime/main.cpp index 12b13be..100d6c8 100644 --- a/src/candlewick/runtime/main.cpp +++ b/src/candlewick/runtime/main.cpp @@ -18,13 +18,13 @@ int main(int argc, char **argv) { CLI::App app{"Candlewick visualizer runtime"}; argv = app.ensure_utf8(argv); - std::vector viz_dims{1920u, 1080u}; - app.add_option("--dims", viz_dims, "Window dimensions.") + std::vector window_dims{1920u, 1080u}; + app.add_option("--dims", window_dims, "Window dimensions.") ->capture_default_str(); CLI11_PARSE(app, argc, argv); - if (viz_dims.size() != 2) { + if (window_dims.size() != 2) { cdw::terminate_with_message("Expected only two values for argument --dims"); } @@ -49,8 +49,8 @@ int main(int argc, char **argv) { geom_model.loadFromString(recv_models[1].to_string()); Visualizer::Config config; - config.width = viz_dims[0]; - config.height = viz_dims[1]; + config.width = window_dims[0]; + config.height = window_dims[1]; Visualizer viz{config, model, geom_model}; Eigen::VectorXd q0 = pin::neutral(model); From a32ce4f9fcde28724d26eb50eb2f7cee428d3f0a Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Sun, 13 Apr 2025 12:54:37 +0200 Subject: [PATCH 10/10] core/errors.h : use string_view for _error_message_impl --- src/candlewick/core/errors.cpp | 5 +++-- src/candlewick/core/errors.h | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/candlewick/core/errors.cpp b/src/candlewick/core/errors.cpp index 9bb086d..c28b158 100644 --- a/src/candlewick/core/errors.cpp +++ b/src/candlewick/core/errors.cpp @@ -9,9 +9,10 @@ RAIIException::RAIIException(std::string_view msg, location.file_name(), location.line(), msg.data())) {} namespace detail { - std::string _error_message_impl(const char *fname, std::string_view _fmtstr, + std::string _error_message_impl(std::string_view fname, + std::string_view fmtstr, std::format_args args) { - return std::format("{:s} :: {:s}", fname, std::vformat(_fmtstr, args)); + return std::format("{:s} :: {:s}", fname, std::vformat(fmtstr, args)); } } // namespace detail } // namespace candlewick diff --git a/src/candlewick/core/errors.h b/src/candlewick/core/errors.h index f9fdb60..531a81e 100644 --- a/src/candlewick/core/errors.h +++ b/src/candlewick/core/errors.h @@ -21,7 +21,8 @@ struct RAIIException : std::runtime_error { namespace detail { - std::string _error_message_impl(const char *fname, std::string_view fmtstr, + std::string _error_message_impl(std::string_view fname, + std::string_view fmtstr, std::format_args args); template