update urdf

config/robot_description/hc_description/dual_arm.urdf

@@ -574,7 +574,7 @@
   </link>
 
   <joint name="R_FINGER_TIP_FIXED" type="fixed">
-    <origin xyz="-0.021 -0.2851 -0.01" rpy="0 0 -1.5707963267"/>
+    <origin xyz="-0.002 -0.2851 -0.005" rpy="0 0 -1.5707963267"/>
     <parent link="R_WRIST_R_S"/>
     <child link="R_FINGER_TIP"/>
     <axis xyz="0 0 1"/>

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],
+        [0, 0, 1],
-        [1, 0, 0],
+        [-1, 0, 0],
-        [0, 0, 1]
+        [0, -1, 0]
     ])
     print(matrix_to_rpy(R))

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()
+    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

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):
+    try:
-        pipeline.wait_for_frames()
+        for _ in range(warmup):
+            pipeline.wait_for_frames()
 
-    frames = pipeline.wait_for_frames()
+        frames = pipeline.wait_for_frames()
-    aligned_frames = align.process(frames)
+        aligned_frames = align.process(frames)
 
-    depth_frame = aligned_frames.get_depth_frame()
+        depth_frame = aligned_frames.get_depth_frame()
-    color_frame = aligned_frames.get_color_frame()
+        color_frame = aligned_frames.get_color_frame()
-    if not depth_frame or not 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):
     """