cmvr-es/python/vision_servo/follow_target.py

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