diff --git a/src/openarm_servo_control/openarm_servo_control/draw_cricle.py b/src/openarm_servo_control/openarm_servo_control/draw_cricle.py new file mode 100644 index 0000000..26d1812 --- /dev/null +++ b/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() diff --git a/src/openarm_servo_control/openarm_servo_control/ik_controller.py b/src/openarm_servo_control/openarm_servo_control/ik_controller.py index 2d5b0cd..313073c 100644 --- a/src/openarm_servo_control/openarm_servo_control/ik_controller.py +++ b/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[2] = np.clip(curr_pos[2], 0.0, 0.3) + 0.27 + 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.4) + 0.27 # pose.pose.position.x = 0.1 # pose.pose.position.y = 0.25 # pose.pose.position.z = 0.4 diff --git a/src/openarm_servo_control/setup.py b/src/openarm_servo_control/setup.py index 44c11dd..530dc86 100644 --- a/src/openarm_servo_control/setup.py +++ b/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', ], }, )