diff --git a/unitree_interface/include/unitree_interface/unitree_sdk_wrapper.hpp b/unitree_interface/include/unitree_interface/unitree_sdk_wrapper.hpp index 65b23f5..7561d28 100644 --- a/unitree_interface/include/unitree_interface/unitree_sdk_wrapper.hpp +++ b/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& indices, const std::vector& position, diff --git a/unitree_interface/src/mode_transitions.cpp b/unitree_interface/src/mode_transitions.cpp index b23c9f5..bce2da7 100644 --- a/unitree_interface/src/mode_transitions.cpp +++ b/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::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::execute(sdk_wrapper); - - if (!std::holds_alternative(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::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::name() - ); + } else { + sdk_wrapper.damp_low(); } return EmergencyMode{}; @@ -108,7 +94,7 @@ namespace unitree_interface { #endif ControlMode Transition::execute(UnitreeSDKWrapper& sdk_wrapper) { - if (sdk_wrapper.damp()) { + if (sdk_wrapper.damp_high()) { return EmergencyMode{}; } @@ -130,30 +116,7 @@ namespace unitree_interface { } ControlMode Transition::execute(UnitreeSDKWrapper& sdk_wrapper) { - auto possible_high_level_mode = Transition::execute(sdk_wrapper); - - if (!std::holds_alternative(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::execute(sdk_wrapper); - - if (std::holds_alternative(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::name() - ); + sdk_wrapper.damp_low(); return EmergencyMode{}; } diff --git a/unitree_interface/src/unitree_interface.cpp b/unitree_interface/src/unitree_interface.cpp index e07254f..44088d0 100644 --- a/unitree_interface/src/unitree_interface.cpp +++ b/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; diff --git a/unitree_interface/src/unitree_sdk_wrapper.cpp b/unitree_interface/src/unitree_sdk_wrapper.cpp index 2fc3f9a..27aa385 100644 --- a/unitree_interface/src/unitree_sdk_wrapper.cpp +++ b/unitree_interface/src/unitree_sdk_wrapper.cpp @@ -204,17 +204,17 @@ 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; } @@ -222,6 +222,43 @@ namespace unitree_interface { return false; } + void UnitreeSDKWrapper::damp_low() { + if (!initialized_ || !low_cmd_pub_) { + RCLCPP_ERROR(logger_, "UnitreeSDKWrapper not initialized"); + return; + } + + std::vector indices; + std::vector position; + std::vector velocity; + std::vector effort; + std::vector kp; + std::vector kd; + + for (const auto& joint : joints::all_joints) { + const auto index = static_cast(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");