把位置控制器换成关节控制器

src/openarm_servo_control/openarm_servo_control/ik_controller.py

@@ -85,8 +85,8 @@ class ArucoIKController(Node):
         self.control_period = 0.05  # 20 ms
         self.joint_limiter = JointLimiter(
             dof=7,
-            max_velocity=0.2,       # 建议 0.1~0.3，降低以减小速度
-            max_acceleration=0.5,   # 建议 0.2~0.6，降低以减小加速度
+            max_velocity=0.01,       # 建议 0.1~0.3，降低以减小速度
+            max_acceleration=0.01,   # 建议 0.2~0.6，降低以减小加速度
             dt=self.control_period
         )
         self.pose_queue = queue.Queue(maxsize=10)  # 用于存储最近的目标位姿，避免处理过旧的指令
@@ -310,17 +310,17 @@ class ArucoIKController(Node):
         jt.joint_names = self.joint_names
         # 计算受限后的关节目标
         q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)])
-        q_limited = self.joint_limiter.limit(q_sol)
+        # q_limited = self.joint_limiter.limit(q_sol)
 
-        # 计算速度（基于上一帧的 q），并限幅
+        # # 计算速度（基于上一帧的 q），并限幅
         if self.prev_q is None:
-            vel = np.zeros_like(q_limited)
+            vel = np.zeros_like(q_sol)
         else:
-            vel = (q_limited - self.prev_q) / self.control_period
+            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:
@@ -329,26 +329,26 @@ class ArucoIKController(Node):
         acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration)
 
         # 保存为下一次使用
-        self.prev_q = q_limited.copy()
+        self.prev_q = q_sol.copy()
         self.prev_vel = vel.copy()
 
         # 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度
-        # point = JointTrajectoryPoint()
-        msg = Float64MultiArray()
+        point = JointTrajectoryPoint()
+        # msg = Float64MultiArray()
         # 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持）
         # if not self.publish_velocity_only:
         #     point.positions = q_limited.tolist()
-
-        # point.velocities = vel.tolist()
-        # point.accelerations = acc.tolist()
+        point.positions = q_sol.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
-        msg.data = (q_sol).tolist()
+        # 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()}")
-        self.float64MultiArrayPublisher_.publish(msg)
-        # jt.points.append(point)
-        # self.traj_pub.publish(jt)
+        # self.float64MultiArrayPublisher_.publish(msg)
+        jt.points.append(point)
+        self.traj_pub.publish(jt)
 
 def main():
     rclpy.init()