增加运动限速

src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py

@@ -11,10 +11,83 @@ import time
 
 POSITION_DEADZONE = 0.010  # 5 mm
 
+class JointLimiter:
+    def __init__(self,
+                 dof,
+                 max_velocity=0.5,        # rad/s
+                 max_acceleration=1.0,    # rad/s^2
+                 dt=0.02):                # 控制周期
+
+        self.dof = dof
+        self.max_velocity = max_velocity
+        self.max_acceleration = max_acceleration
+        self.dt = dt
+
+        self.q = np.zeros(dof)
+        self.dq = np.zeros(dof)
+        self.initialized = False
+
+    def reset(self, q_init):
+        self.q = np.array(q_init, dtype=float)
+        self.dq = np.zeros(self.dof)
+        self.initialized = True
+
+    def limit(self, q_target):
+        q_target = np.array(q_target, dtype=float)
+
+        if not self.initialized:
+            self.reset(q_target)
+            return q_target
+
+        # ---------- 位置误差 ----------
+        error = q_target - self.q
+
+        # ---------- 理想速度 ----------
+        dq_desired = error / self.dt
+
+        # ---------- 速度限制 ----------
+        dq_desired = np.clip(
+            dq_desired,
+            -self.max_velocity,
+            self.max_velocity
+        )
+
+        # ---------- 加速度限制 ----------
+        ddq = (dq_desired - self.dq) / self.dt
+
+        ddq = np.clip(
+            ddq,
+            -self.max_acceleration,
+            self.max_acceleration
+        )
+
+        # 更新速度
+        self.dq = self.dq + ddq * self.dt
+
+        # 再做一次速度保护
+        self.dq = np.clip(
+            self.dq,
+            -self.max_velocity,
+            self.max_velocity
+        )
+
+        # 更新位置
+        self.q = self.q + self.dq * self.dt
+
+        return self.q.copy()
+
 class ArucoIKController(Node):
 
     def __init__(self):
         super().__init__("aruco_ik_controller")
+        self.control_period = 0.02  # 20 ms
+        self.joint_limiter = JointLimiter(
+            dof=7,
+            max_velocity=0.4,       # 建议 0.3~0.6
+            max_acceleration=1.2,   # 建议 1.0~2.0
+            dt=self.control_period
+        )
+
         self.ik_ready = False
         self.prev_filtered_pos = None
         self.prev_filtered_quat = None
@@ -140,7 +213,7 @@ class ArucoIKController(Node):
         req.ik_request.pose_stamped.pose.orientation.w = curr_quat[3]
         req.ik_request.ik_link_name = self.ee_link
         req.ik_request.timeout.sec = 1
-        req.ik_request.timeout.nanosec = 0  # 0.2s
+        req.ik_request.timeout.nanosec = 200_000_000  # 0.2s
         # req.ik_request.robot_state.joint_state.name = seed_state.name
         # req.ik_request.robot_state.joint_state.position = seed_state.position
         req.ik_request.robot_state.joint_state = seed_state
@@ -157,14 +230,16 @@ class ArucoIKController(Node):
         if res.error_code.val != res.error_code.SUCCESS:
             self.get_logger().warn("IK failed")
             return
-
+        
         jt = JointTrajectory()
         jt.joint_names = self.joint_names
 
         point = JointTrajectoryPoint()
-        point.positions = res.solution.joint_state.position[:len(self.joint_names)]
+        self.q_limited = self.joint_limiter.limit(res.solution.joint_state.position[:len(self.joint_names)])
+        # point.positions = res.solution.joint_state.position[:len(self.joint_names)]
+        point.positions = self.q_limited.tolist()
         point.time_from_start.sec = 0
-        point.time_from_start.nanosec = 300_000_00  # 0.3s
+        point.time_from_start.nanosec = int(self.control_period * 1e9)  # 0.3s
 
         jt.points.append(point)
         self.traj_pub.publish(jt)