import os os.environ["GLOG_minloglevel"] = "1" import sys from utils import root_path sys.path.append(os.path.join(root_path, "cmake-build-debug/example")) from robot_wrapper import Robot import time import signal import numpy as np from scipy.spatial.transform import Rotation as R from utils import solve_ik from utils import get_pose from target_position import init_realsense, get_closest_red_point def signal_handler(sig, frame): print("\n收到 Ctrl+C，准备退出...") raise SystemExit signal.signal(signal.SIGINT, signal_handler) def rt_to_transform(R, t): """拼接旋转矩阵和平移向量为齐次矩阵""" T = np.eye(4) T[:3, :3] = R T[:3, 3] = np.squeeze(t) return T def transform_point(T, P): """用齐次变换矩阵 T (4x4) 把点 P(3,) 转换到新坐标系""" P_h = np.append(P, 1) # [x, y, z, 1] P_new = T @ P_h return P_new[:3] def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam) T_base_cam = T_base_ee @ T_ee_cam P_base_target = transform_point(T_base_cam, P_cam_target) return P_base_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] EE_LINK = "FINGER" def main(): robot = Robot(os.path.join(root_path, "config/cabin_robot.xml"), "hc01") current_q = robot.getJointQ('right') diff = np.array(IDEL_Q) - np.array(current_q) if np.max(np.abs(diff)) >= 0.01: robot.moveJ("right", IDEL_Q) pipeline, align = init_realsense() ok, _, init_rq = solve_ik(INIT_POSE, base_link="PELVIS_S", target_link="R_FINGER_TIP") if not ok: print("solve ik init_pose failed, init_pose:", INIT_POSE) return print("init q:", init_rq) print("Robot movej: ", init_rq) robot.moveJ("right", init_rq) try: while True: current_q = robot.getJointQ('right') 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") P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") 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) ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_WRIST_R_S") elif EE_LINK == "CAM": desire_rpy_base_tool = R.from_matrix(T_base_cam[:3, :3]).as_euler('xyz', degrees=True) desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_CAM") elif EE_LINK == "FINGER": desire_rpy_base_tool = R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_FINGER_TIP") else: raise RuntimeError("undefined EE_LINK: ", EE_LINK) if not ok: print("solve ik failed, pose=", desire_pose) time.sleep(1) else: current_q = robot.getJointQ('right') diff = np.array(rq) - np.array(current_q) if np.max(diff) < 0.01: continue print("move position:", desire_P_base_tool) print("Robot movej: ", rq) 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) robot.moveJ("right", init_rq) print("Return to init pose, Robot movej: ", init_rq) except SystemExit: print("安全退出程序...") finally: robot.moveJ("right", IDEL_Q) pipeline.stop() 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