机械臂手眼标定

机械臂手眼标定程序，适用于手在眼上的情况，需要收集多个时刻的“手”变换矩阵和“眼”变换矩阵，其中“手”变换矩阵是从机械臂底座坐标系到末端执行器坐标系的变换矩阵，“眼”变换矩阵是从相机坐标系到物体坐标系的变换矩阵。

import numpy as np

def calc_HandEye(Mat_Hand:list, Mat_Eye:list) -> np.ndarray:
    """Calculate the transformation matrix of an eye-in-hand system

    Reference
    ----------
    Park, Frank C., and Bryan J. Martin. “Robot sensor calibration: solving AX=XB on the Euclidean group.” IEEE Transactions on Robotics and Automation10.5 (1994): 717-721.

    Parameters
    ----------
    Mat_Hand : list
        Each entry in the list is a 4x4 transformation matrix from the arm base to the end point of the arm
    Mat_Eye : list
        Each entry in the list is a 4x4 transformation matrix from the camera coordinate to the fixed arucode coordinate

    Returns
    -------
    np.ndarray shape=(4,4)
        The transformation matrix from the end point of the arm to the camera
    """
    T = np.eye(4)
    A = []
    B = []

    # for mat in Mat_Hand:
    #     assert mat.shape==(4,4)
    # for mat in Mat_Eye:
    #     assert mat.shape==(4,4)

    assert len(Mat_Hand) == len(Mat_Eye)
    n = len(Mat_Hand)
    for i in range(n):
        for j in range(i+1, n):
            A.append(np.linalg.inv(Mat_Hand[i]) @ Mat_Hand[j])
            B.append(Mat_Eye[i] @ np.linalg.inv(Mat_Eye[j]))

    def Rodrigues(matrix):
        theta = np.arccos((np.trace(matrix) - 1.) / 2.0)
        w = (matrix - matrix.T) / (2. * np.sin(theta))
        return np.array([[-w[1,2], w[0,2], -w[0,1]]]).T * theta

    M = np.zeros((3,3))
    for a, b in zip(A, B):
        M += Rodrigues(b[:3,:3]) @ Rodrigues(a[:3,:3]).T

    MTM = M.T @ M
    v, Q = np.linalg.eig(MTM)
    R = (Q @ np.diag(1. / np.sqrt(v)) @ np.linalg.inv(Q)) @ M.T

    CTC = np.zeros((3,3))
    CTD = np.zeros((3,1))
    for a, b in zip(A, B):
        c = np.eye(3) - a[:3,:3]
        d = a[:3,3:] - (R @ b[:3,3:])
        CTC += c.T @ c
        CTD += c.T @ d
    
    T[:3,:3] = R
    T[:3,3:] = np.linalg.inv(CTC) @ CTD

    return T


