add low-level damp mode

unitree_interface/include/unitree_interface/unitree_sdk_wrapper.hpp

@@ -87,7 +87,7 @@ namespace unitree_interface {
             float vyaw
         );
 
-        bool damp();
+        bool damp_high();
 
         bool stand_up();
 
@@ -107,6 +107,8 @@ namespace unitree_interface {
         void release_arms(int steps, int interval_ms);
 
         // ========== Low-level capabilities ==========
+        void damp_low();
+
         void send_low_commands(
             const std::vector<std::uint8_t>& indices,
             const std::vector<float>& position,

unitree_interface/src/mode_transitions.cpp

@@ -28,7 +28,7 @@ namespace unitree_interface {
         the emergency stop procedure.
         */
 
-        const bool damp_success = sdk_wrapper.damp();
+        const bool damp_success = sdk_wrapper.damp_high();
 
         if (!damp_success) {
             RCLCPP_ERROR(
@@ -62,31 +62,17 @@ namespace unitree_interface {
 #endif
 
     ControlMode Transition<IdleMode, EmergencyMode>::execute(UnitreeSDKWrapper& sdk_wrapper) {
-        /*
-        Our modes don't truly reflect unitree's internal controller state. We can land up in
-        a situation wherein we transition from LowLevelMode to IdleMode and attempt to transition
-        to EmergencyMode. No longer knowing whether the internal controller itself is in "high-level"
-        or "low-level" mode, if we erroneously attempt to enter damping mode without high-level
-        control services active, we could possible land up in a dangerous situation.
-        */
-
-        if (!sdk_wrapper.has_active_mode()) {
-            // Not in high-level mode - attempt to transition
-            auto possible_high_level_mode = Transition<IdleMode, HighLevelMode>::execute(sdk_wrapper);
-
-            if (!std::holds_alternative<HighLevelMode>(possible_high_level_mode)) {
-                return IdleMode{};
+        if (sdk_wrapper.has_active_mode()) {
+            if (!sdk_wrapper.damp_high()) {
+                RCLCPP_ERROR(
+                    sdk_wrapper.get_logger(),
+                    "Call to damp_high failed during %s to Emergency transition. "
+                    "The system will be left in emergency mode. Repeated calls to damp(estop) may be required",
+                    ControlModeTraits<IdleMode>::name()
+                );
             }
-        }
-
-        // At this point, we should have high-level services active
-        if (!sdk_wrapper.damp()) {
-            RCLCPP_ERROR(
-                sdk_wrapper.get_logger(),
-                "Call to damp failed during %s to Emergency transition. "
-                "The system will be left in emergency mode. Repeated calls to damp(estop) may be required",
-                ControlModeTraits<IdleMode>::name()
-            );
+        } else {
+            sdk_wrapper.damp_low();
         }
 
         return EmergencyMode{};
@@ -108,7 +94,7 @@ namespace unitree_interface {
 #endif
 
     ControlMode Transition<HighLevelMode, EmergencyMode>::execute(UnitreeSDKWrapper& sdk_wrapper) {
-        if (sdk_wrapper.damp()) {
+        if (sdk_wrapper.damp_high()) {
             return EmergencyMode{};
         }
 
@@ -130,30 +116,7 @@ namespace unitree_interface {
     }
 
     ControlMode Transition<LowLevelMode, EmergencyMode>::execute(UnitreeSDKWrapper& sdk_wrapper) {
-        auto possible_high_level_mode = Transition<LowLevelMode, HighLevelMode>::execute(sdk_wrapper);
-
-        if (!std::holds_alternative<HighLevelMode>(possible_high_level_mode)) {
-            return LowLevelMode{};
-        }
-
-        if (sdk_wrapper.damp()) {
-            return EmergencyMode{};
-        }
-
-        // The internal controller should be high-level at this point, so we need to rollback to low-level
-        auto possible_low_level_mode = Transition<HighLevelMode, LowLevelMode>::execute(sdk_wrapper);
-
-        if (std::holds_alternative<LowLevelMode>(possible_low_level_mode)) {
-            return EmergencyMode{};
-        }
-
-        // damp() failed, and we also failed to transition back to low-level
-        RCLCPP_ERROR(
-            sdk_wrapper.get_logger(),
-            "Call to damp failed during %s to Emergency transition. "
-            "The system will be left in emergency mode. Repeated calls to damp(estop) may be required",
-            ControlModeTraits<LowLevelMode>::name()
-        );
+        sdk_wrapper.damp_low();
 
         return EmergencyMode{};
     }

unitree_interface/src/unitree_interface.cpp

@@ -316,7 +316,7 @@ namespace unitree_interface {
             const std::int64_t stand_up_delay = get_parameter("ready_locomotion_stand_up_delay").as_int();
             const std::int64_t start_delay = get_parameter("ready_locomotion_start_delay").as_int();
 
-            if (!sdk_wrapper_->damp()) {
+            if (!sdk_wrapper_->damp_high()) {
                 response->success = false;
                 response->message = "damp() failed";
                 return;

unitree_interface/src/unitree_sdk_wrapper.cpp

@@ -204,24 +204,61 @@ namespace unitree_interface {
         return false;
     }
 
-    bool UnitreeSDKWrapper::damp() {
+    bool UnitreeSDKWrapper::damp_high() {
         if (!initialized_ || !loco_client_) {
             RCLCPP_ERROR(logger_, "UnitreeSDKWrapper not initialized");
             return false;
         }
 
-        RCLCPP_INFO(logger_, "Entering damp mode");
+        RCLCPP_INFO(logger_, "Entering high-level damp mode");
         const std::int32_t ret = loco_client_->Damp();
 
         if (ret == 0) {
-            RCLCPP_INFO(logger_, "Damp mode activated");
+            RCLCPP_INFO(logger_, "High-level damp mode activated");
             return true;
         }
 
         RCLCPP_WARN(logger_, "Damp failed with error code: %d", ret);
         return false;
     }
 
+    void UnitreeSDKWrapper::damp_low() {
+        if (!initialized_ || !low_cmd_pub_) {
+            RCLCPP_ERROR(logger_, "UnitreeSDKWrapper not initialized");
+            return;
+        }
+
+        std::vector<std::uint8_t> indices;
+        std::vector<float> position;
+        std::vector<float> velocity;
+        std::vector<float> effort;
+        std::vector<float> kp;
+        std::vector<float> kd;
+
+        for (const auto& joint : joints::all_joints) {
+            const auto index = static_cast<std::uint8_t>(joint);
+            indices.push_back(index);
+            position.push_back(0.0F);
+            velocity.push_back(0.0F);
+            effort.push_back(0.0F);
+            kp.push_back(Damp::kp[index]);
+            kd.push_back(Damp::kd[index]);
+        }
+
+        auto command = construct_low_cmd(
+            indices,
+            position,
+            velocity,
+            effort,
+            kp,
+            kd
+        );
+
+        low_cmd_pub_->Write(command);
+
+        RCLCPP_INFO(logger_, "Low-level damp command sent");
+    }
+
     bool UnitreeSDKWrapper::stand_up() {
         if (!initialized_ || !loco_client_) {
             RCLCPP_ERROR(logger_, "UnitreeSDKWrapper not initialized");