diff --git a/config/robot_description/hc_description/dual_arm.urdf b/config/robot_description/hc_description/dual_arm.urdf index 346e7fda..92c2d344 100644 --- a/config/robot_description/hc_description/dual_arm.urdf +++ b/config/robot_description/hc_description/dual_arm.urdf @@ -574,7 +574,7 @@ - + diff --git a/python/math/rpy2rotation.py b/python/math/rpy2rotation.py index 075d3848..59ff041e 100644 --- a/python/math/rpy2rotation.py +++ b/python/math/rpy2rotation.py @@ -72,8 +72,8 @@ if __name__ == "__main__": # print("rpy back:", rpy_back) # print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back)))) R = np.array([ - [0, -1, 0], - [1, 0, 0], - [0, 0, 1] + [0, 0, 1], + [-1, 0, 0], + [0, -1, 0] ]) print(matrix_to_rpy(R)) \ No newline at end of file diff --git a/python/vision_servo/follow_target.py b/python/vision_servo/follow_target.py index e66f4e1d..1e908869 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -43,9 +43,12 @@ def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): IDEL_Q = [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, -1.498103103566999, 0.039690803641003906, -0.08653132466712826] INIT_POSE = [0.45, -0.25, -0.15, 0, 0, 0] + # FOLLOW_OFFSET = [-0.3, 0, 0] # EE_LINK = "EE" -FOLLOW_OFFSET = [-0.05, 0, 0] +# FOLLOW_OFFSET = [-0.2, 0, 0] +# EE_LINK = "CAM" +FOLLOW_OFFSET = [-0.01, 0, 0] EE_LINK = "FINGER" def main(): @@ -69,13 +72,19 @@ def main(): T_base_ee = get_pose(current_q, base_link="PELVIS_S", target_link="R_WRIST_R_S") T_base_cam = get_pose(current_q, base_link="PELVIS_S", target_link="R_CAM") T_base_tool = get_pose(current_q, base_link="PELVIS_S", target_link="R_FINGER_TIP") + print(T_base_cam) P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") + if P_cam_target is None: + print("can not find red point") + time.sleep(2) + continue + T_cam_target = np.eye(4) T_cam_target[:3, 3] = P_cam_target T_base_target = T_base_cam @ T_cam_target P_base_target = T_base_target[:3, 3] - desire_P_base_tool = P_base_target + FOLLOW_OFFSET + if EE_LINK == "EE": desire_rpy_base_tool = R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) @@ -104,7 +113,7 @@ def main(): input_data = input("move robot? type \"yes\" to move, or type \"no\" to skip \n") if input_data == "yes": robot.moveJ("right", rq) - time.sleep(10) + time.sleep(5) robot.moveJ("right", init_rq) print("Return to init pose, Robot movej: ", init_rq) @@ -118,85 +127,4 @@ def main(): if __name__ == '__main__': - main() - - -# if __name__ == "__main__": -# pipeline, align = init_realsense() -# try: -# # 初始化机器人 -# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") -# -# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) -# -# robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) -# js = robot.getJointQ('right') -# print("js = ") -# print(js) -# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") -# # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") -# -# # P_base_cam = T_base_cam[:3, 3] -# -# print("T_base_ee =\n", T_base_ee) -# # print("T_ee_cam =\n", T_ee_cam) -# # print("T_base_cam =\n", T_base_cam) -# -# -# P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") -# # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") -# print("P_cam_target (camera frame) =", P_cam_target) -# -# # T_cam_target = np.eye(4) -# # T_cam_target[:3, 3] = P_cam_target -# # T_base_target = T_base_cam @ T_cam_target -# # P_base_target = T_base_target[:3, 3] -# -# # R_ee_cam = np.array([ -# # [-1, 0, 0], -# # [ 0, 0, -1], -# # [ 0, -1, 0] -# # ]) -# # t_ee_cam = np.array([-0.01212, -0.17655, 0.07506]) -# R_ee_cam = np.array( [[-0.99732744 , 0.02734315 , 0.0677519 ], -# [-0.06713675 ,0.0228221 , -0.99748274], -# [-0.02882056,-0.99936555, -0.02092538]]) -# t_ee_cam = np.array([-0.00078482, -0.17419722, 0.06796275]) -# # t_ee_cam = np.array([-0.05212, -0.17419722, 0.06796275]) -# -# # R_ee_cam = np.array( [[-0.99486349, 0.07846371 , 0.06395369], -# # [-0.05884529 ,0.06577685, -0.9960977 ], -# # [-0.08236419 ,-0.99474462 ,-0.06082177]]) -# # -# # t_ee_cam = np.array([-0.00797824, -0.17640328, 0.07459845]) -# P_base_target = compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target) -# -# print("P_base_target (base frame) =", P_base_target) -# -# # 位置：直接用 P_base_target -# target_x, target_y, target_z = P_base_target - np.array([0.45, 0, 0]) -# target_rx, target_ry, target_rz =R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) -# -# # 调 IK -# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) -# -# -# print("rq =", rq) -# -# if ok: -# robot.moveJ("right", rq) -# else: -# print("solve ik failed") -# -# -# print("已到达目标点，按 Ctrl+C 回到初始位姿...") -# while True: -# time.sleep(1) -# except SystemExit: -# print("安全退出程序...") -# -# finally: -# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) -# pipeline.stop() -# align = None -# depth_intr = None \ No newline at end of file + main() \ No newline at end of file diff --git a/python/vision_servo/red_point.png b/python/vision_servo/red_point.png index 81de0437..c85c560f 100644 Binary files a/python/vision_servo/red_point.png and b/python/vision_servo/red_point.png differ diff --git a/python/vision_servo/target_position.py b/python/vision_servo/target_position.py index 8e7e652e..9eecc2d1 100644 --- a/python/vision_servo/target_position.py +++ b/python/vision_servo/target_position.py @@ -21,51 +21,55 @@ def get_closest_red_point(pipeline, align, vis_path = None,warmup=10): 返回 (x, y, z) 或 None（如果没检测到）。 """ # 丢掉前几帧，让相机稳定 - for _ in range(warmup): - pipeline.wait_for_frames() + try: + for _ in range(warmup): + pipeline.wait_for_frames() - frames = pipeline.wait_for_frames() - aligned_frames = align.process(frames) + frames = pipeline.wait_for_frames() + aligned_frames = align.process(frames) - depth_frame = aligned_frames.get_depth_frame() - color_frame = aligned_frames.get_color_frame() - if not depth_frame or not color_frame: + depth_frame = aligned_frames.get_depth_frame() + color_frame = aligned_frames.get_color_frame() + if not depth_frame or not color_frame: + return None + + depth_image = np.asanyarray(depth_frame.get_data()) + color_image = np.asanyarray(color_frame.get_data()) + depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics + + hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV) + lower_red1 = np.array([0, 100, 100]) + upper_red1 = np.array([10, 255, 255]) + lower_red2 = np.array([160, 100, 100]) + upper_red2 = np.array([179, 255, 255]) + mask1 = cv2.inRange(hsv, lower_red1, upper_red1) + mask2 = cv2.inRange(hsv, lower_red2, upper_red2) + mask = cv2.bitwise_or(mask1, mask2) + + mask_blur = cv2.GaussianBlur(mask, (9, 9), 2) + circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20, + param1=50, param2=15, minRadius=5, maxRadius=50) + + closest_point = None + min_depth = float('inf') + + if circles is not None: + circles = np.uint16(np.around(circles)) + for i in circles[0, :]: + u, v, r = i + depth = depth_frame.get_distance(u, v) + if 0 < depth < min_depth: + min_depth = depth + closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [u, v], depth) + + if vis_path is not None and closest_point is not None: + cv2.circle(color_image, (u, v), 6, (0,0,255), -1) + cv2.imwrite(vis_path, color_image) + return closest_point + except Exception as e: + print(e) return None - depth_image = np.asanyarray(depth_frame.get_data()) - color_image = np.asanyarray(color_frame.get_data()) - depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics - - hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV) - lower_red1 = np.array([0, 100, 100]) - upper_red1 = np.array([10, 255, 255]) - lower_red2 = np.array([160, 100, 100]) - upper_red2 = np.array([179, 255, 255]) - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - mask = cv2.bitwise_or(mask1, mask2) - - mask_blur = cv2.GaussianBlur(mask, (9, 9), 2) - circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20, - param1=50, param2=15, minRadius=5, maxRadius=50) - - closest_point = None - min_depth = float('inf') - - if circles is not None: - circles = np.uint16(np.around(circles)) - for i in circles[0, :]: - u, v, r = i - depth = depth_frame.get_distance(u, v) - if 0 < depth < min_depth: - min_depth = depth - closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [u, v], depth) - - if vis_path is not None and closest_point is not None: - cv2.circle(color_image, (u, v), 6, (0,0,255), -1) - cv2.imwrite(vis_path, color_image) - return closest_point - def black_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=10, avg_window=3): """