Implementation of get_forward_kinematics()

Technical Questions
1

I am currently trying to implement a local Python version of the get_forward_kinematics() method following the formula used in this document:
https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/

I already downloaded the calibration delta values from my robot and so far I am getting the correct values for x,y,z as long as I use [0,0,0,0,0,0] as the tcp offset. My problem is that the orientation angles I am seeing are entirely different than the ones returned by the native call and my x,y,z values are also very different once I use a tcp offset different from 0.

I suspect that the beta/alpha/gamma values returned in that document/excel sheet above are euler angles and not axis-angle values, but if that is the case I have not managed to find the correct conversion function. Any pointers how I have to convert them?

The other part where I am quite clueless is how to factor the tcp offset into the result - is that something I can add/transform after getting the forward kinematics values without the offset or do I have to already add them to the Denavit–Hartenberg parameters?

And I guess there is no hope that you could share the native algorithm used in get_forward_kinematics()?

2

After some more experimentation I found the working solution - I’ll share it here in case someone else finds it useful (this is for a UR-10e, other models will need different values and might have more or less joints):

import numpy as np
from pytransform3d import rotations as pr

def forwardKinematics(theta,tcp=None):  
    # theta are the joint angles in radians
    # tcp is the tcp offset as a pose (x,y,z,rx,ry,rz)

    #values for UR-10e (https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/):
    a = np.array([0.0000,-0.6127,-0.57155,0.0000,0.0000,0.0000])
    d = np.array([0.1807,0.0000,0.0000,0.17415,0.11985,0.11655])
    alpha = np.array([np.pi/2,0.,0.,np.pi/2,-np.pi/2,0.])
    
    #values from calibration.conf:
    delta_a = np.array([ 3.1576640107943976e-05, 0.298634925475782076, 0.227031257526500829, -8.27068507303316573e-05, 3.6195435783833642e-05, 0])
    delta_d = np.array([ 5.82932048768247668e-05, 362.998939868892023, -614.839459588742898, 251.84113332747981, 0.000164511802564715204, -0.000899906496469232708])
    delta_alpha = np.array([ -0.000774756642435869836, 0.00144883356002286951, -0.00181081418698111852, 0.00068792563586761446, 0.000450856239573305118, 0])
    delta_theta = np.array([ 1.09391516130152855e-07, 1.03245736607748673, 6.17452995676434124, -0.92380698472218048, 6.42771759845617296e-07, -3.18941184192234051e-08])

    a += delta_a
    d += delta_d
    alpha+=delta_alpha
    theta=theta.copy()+delta_theta

    ot = np.eye(4)
    for i in range(6):
        ot = ot @ np.array([[np.cos(theta[i]), -(np.sin(theta[i]))*np.cos(alpha[i]), np.sin(theta[i])*np.sin(alpha[i]), a[i]*np.cos(theta[i])],[np.sin(theta[i]),np.cos(theta[i])*np.cos(alpha[i]),-(np.cos(theta[i]))*np.sin(alpha[i]),a[i]*np.sin(theta[i])], [0.0,np.sin(alpha[i]),np.cos(alpha[i]),d[i]],[0.0,0.0,0.0,1.0]])

    if not tcp is None:
        offset = np.array([ot[0,3],ot[1,3],ot[2,3]])  + (ot[:3,:3] @ tcp[:3])
        newAngle = pr.compact_axis_angle_from_matrix( ot[:3,:3] @ pr.matrix_from_compact_axis_angle(tcp[3:]))
        return np.array([offset[0],offset[1],offset[2],newAngle[0],newAngle[1],newAngle[2]])
    else:
        axisAngle = pr.compact_axis_angle_from_matrix(ot[:3,:3])
        return np.array([ot[0,3],ot[1,3],ot[2,3],axisAngle[0],axisAngle[1],axisAngle[2]])