Working transfer of pin models over zmq

Author: ManifoldFR

src/candlewick/runtime/main.cpp

@@ -1,13 +1,33 @@
 #include <zmq.hpp>
 #include <zmq_addon.hpp>
 #include "candlewick/multibody/Multibody.h"
+#include "candlewick/multibody/Visualizer.h"
 
 #include <pinocchio/serialization/model.hpp>
 #include <pinocchio/serialization/geometry.hpp>
 
+#include <CLI/App.hpp>
+#include <CLI/Formatter.hpp>
+#include <CLI/Config.hpp>
+
+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<Uint32> 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<zmq::message_t> 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;
 }

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")