diff --git a/dynamixel_general_hw/src/dynamixel_general_hw.cpp b/dynamixel_general_hw/src/dynamixel_general_hw.cpp index 499de45..3cbf01e 100644 --- a/dynamixel_general_hw/src/dynamixel_general_hw.cpp +++ b/dynamixel_general_hw/src/dynamixel_general_hw.cpp @@ -1174,6 +1174,12 @@ void DynamixelGeneralHw::write(const ros::Time& time, const ros::Duration& perio normal_modes_.clear(); } } + if (isJntCmdIgnored()) + { + // Reset joint limit interfaces to prevent runaway when going back to normal state + pos_jnt_sat_interface_.reset(); + pos_jnt_soft_interface_.reset(); + } prev_is_servo_ = is_servo_; prev_is_hold_pos_ = is_hold_pos_; }