add retrigger logic for waist

src/robot_control/include/control_base.hpp

@@ -5,6 +5,8 @@
 #include "trapezoidal_trajectory.hpp"
 #include "motion_parameters.hpp"
 
+#define POSITION_TOLERANCE 0.01
+
 namespace robot_control
 {
     class ControlBase
@@ -71,6 +73,8 @@ namespace robot_control
             // 6. 判断是否运动中（通用逻辑，父类实现）
             virtual bool IsMoving();
 
+            virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
+
             bool checkJointLimits(const std::vector<double>& target_joint);
     };
 

src/robot_control/src/control_base.cpp

@@ -61,20 +61,7 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
 
     if (!is_moving_)
     {
-        bool all_joints_reached = true;  
-        const double position_tolerance = 1e-3; 
-
-        for (size_t i = 0; i < target_joint.size(); i++)
-        {
-            double position_error = std::fabs(joint_position_[i] - target_joint[i]);
-            if (position_error > position_tolerance)
-            {
-                all_joints_reached = false;  
-                break;
-            }
-        }
-
-        if (all_joints_reached)
+        if (IsReached(target_joint))
         {
             joint_positions = target_joint;
 
@@ -165,6 +152,22 @@ bool ControlBase::IsMoving()
     return is_moving_;
 }
 
+bool ControlBase::IsReached(const std::vector<double>& target_joint)
+{
+    bool result = true;
+
+    for (size_t i = 0; i < target_joint.size(); i++)
+    {
+        if (std::abs(joint_position_[i] - target_joint[i]) > POSITION_TOLERANCE)
+        {
+            result = false;
+            break;
+        }
+    }
+
+    return result;
+}
+
 bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
 {
     for (size_t i = 0; i < target_joint.size(); i++)

src/robot_control/src/waist_control.cpp

@@ -67,7 +67,16 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
 
     if (result)
     {
-        is_target_set_ = false;
+        if (IsReached(joint_position_desired_))
+        {
+            std::cout << "Waist reached target position!" << std::endl;
+            is_target_set_ = false;
+        }
+        else
+        {
+            std::cout << "Waist not reached target position!" << std::endl;
+            return false;
+        }
     }
 
     return result;