diff --git a/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py b/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py new file mode 100644 index 0000000..5e99c15 --- /dev/null +++ b/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py @@ -0,0 +1,248 @@ +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import PoseStamped +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from moveit_msgs.srv import GetPositionIK +from sensor_msgs.msg import JointState +import numpy as np +from scipy.spatial.transform import Rotation as R + + + + +POSITION_DEADZONE = 0.050 # 5 mm +# ---- 配置 ---- +LEFT_ARM_JOINTS = [ + "openarm_left_joint1", + "openarm_left_joint2", + "openarm_left_joint3", + "openarm_left_joint4", + "openarm_left_joint5", + "openarm_left_joint6", + "openarm_left_joint7", +] + +CONTROL_PERIOD = 0.02 # 20ms +MAX_VELOCITY = 0.5 +MAX_ACCELERATION = 1.0 +POSE_ALPHA = 0.3 + +# ---- 平滑/限制类 ---- +class PoseFilter: + def __init__(self, alpha=0.3): + self.alpha = alpha + self.pos = None + self.ori = None + + def slerp_quat(q0, q1, alpha): + """ + q0, q1: numpy array [x, y, z, w] + alpha: float, 0~1 + """ + r0 = R.from_quat(q0) + r1 = R.from_quat(q1) + r_slerp = R.slerp(0, 1, [r0, r1])([alpha])[0] + return r_slerp.as_quat() + + def filter(self, new_pos, new_ori): + if self.pos is None: + self.pos = np.array([new_pos.x, new_pos.y, new_pos.z]) + self.ori = np.array([new_ori.x, new_ori.y, new_ori.z, new_ori.w]) + return self.pos, self.ori + self.pos = self.alpha * np.array([new_pos.x, new_pos.y, new_pos.z]) + (1 - self.alpha) * self.pos + self.ori = self.slerp_quat(self.ori, np.array([new_ori.x, new_ori.y, new_ori.z, new_ori.w]), self.alpha) + return self.pos, self.ori + +class JointLimiter: + def __init__(self, joint_names, max_velocity=0.5, max_acceleration=1.0, dt=0.02): + self.joint_names = joint_names + self.max_velocity = max_velocity + self.max_acceleration = max_acceleration + self.dt = dt + self.prev_q = np.zeros(len(joint_names)) + self.prev_delta = np.zeros(len(joint_names)) + + def limit(self, q_target): + q_target = np.array(q_target) + delta = q_target - self.prev_q + max_delta = self.max_velocity * self.dt + delta = np.clip(delta, -max_delta, max_delta) + delta_change = delta - self.prev_delta + max_delta_change = self.max_acceleration * self.dt + delta_change = np.clip(delta_change, -max_delta_change, max_delta_change) + delta = self.prev_delta + delta_change + q_next = self.prev_q + delta + self.prev_q = q_next + self.prev_delta = delta + return q_next + +class ArucoIKController(Node): + + def __init__(self): + super().__init__("aruco_ik_controller") + self.pose_filter = PoseFilter(POSE_ALPHA) + self.joint_limiter = JointLimiter(LEFT_ARM_JOINTS, MAX_VELOCITY, MAX_ACCELERATION, CONTROL_PERIOD) + self.ik_ready = False + self.prev_filtered_pos = None + self.prev_joint_angles = None + # === 参数（根据你的 openarm 改）=== + self.arm_group = "left_arm" + self.base_frame = "base_link" + self.ee_link = "openarm_left_link7" + + self.joint_names = [ + "openarm_left_joint1", + "openarm_left_joint2", + "openarm_left_joint3", + "openarm_left_joint4", + "openarm_left_joint5", + "openarm_left_joint6", + "openarm_left_joint7", + ] + + self.latest_joint_state = None + + self.create_subscription( + JointState, + "/joint_states", + self.joint_state_cb, + 10, + ) + self.traj_pub = self.create_publisher( + JointTrajectory, + "/left_joint_trajectory_controller/joint_trajectory", + 10 + ) + + self.ik_client = self.create_client( + GetPositionIK, + "/compute_ik" + ) + + self.get_logger().info("Waiting for IK service...") + self.ik_client.wait_for_service() + + self.create_subscription( + PoseStamped, + "/hand_3d", # 已经转到 base 的 ArUco + self.on_pose, + 10 + ) + + self.timer = self.create_timer(3.0, self.check_ik_service) + + def joint_state_cb(self, msg: JointState): + self.latest_joint_state = msg + + def build_seed_state(self): + if self.latest_joint_state is None: + return None + + seed = [] + for j in self.joint_names: + try: + idx = self.latest_joint_state.name.index(j) + seed.append(self.latest_joint_state.position[idx]) + except ValueError: + self.get_logger().error(f"Joint {j} not in joint_states") + return None + + js = JointState() + js.name = self.joint_names + js.position = seed + return js + + def check_ik_service(self): + if self.ik_client.service_is_ready(): + self.get_logger().info("IK service ready!") + self.ik_ready = True + self.timer.cancel() + + def on_pose(self, pose: PoseStamped): + if not self.ik_ready: + self.get_logger().warn("IK service not ready, skip IK") + return + seed_state = self.build_seed_state() + if seed_state is None: + self.get_logger().warn("No valid seed, skip IK") + return + + req = GetPositionIK.Request() + req.ik_request.group_name = self.arm_group + + pos, ori = self.pose_filter.filter(pose.pose.position, pose.pose.orientation) + + if self.prev_filtered_pos is not None: + if np.linalg.norm(filtered_pose.pose.position - self.prev_filtered_pos) < POSITION_DEADZONE: + return # EE 位置变化太小，跳过 + + self.prev_filtered_pos = pos + + filtered_pose = PoseStamped() + filtered_pose.header = pose.header + filtered_pose.pose.position.x, filtered_pose.pose.position.y, filtered_pose.pose.position.z = pos + # filtered_pose.pose.orientation.x, filtered_pose.pose.orientation.y, filtered_pose.pose.orientation.z, filtered_pose.pose.orientation.w = ori + # smoothed_ori = slerp(last_ori, new_ori, alpha) + target_pos = [ + pos.x, + pos.y, + pos.z + ] + + target_pos[0] = np.clip(target_pos[0], 0, 0.8) + target_pos[1] = np.clip(target_pos[1], -0.2, 0.2) + 0.15 + target_pos[2] = np.clip(target_pos[2], -0.3, 0.3) + 0.3 + # pose.pose.position.x = 0.1 + # pose.pose.position.y = 0.25 + # pose.pose.position.z = 0.4 + pose.pose.orientation.x = 1.0 + pose.pose.orientation.y = 0.0 + pose.pose.orientation.z = 0.0 + pose.pose.orientation.w = 0.0 + + + + req.ik_request.pose_stamped.pose.position.x = target_pos[0] + req.ik_request.pose_stamped.pose.position.y = target_pos[1] + req.ik_request.pose_stamped.pose.position.z = target_pos[2] + # req.ik_request.pose_stamped.pose.position = pose.pose.position + req.ik_request.pose_stamped.pose.orientation = pose.pose.orientation + 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.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 + self.get_logger().info(f"Seed joints: {seed_state.position}") + # self.get_logger().info(f"joint names: {seed_state.name}") + self.get_logger().info(f"Target pose: {pose.pose.position}, {pose.pose.orientation}") + + future = self.ik_client.call_async(req) + future.add_done_callback(self.on_ik_result) + + def on_ik_result(self, future): + res = future.result() + self.get_logger().info(f"IK result code: {res.error_code.val}") + 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)] + point.time_from_start.sec = 0 + point.time_from_start.nanosec = 300_000_00 # 0.3s + + jt.points.append(point) + self.traj_pub.publish(jt) + +def main(): + rclpy.init() + node = ArucoIKController() + rclpy.spin(node) + +if __name__ == "__main__": + main() diff --git a/src/openarm_moveit_control/setup.py b/src/openarm_moveit_control/setup.py index 53486a1..838cade 100644 --- a/src/openarm_moveit_control/setup.py +++ b/src/openarm_moveit_control/setup.py @@ -29,6 +29,7 @@ setup( 'tf_test = openarm_moveit_control.tf_test:main', 'hand_follow_moveit = openarm_moveit_control.hand_follow_moveit:main', 'follow_pose_goal = openarm_moveit_control.follow_pose_goal:main', + 'aruco_ik_controller = openarm_moveit_control.aruco_ik_controller:main', ], }, )