增加机械臂画圆功能

src/openarm_servo_control/openarm_servo_control/draw_cricle.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+import math
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+
+
+class DrawCircle(Node):
+    def __init__(self):
+        super().__init__("draw_circle")
+
+        # Circle and publish settings
+        self.declare_parameter("topic", "/hand_3d")
+        self.declare_parameter("frame_id", "base_link")
+        self.declare_parameter("center_x", 0.15)
+        self.declare_parameter("center_y", 0.0)
+        self.declare_parameter("center_z", 0.16)
+        self.declare_parameter("radius", 0.08)
+        self.declare_parameter("publish_rate", 50.0)  # Hz
+        self.declare_parameter("angular_speed", 1.5)  # rad/s
+
+        topic = self._get_param_str("topic", "/hand_3d")
+        self.frame_id = self._get_param_str("frame_id", "base_link")
+        self.center_x = self._get_param_float("center_x", 0.15)
+        self.center_y = self._get_param_float("center_y", 0.2)
+        self.center_z = self._get_param_float("center_z", 0.16)
+        self.radius = self._get_param_float("radius", 0.05)
+        self.publish_rate = self._get_param_float("publish_rate", 10.0)
+        self.angular_speed = self._get_param_float("angular_speed", 1.0)
+
+        if self.publish_rate <= 0.0:
+            self.get_logger().warn("publish_rate must be > 0.0, fallback to 50.0")
+            self.publish_rate = 50.0
+
+        self.publisher_ = self.create_publisher(PoseStamped, topic, 10)
+        self.theta = 0.0
+        self.dt = 1.0 / self.publish_rate
+        self.timer = self.create_timer(self.dt, self.publish_circle_pose)
+
+        self.get_logger().info(
+            f"Publishing circle pose on {topic} (frame_id={self.frame_id}, "
+            f"center=({self.center_x:.3f}, {self.center_y:.3f}, {self.center_z:.3f}), "
+            f"radius={self.radius:.3f}, rate={self.publish_rate:.1f}Hz, "
+            f"omega={self.angular_speed:.3f}rad/s)"
+        )
+
+    def _get_param_float(self, name: str, default: float) -> float:
+        value = self.get_parameter(name).value
+        if value is None:
+            return default
+        return float(value)
+
+    def _get_param_str(self, name: str, default: str) -> str:
+        value = self.get_parameter(name).value
+        if value is None:
+            return default
+        return str(value)
+
+    def publish_circle_pose(self):
+        msg = PoseStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.frame_id
+
+        x = self.center_x + self.radius * math.cos(self.theta)
+        y = self.center_y + self.radius * math.sin(self.theta)
+        z = self.center_z
+
+        msg.pose.position.x = x
+        msg.pose.position.y = y
+        msg.pose.position.z = z
+
+        # Keep orientation fixed (identity quaternion)
+        msg.pose.orientation.x = 1.0
+        msg.pose.orientation.y = 0.0
+        msg.pose.orientation.z = 0.0
+        msg.pose.orientation.w = 0.0
+
+        self.publisher_.publish(msg)
+
+        self.theta += self.angular_speed * self.dt
+        if self.theta > 2.0 * math.pi:
+            self.theta -= 2.0 * math.pi
+
+
+def main():
+    rclpy.init()
+    node = DrawCircle()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/openarm_servo_control/openarm_servo_control/ik_controller.py

@@ -89,12 +89,12 @@ class ArucoIKController(Node):
             max_acceleration=0.01,   # 建议 0.2~0.6，降低以减小加速度
             dt=self.control_period
         )
-        self.Kp = 15.0                   # 比例增益（需调试）
+        self.Kp = 25.0                   # 比例增益（需调试）
         self.max_vel = 2.5              # rad/s 每关节上限
         self.max_acc = 0.1              # rad/s^2 加速度限幅
         self.currPose = PoseStamped()
 
-        self.pose_queue = queue.Queue(maxsize=10)  # 用于存储最近的目标位姿，避免处理过旧的指令
+        self.pose_queue = queue.Queue(maxsize=100)  # 用于存储最近的目标位姿，避免处理过旧的指令
         self.ik_ready = False
         self.prev_filtered_pos = None
         self.prev_filtered_quat = None
@@ -161,7 +161,7 @@ class ArucoIKController(Node):
 
         self.timer = self.create_timer(3.0, self.check_ik_service)
         time.sleep(3)
-        self.timer1 = self.create_timer(0.05, self.on_pose)
+        self.timer1 = self.create_timer(0.02, self.on_pose)
         self.startTimer = self.create_timer(0.05, self.readyArm)
         # 用于计算速度/加速度（用于平滑输出）
         self.prev_q = None
@@ -177,7 +177,7 @@ class ArucoIKController(Node):
         # posebuf.pose.position.z += pose.pose.position.z
         posebuf.pose.position.x = pose.pose.position.x
         posebuf.pose.position.y = pose.pose.position.y
-        posebuf.pose.position.z = pose.pose.position.z + 0.2
+        posebuf.pose.position.z = pose.pose.position.z
         # posebuf.pose.orientation.x = 1.0
         # posebuf.pose.orientation.y = 0.0
         # posebuf.pose.orientation.z = 0.0
@@ -300,8 +300,8 @@ class ArucoIKController(Node):
         self.prev_filtered_quat = curr_quat
 
         curr_pos[0] = np.clip(curr_pos[0], -0.8, 0.8)
-        curr_pos[1] = np.clip(curr_pos[1], -0.2, 0.2) + 0.15
+        curr_pos[1] = np.clip(curr_pos[1], -0.5, 0.5) + 0.15
-        curr_pos[2] = np.clip(curr_pos[2], 0.0, 0.3) + 0.27
+        curr_pos[2] = np.clip(curr_pos[2], 0.0, 0.4) + 0.27
         # pose.pose.position.x = 0.1
         # pose.pose.position.y = 0.25
         # pose.pose.position.z = 0.4

src/openarm_servo_control/setup.py

@@ -27,6 +27,7 @@ setup(
             'servo_control_node = openarm_servo_control.servo_control:main',
             'ik_controller_node = openarm_servo_control.ik_controller:main',
             'keyboard_ctrl_node = openarm_servo_control.keyboard_ctrl:main',
+            'draw_circle_node = openarm_servo_control.draw_cricle:main',
         ],
     },
 )