fix: stop ctrl write from fighting MuJoCo viewer when no controller is connected

simulate/src/main.cc

@@ -102,6 +102,11 @@ namespace
 
   // control noise variables
   mjtNum *ctrlnoise = nullptr;
+  // Pause/resume handshake for safe model reload: PhysicsLoop signals the
+  // bridge to stop before freeing m/d, bridge confirms before Physics proceeds.
+  std::atomic<bool> g_bridge_pause_request{false};
+  std::atomic<bool> g_bridge_paused{false};
+
 
   using Seconds = std::chrono::duration<double>;
 
@@ -350,13 +355,19 @@ namespace
         {
           sim.Load(mnew, dnew, sim.dropfilename);
 
+          g_bridge_pause_request.store(true);
+          while (!g_bridge_paused.load())
+              std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
           mj_deleteData(d);
           mj_deleteModel(m);
 
           m = mnew;
           d = dnew;
           mj_forward(m, d);
 
+          g_bridge_pause_request.store(false);
+
           // allocate ctrlnoise
           free(ctrlnoise);
           ctrlnoise = (mjtNum *)malloc(sizeof(mjtNum) * m->nu);
@@ -587,23 +598,32 @@ void *UnitreeSdk2BridgeThread(void *arg)
   unitree::robot::ChannelFactory::Instance()->Init(param::config.domain_id, param::config.interface);
 
 
-  int body_id = mj_name2id(m, mjOBJ_BODY, "torso_link");
-  if (body_id < 0) {
-    body_id = mj_name2id(m, mjOBJ_BODY, "base_link");
-  }
-  param::config.band_attached_link = 6 * body_id;
-
-  std::unique_ptr<UnitreeSDK2BridgeBase> interface = nullptr;
-  if (m->nu > NUM_MOTOR_IDL_GO) {
-    interface = std::make_unique<G1Bridge>(m, d);
-  } else {
-    interface = std::make_unique<Go2Bridge>(m, d);
-  }
-  interface->start();
-
   while (true)
   {
-    sleep(1);
+    int body_id = mj_name2id(m, mjOBJ_BODY, "torso_link");
+    if (body_id < 0) {
+      body_id = mj_name2id(m, mjOBJ_BODY, "base_link");
+    }
+    param::config.band_attached_link = 6 * body_id;
+
+    std::unique_ptr<UnitreeSDK2BridgeBase> interface = nullptr;
+    if (m->nu > NUM_MOTOR_IDL_GO) {
+      interface = std::make_unique<G1Bridge>(m, d);
+    } else {
+      interface = std::make_unique<Go2Bridge>(m, d);
+    }
+    interface->start();
+
+    while (!g_bridge_pause_request.load())
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+    interface->pre_destroy();
+    interface.reset();
+    g_bridge_paused.store(true);
+
+    while (g_bridge_pause_request.load())
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    g_bridge_paused.store(false);
   }
 }
 //------------------------------------------ main --------------------------------------------------

simulate/src/unitree_sdk2_bridge.h

@@ -10,6 +10,9 @@
 #include <unitree/idl/hg/IMUState_.hpp>
 
 #include <iostream>
+#include <atomic>
+#include <chrono>
+#include <thread>
 
 #include "param.h"
 #include "physics_joystick.h"
@@ -40,6 +43,7 @@ class UnitreeSDK2BridgeBase
     }
 
     virtual void start() {}
+    virtual void pre_destroy() {}
 
     void printSceneInformation()
     {
@@ -165,18 +169,35 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController;
         wireless_controller->joystick = joystick;
     }
 
+    ~RobotBridge()
+    {
+        pre_destroy();
+    }
+
     void start()
     {
-        thread_ = std::make_shared<unitree::common::RecurrentThread>(
-            "unitree_bridge", UT_CPU_ID_NONE, 1000, [this]() { this->run(); });
+        stop_flag_.store(false);
+        run_thread_ = std::thread([this]() {
+            while (!stop_flag_.load()) {
+                this->run();
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            }
+        });
+    }
+
+    void pre_destroy() override
+    {
+        stop_flag_.store(true);
+        if (run_thread_.joinable())
+            run_thread_.join();
     }
 
     virtual void run()
     {
         if(!mj_data_) return;
         if(lowstate->joystick) { lowstate->joystick->update(); }
-        // lowcmd
-        {
+        // lowcmd — only apply when a real command has been received
+        if (!lowcmd->isTimeout()) {
             std::lock_guard<std::mutex> lock(lowcmd->mutex_);
             for(int i(0); i<num_motor_; i++) {
                 auto & m = lowcmd->msg_.motor_cmd()[i];
@@ -251,7 +272,8 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController;
     std::unique_ptr<LowState_t> lowstate;
 
 private:
-    unitree::common::RecurrentThreadPtr thread_;
+    std::atomic<bool> stop_flag_{false};
+    std::thread run_thread_;
 };
 
 using Go2Bridge = RobotBridge<unitree::robot::go2::subscription::LowCmd, unitree::robot::go2::publisher::LowState>;