Sim motor timeouts (#30)

* added arm motor timeouts to sim * motor timeouts * minor tweaks * changed motor timeout to ros param * fix style check
umrover · Dec 3, 2024 · 383d9ae · 383d9ae
1 parent c5ca25b
commit 383d9ae
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 2 deletions.
diff --git a/config/simulator.yaml b/config/simulator.yaml
@@ -9,6 +9,7 @@ simulator:
     save_rate: 1.0
     save_history: 4096
     headless: false
+    motor_timeout: 100
 
     ref_heading: 90.0  # For the GPS sensor to work
 

diff --git a/simulator/simulator.cpp b/simulator/simulator.cpp
@@ -18,6 +18,8 @@ namespace mrover {
 
             mIkTargetPub = create_publisher<msg::IK>("arm_ik", 1);
 
+            mMotorTimeoutMs = get_parameter("motor_timeout").as_int();
+
             mIsHeadless = get_parameter("headless").as_bool();
             mEnablePhysics = mIsHeadless;
             {

diff --git a/simulator/simulator.hpp b/simulator/simulator.hpp
@@ -84,6 +84,7 @@ namespace mrover {
             int index{};
             boost::container::small_vector<Uniform<ModelUniforms>, 2> visualUniforms;
             boost::container::small_vector<Uniform<ModelUniforms>, 2> collisionUniforms;
+            Clock::time_point lastUpdate = Clock::now();
         };
 
         urdf::Model model;
@@ -263,6 +264,8 @@ namespace mrover {
 
         bool mIsHeadless{};
 
+        int64_t mMotorTimeoutMs{};
+
         // Rendering
 
         GlfwInstance mGlfwInstance;
@@ -362,7 +365,7 @@ namespace mrover {
             }
 
             if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) {
-                URDF const& rover = it->second;
+                URDF& rover = it->second;
 
                 for (std::size_t i = 0; i < names.size(); ++i) {
                     std::string const& name = names[i];
@@ -375,7 +378,8 @@ namespace mrover {
                     }
 
                     std::string const& urdfName = it->second;
-                    URDF::LinkMeta const& linkMeta = rover.linkNameToMeta.at(urdfName);
+                    URDF::LinkMeta& linkMeta = rover.linkNameToMeta.at(urdfName);
+                    linkMeta.lastUpdate = Clock::now();
 
                     auto* motor = std::bit_cast<btMultiBodyJointMotor*>(rover.physics->getLink(linkMeta.index).m_userPtr);
                     assert(motor);

diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp
@@ -53,6 +53,19 @@ namespace mrover {
                 motor->setMaxAppliedImpulse(0.5);
                 motor->setPositionTarget(0);
             }
+            // check if arm motor commands have expired
+            // TODO: fix hard-coded names?
+            for (auto const& name: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) {
+                bool expired = std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs;
+                if (expired) {
+                    int linkIndex = rover.linkNameToMeta.at(name).index;
+                    auto* motor = std::bit_cast<btMultiBodyJointMotor*>(rover.physics->getLink(linkIndex).m_userPtr);
+                    assert(motor);
+                    motor->setVelocityTarget(0, 1);
+                    // set p gain to 0 to stop position control
+                    motor->setPositionTarget(0, 0);
+                }
+            }
         }
 
         float updateDuration = std::clamp(std::chrono::duration_cast<std::chrono::duration<float>>(dt).count(), 0.0f, 0.1f);