""" A simple example, collecting data from simulator """
if __name__ == "__main__":
    Harr = np.array([[[0.7875422755624356, -0.2835834420570604, 0.547135810921759, 0.3738809367529083], [0.2336542614262599, 0.9589467145895108, 0.16070745065283232, -0.22704416639776515], [-0.5702480603380338, 0.0012767024138120031, 0.8214715574575037, 0.35515494166073525], [0.0, 0.0, 0.0, 1.0]], [[0.8760092626315509, 0.0005886073025513153, 0.4822939200582671, 0.4003143415378911], [-0.0003968467215800149, 0.9999997964440812, -0.0004996243343864821, -0.0006716298138358526], [-0.48229411596701677, 0.0002462787837455843, 0.876009317901559, 0.40045597088690016], [0.0, 0.0, 0.0, 1.0]], [[0.812511817202977, 0.2648625298396383, 0.5192999010133379, 0.3886741961338031], [-0.21521389879842906, 0.9641814685888549, -0.15503894153991915, 0.10691971486068724], [-0.5417633474770098, 0.014210415785107164, 0.8404109348489696, 0.34585304097164477], [0.0, 0.0, 0.0, 1.0]], [[0.8133777093995649, 0.30754522882344, 0.49379412114752325, 0.3792039417868621], [-0.09320236434087553, 0.9067569001902831, -0.411224079108434, 0.23045441236213132], [-0.574221230131025, 0.28845771992658953, 0.7661997929271225, 0.391167013906341], [0.0, 0.0, 0.0, 1.0]], [[0.8265406565515646, -0.012933110893160293, 0.5627284226960048, 0.5785896588025545], [-0.0005970928965680911, 0.9997152903858513, 0.02385333621961208, 0.07686173472041458], [-0.5628767063464084, -0.020051753323792117, 0.8262976102115148, 0.30222361817173404], [0.0, 0.0, 0.0, 1.0]], [[0.7544802738788485, 0.5750563287388658, 0.31633800768335596, 0.5785198940220675], [0.20939384493244007, 0.24589213135390872, -0.9464096773821796, -0.09721341155051844], [-0.6220239014958401, 0.7802866643199854, 0.0651075068811866, 0.3020039733591908], [0.0, 0.0, 0.0, 1.0]], [[0.8955280762088912, -0.10431498067372014, 0.43260588244803555, 0.20884241664779346], [0.08997779704268982, 0.9945033489886662, 0.053545166818971646, -0.16122704328764728], [-0.4358135619286607, -0.00902627594129482, 0.8999917030626918, 0.45369259690738406], [0.0, 0.0, 0.0, 1.0]], [[0.9359127241093685, 0.000735847460572687, 0.3522312186315915, 0.10084455506614737], [-0.0007415328803440024, 0.9999997180103893, -0.00011877764604302325, -2.0229089250724567e-05], [-0.35223120670827646, -0.00015002551982753781, 0.9359130058467906, 0.4059863804552266], [0.0, 0.0, 0.0, 1.0]], [[0.8987146119893002, 0.000728805705560391, 0.43853336821633654, 0.19868211371522704], [-0.0006548435648657985, 0.9999997344210318, -0.00031990275281323715, -0.00024823719245689856], [-0.43853348489804866, 3.305242134659216e-07, 0.8987148505633271, 0.40556237708982434], [0.0, 0.0, 0.0, 1.0]], [[0.8502532744495317, 0.0007794227442502731, 0.5263732153026359, 0.2976759128018616], [-0.0005773089271618507, 0.9999996830893918, -0.0005482111991369455, -0.00048422206731948286], [-0.5263734757776575, 0.0001622384109429935, 0.8502534549629576, 0.4061095933245188], [0.0, 0.0, 0.0, 1.0]], [[0.7993462309532966, 0.0008239600062060601, 0.6008701391737372, 0.39860591797590145], [-0.0004775356415470328, 0.9999996151281663, -0.0007360049119410531, -0.0007288440693011776], [-0.600870514354357, 0.0003013858449264085, 0.7993463167777172, 0.4088242841319234], [0.0, 0.0, 0.0, 1.0]]])
    Earr = np.array([[[-0.932501558805554, -0.339571995784672, 0.12301098529817192, 0.09499064832925797], [-0.07261011285450378, 0.5099044701443108, 0.8571611300322164, -0.11377467215061188], [-0.35379176691447844, 0.7903722483777278, -0.5001430741876439, 0.7626067399978638], [0.0, 0.0, 0.0, 1.0]], [[-0.9929310590811973, -0.08803824999345017, 0.07960639704183964, 0.03368014097213745], [0.02158139706665127, 0.5256011441161204, 0.8504573361459568, -0.18738281726837158], [-0.11671398893253448, 0.8461635207463839, -0.5199857122513465, 0.6840453743934631], [0.0, 0.0, 0.0, 1.0]], [[-0.9802807843970361, 0.19343391402729232, 0.04040921486509815, 0.08661741018295288], [0.14593318973442804, 0.5707521269014445, 0.8080504401158464, -0.06506818532943726], [0.1332407140324121, 0.7980133648890597, -0.5877257707320426, 0.6749086380004883], [0.0, 0.0, 0.0, 1.0]], [[-0.9226855859475313, 0.2677490535099253, 0.27741981513439207, -0.1267876923084259], [0.38309915612675016, 0.5556293659995465, 0.7379099160560956, -0.0028244939167052507], [0.043432085600774356, 0.7871381403435367, -0.6152456435903354, 0.724372923374176], [0.0, 0.0, 0.0, 1.0]], [[-0.9951793190255825, -0.08597165225205988, 0.047191079589568904, -0.01112071331590414], [-0.015366276472331998, 0.6119341641462849, 0.7907594174576511, -0.13423371315002441], [-0.09686072749651427, 0.7862222674025703, -0.6103052889408772, 0.48312878608703613], [0.0, 0.0, 0.0, 1.0]], [[-0.20059109018015453, 0.5947602598927271, 0.7784750784660182, -0.11262881010770798], [0.941190084905775, 0.3375284023554008, -0.015355835323115352, -0.007183868903666735], [-0.27189049011577643, 0.7296127814503708, -0.6274876497022523, 0.5313702821731567], [0.0, 0.0, 0.0, 1.0]], [[-0.9769572940802238, -0.1766133106066941, 0.11984233000067508, 0.11067309230566025], [0.04998744307041972, 0.356542776064342, 0.932940782885824, -0.309842586517334], [-0.20749867729392607, 0.9174339144335553, -0.33949861791824015, 0.8668667674064636], [0.0, 0.0, 0.0, 1.0]], [[-0.9941758645126204, -0.09503578451990317, 0.05081879653504785, 0.005017117131501436], [0.011375865868982912, 0.3763724473144332, 0.9263986024267715, -0.1848912090063095], [-0.10716781278124028, 0.9215812392639245, -0.37309928885010524, 0.9447261691093445], [0.0, 0.0, 0.0, 1.0]], [[-0.993929992724257, -0.09059346867844115, 0.06241789003136064, 0.01432306133210659], [0.013312704035295699, 0.46414967322677325, 0.8856567352844714, -0.13858845829963684], [-0.10920595896940491, 0.8811117433539789, -0.4601262372972059, 0.8718011379241943], [0.0, 0.0, 0.0, 1.0]], [[-0.9938204878144046, -0.08326456663799908, 0.07340197506147403, 0.022652138024568558], [0.015393762426652781, 0.5515046502326457, 0.8340297673645222, -0.09906473010778427], [-0.10992665772546176, 0.8300058028196872, -0.546814865568737, 0.7958475351333618], [0.0, 0.0, 0.0, 1.0]], [[-0.9933238579945858, -0.07764446590227378, 0.08531734907686042, 0.03020879067480564], [0.01803086753862388, 0.6259912583288147, 0.7796215955909072, -0.09040360152721405], [-0.11394121710143706, 0.775955076928206, -0.6204120547142724, 0.7224215269088745], [0.0, 0.0, 0.0, 1.0]]])
    HE = calc_HandEye(Harr, Earr)    
    print(HE)

    # Reference :
    # [[-0.01276  0.      -0.99992 -0.11025]
    #  [-0.99992  0.       0.01276 -0.01859]
    #  [-0.       1.       0.       0.07   ]
    #  [ 0.       0.       0.       1.     ]]