增加速度控制器

src/openarm_servo_control/openarm_servo_control/ik_controller.py

@@ -89,6 +89,10 @@ class ArucoIKController(Node):
             max_acceleration=0.01,   # 建议 0.2~0.6，降低以减小加速度
             dt=self.control_period
         )
+        self.Kp = 20.0                   # 比例增益（需调试）
+        self.max_vel = 3.0              # rad/s 每关节上限
+        self.max_acc = 1.0              # rad/s^2 加速度限幅
+
         self.pose_queue = queue.Queue(maxsize=10)  # 用于存储最近的目标位姿，避免处理过旧的指令
         self.ik_ready = False
         self.prev_filtered_pos = None
@@ -312,25 +316,30 @@ class ArucoIKController(Node):
         q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)])
         # q_limited = self.joint_limiter.limit(q_sol)
 
-        # # 计算速度（基于上一帧的 q），并限幅
-        if self.prev_q is None:
-            vel = np.zeros_like(q_sol)
-        else:
-            vel = (q_sol - self.prev_q) / self.control_period
-
-        vel = np.clip(vel, -self.joint_limiter.max_velocity, self.joint_limiter.max_velocity)
-
-        # # 计算加速度并限幅
-        if self.prev_vel is None:
-            acc = np.zeros_like(vel)
-        else:
-            acc = (vel - self.prev_vel) / self.control_period
-
-        acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration)
 
         # 保存为下一次使用
         self.prev_q = q_sol.copy()
-        self.prev_vel = vel.copy()
+        # 得到当前关节（按 self.joint_names 顺序）
+        if self.latest_joint_state is None:
+            return
+        q_curr = np.array([self.latest_joint_state.position[self.latest_joint_state.name.index(j)]
+                        for j in self.joint_names])
+
+        # q_sol 是 IK 求得的目标位置（numpy）
+        q_target = q_sol.copy()
+
+        # 1) 计算速度（比例律）
+        v_des = self.Kp * (q_target - q_curr)
+
+        # 2) 限幅速度与加速度（基于 self.prev_vel）
+        v_des = np.clip(v_des, -self.max_vel, self.max_vel)
+        if getattr(self, 'prev_vel', None) is not None:
+            dv = (v_des - self.prev_vel) / self.control_period
+            dv = np.clip(dv, -self.max_acc, self.max_acc)
+            v_des = self.prev_vel + dv * self.control_period
+
+        # 3) 积分得到下一个位置目标
+        q_cmd = q_curr + v_des * self.control_period
 
         # 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度
         point = JointTrajectoryPoint()
@@ -338,11 +347,11 @@ class ArucoIKController(Node):
         # 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持）
         # if not self.publish_velocity_only:
         #     point.positions = q_limited.tolist()
-        point.positions = q_sol.tolist()
-        point.velocities = vel.tolist()
-        point.accelerations = acc.tolist()
+        point.positions = q_cmd.tolist()
+        # point.velocities = vel.tolist()
+        # point.accelerations = acc.tolist()
         # point.time_from_start.sec = 0
-        # point.time_from_start.nanosec = int(0.1 * 1e9)  # 0.1s
+        # point.time_from_start.nanosec = int(0.05 * 1e9)  # 0.1s
         # msg.data = (q_sol).tolist()
         # self.get_logger().info(f"msg.data: {msg.data}")
         # self.get_logger().info(f"Publishing joint command: vel={np.round(vel,3).tolist()}, acc={np.round(acc,3).tolist()}, pos={np.round(q_limited,3).tolist()}")