MainCode.py 文件源码

def get_xyz(interface, xyz_from_camera):
    angles = interface.current_status.angles[0:3]

    # Get current XYZ
    P0t = DobotModel.forward_kinematics(angles)

    # Getting Desired XYZ of end effector
    Pct = np.array(CAMERA_OFFSET)
    R0t = DobotModel.R0T(angles)
    Rtc = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
    R0c = np.matmul(R0t, Rtc)

    Pta = np.matmul(R0c, xyz_from_camera) - np.matmul(R0c, Pct)
    target = np.reshape(Pta, (3, 1)) + np.reshape(P0t, (3, 1))
    return target


# FUNCTION: Touch - Place the end effector on top of an AR tag
# AR TAGS: DUCKY = 0  DUCKYBOT = 1   OBSTACLE = 2