C implemented RNE algorithm gives incorrect results

[WillemStev]

Dear

For my thesis, I need to perform a simulation experiment where I determine payload parameters using linear regression.

In order to execute the experiment correctly, the 2 equations (1) and (2) below must be satisfied.

• Y is the regression matrix used to determine the payload parameters

• theta is the payload parameter vector consisting of payload mass, offset center of mass and inertias. All parameters expressed in the local frame of reference of the last link

• RNE_payload and RNE_nopayload represent the output of the RNE algorithm of the robot with and without payload respectively

• M, C and g are inertia matrix, coriolis matrix and gravitational load vector respectively

• q is the position of the robot in joint space

I calculate the regression matrix Y using the RNE algorithm. In the code below you can see how it works.

For example, to determine the first column of Y, I create a robot object with all parameters zero except for a payload with mass m_p = 1. So a weightless robot with a points mass of 1kg attached to the end effector. When I calculate the RNE algorithm for this robot object, I then obtain the first column of the regression matrix Y.

Equation (1) and (2) only hold simultaneously when working with the rne_python function for the Panda robot. But the Python implementation of the RNE algorithm is way too slow for my purposes so I can't use that.

For the C implemented RNE algorithm and also for the Python implementation used in other robot types, only 1 of the 2 equations simultaneously hold.

My thesis mentor told me that this problem doesn't exist in the robotics toolbox in MATLAB. I hope you can have a look at it and fix the C implementation of the RNE algorithm for the Python toolbox.

As a small side note, I want to mention that I changed line 1640 in the rne_python function in DHRobot.py. This is because joint velocities went to infinity when I wanted to control the position of the robot with a payload attached to it.

Instead of

vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd # line 1640 in DHRobot.py

I changed it to

vd_ = Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd) # line 1640 in DHRobot.py

Below I provide the code I used to construct the regression matrix and the verification of the toolbox. The verification output is also given.

The code

import roboticstoolbox as rtb
import numpy as np

# I have implemented the function robot.payload(mp, r_offset, Ip_cm) in Dynamics.py to add the payload to the robot
'''
def payload(self: RobotProto, mp: float, r_offset=np.zeros(3), I_payload_reference_cm_payload=np.zeros((3, 3))):
    lastlink = self.links[self.n - 1]
    lastlink.r = (lastlink.r * lastlink.m + r_offset * mp)/(lastlink.m + mp)
    lastlink.m += mp
    lastlink.I += mp*(np.dot(r_offset, r_offset) * np.eye(3) - np.outer(r_offset, r_offset)) # --> Steiner theorem; equivalent to expression Ip_n = Ip_cm + mp * transpose(S(r_offset)) @ S(r_offset) with S(r_offset) the skew symmetric matrix of r_offset
    lastlink.I += I_payload_reference_cm_payload
    return
'''

### Calculation of regression matrix 
### ---------------------------------

def params2zero(robot):
    for i in range(7):
        robot.links[i].m = 0
        robot.links[i].I -= robot.links[i].I
        robot.links[i].r = np.zeros(3)
    return

# robot with mp = 1 (all other params 0)
panda_mp = rtb.models.DH.Panda()
params2zero(panda_mp)
panda_mp.links[-1].m = 1

# robot with mp = 1 & rp_i = 1 (all other params 0); i = x, y, z
panda_mpri = np.array([rtb.models.DH.Panda(), rtb.models.DH.Panda(), rtb.models.DH.Panda()])
for i in range(3):
    params2zero(panda_mpri[i])
    panda_mpri[i].links[-1].m = 1
    panda_mpri[i].links[-1].r[i] = 1

panda_mprx, panda_mpry, panda_mprz = panda_mpri[0], panda_mpri[1], panda_mpri[2]

# robot with Ip_ij = Ip_ji = 1 (all other params 0); i, j = x, y, z
panda_Iij = np.array([rtb.models.DH.Panda(), rtb.models.DH.Panda(), rtb.models.DH.Panda(), rtb.models.DH.Panda(), rtb.models.DH.Panda(), rtb.models.DH.Panda()])
index = 0
for i in range(3):
    for j in range(i, 3):
        params2zero(panda_Iij[index])
        matrix = np.zeros((3, 3))
        matrix[i, j] = 1
        matrix[j, i] = 1
        panda_Iij[index].links[-1].I += matrix
        index += 1

panda_Ixx, panda_Ixy, panda_Ixz, panda_Iyy, panda_Iyz, panda_Izz = panda_Iij[0], panda_Iij[1], panda_Iij[2], panda_Iij[3], panda_Iij[4], panda_Iij[5]

# regression matrix using RNE in C code
def regression_matrix_toolbox_C(q, qd, qdd):
    Y_rtb = np.zeros((7, 10))

    Y_rtb[:, 0] = panda_mp.rne(q, qd, qdd)                      # mp = 1, rest = 0              ==> torque == 1st column in regression matrix
    Y_rtb[:, 1] = panda_mprx.rne(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, rx = 1, rest = 0      ==> torque == Sum 1st & 2nd column in regression matrix
    Y_rtb[:, 2] = panda_mpry.rne(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, ry = 1, rest = 0      ==> torque == Sum 1st & 3rd column in regression matrix
    Y_rtb[:, 3] = panda_mprz.rne(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, rz = 1, rest = 0      ==> torque == Sum 1st & 4th column in regression matrix
    Y_rtb[:, 4] = panda_Ixx.rne(q, qd, qdd)                     # Ixx = 1, rest = 0             ==> torque == 5th column in regression matrix
    Y_rtb[:, 5] = panda_Iyy.rne(q, qd, qdd)                     # Iyy = 1, rest = 0             ==> torque == 6th column in regression matrix
    Y_rtb[:, 6] = panda_Izz.rne(q, qd, qdd)                     # Izz = 1, rest = 0             ==> torque == 7th column in regression matrix
    Y_rtb[:, 7] = panda_Ixy.rne(q, qd, qdd)                     # Ixy = 1, rest = 0             ==> torque == 8th column in regression matrix
    Y_rtb[:, 8] = panda_Ixz.rne(q, qd, qdd)                     # Ixz = 1, rest = 0             ==> torque == 9th column in regression matrix
    Y_rtb[:, 9] = panda_Iyz.rne(q, qd, qdd)                     # Iyz = 1, rest = 0             ==> torque == 10th column in regression matrix

    return Y_rtb

# regression matrix using RNE in Python code
def regression_matrix_toolbox_Python(q, qd, qdd):
    Y_rtb = np.zeros((7, 10))

    Y_rtb[:, 0] = panda_mp.rne_python(q, qd, qdd)                      # mp = 1, rest = 0              ==> torque == 1st column in regression matrix
    Y_rtb[:, 1] = panda_mprx.rne_python(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, rx = 1, rest = 0      ==> torque == Sum 1st & 2nd column in regression matrix
    Y_rtb[:, 2] = panda_mpry.rne_python(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, ry = 1, rest = 0      ==> torque == Sum 1st & 3rd column in regression matrix
    Y_rtb[:, 3] = panda_mprz.rne_python(q, qd, qdd) - Y_rtb[:, 0]      # mp = 1, rz = 1, rest = 0      ==> torque == Sum 1st & 4th column in regression matrix
    Y_rtb[:, 4] = panda_Ixx.rne_python(q, qd, qdd)                     # Ixx = 1, rest = 0             ==> torque == 5th column in regression matrix
    Y_rtb[:, 5] = panda_Iyy.rne_python(q, qd, qdd)                     # Iyy = 1, rest = 0             ==> torque == 6th column in regression matrix
    Y_rtb[:, 6] = panda_Izz.rne_python(q, qd, qdd)                     # Izz = 1, rest = 0             ==> torque == 7th column in regression matrix
    Y_rtb[:, 7] = panda_Ixy.rne_python(q, qd, qdd)                     # Ixy = 1, rest = 0             ==> torque == 8th column in regression matrix
    Y_rtb[:, 8] = panda_Ixz.rne_python(q, qd, qdd)                     # Ixz = 1, rest = 0             ==> torque == 9th column in regression matrix
    Y_rtb[:, 9] = panda_Iyz.rne_python(q, qd, qdd)                     # Iyz = 1, rest = 0             ==> torque == 10th column in regression matrix

    return Y_rtb

# verify that RNE_pay - (Y @ theta + RNE_nopay) = 0 --> for C & Python implemented RNE
# Payload data
mp = 5

rx = 0.1
ry = 0.1
rz = 0.2
r_offset = np.array([rx, ry, rz])

a, b, h = 0.07, 0.07, 0.2 # --> rectangular cuboid shape with dims 7cm x 7cm x 20cm

# inertias wrt reference frame in payload centre of mass
Ixx_cm, Iyy_cm, Izz_cm, Ixy_cm, Ixz_cm, Iyz_cm = (1/12) * mp * (b**2 + h**2), (1/12) * mp * (a**2 + h**2), (1/12) * mp * (a**2 + b**2), 0, 0, 0

Ip_cm = np.array([[Ixx_cm, Ixy_cm, Ixz_cm], [Ixy_cm, Iyy_cm, Iyz_cm], [Ixz_cm, Iyz_cm, Izz_cm]])

# inertia tensor wrt to reference frame in end effector (without payload) centre of mass
Ip_n = Ip_cm + mp*(np.dot(r_offset, r_offset) * np.eye(3) - np.outer(r_offset, r_offset)) # --> Steiner theorem

Ixx_n, Iyy_n, Izz_n, Ixy_n, Ixz_n, Iyz_n = Ip_n[0, 0], Ip_n[1, 1], Ip_n[2, 2], Ip_n[0, 1], Ip_n[0, 2], Ip_n[1, 2]

# true payload parameter vector theta
theta = np.array([mp, mp*rx, mp*ry, mp*rz, Ixx_n, Iyy_n, Izz_n, Ixy_n, Ixz_n, Iyz_n])


q = np.ones(7)
qd = np.ones(7)
qdd = np.ones(7)

### verification of toolbox with RNE using C
panda = rtb.models.DH.Panda()

rne_nopay = panda.rne(q, qd, qdd)
panda.payload(mp, r_offset, Ip_cm)
rne_pay = panda.rne(q, qd, qdd)

# verify equation 1
Y = regression_matrix_toolbox_C(q, qd, qdd)

print(f"C implemented RNE: Y @ theta + RNE_nopay - RNE_pay = {Y @ theta + rne_nopay - rne_pay}")

# verify equation 2
print(f"\n C implemented RNE: RNE - (M @ qdd + C @ qd + g) = {panda.rne(q, qd, qdd) - panda.inertia(q) @ qdd - panda.coriolis(q, qd) @ qd - panda.gravload(q)}")

### verification of toolbox with RNE using Python
panda_python = rtb.models.DH.Panda()

rne_nopay_python = panda_python.rne_python(q, qd, qdd)
panda_python.payload(mp, r_offset, Ip_cm)
rne_pay_python = panda_python.rne_python(q, qd, qdd)

# verify equation 1
Y_python = regression_matrix_toolbox_Python(q, qd, qdd)

print("\n-------------------------------------------------------")
print(f"Python implemented RNE: Y @ theta + RNE_nopay - RNE_pay = {Y_python @ theta + rne_nopay_python - rne_pay_python}")

# verify equation 2
print(f"\n Python implemented RNE: RNE - (M @ qdd + C @ qd + g) = {panda_python.rne_python(q, qd, qdd) - panda_python.mass_matrix(q) @ qdd - panda_python.damping_term(q, qd)}")

# mass_matrix, damping_term are functions I implemented in Dynamics.py to calculate the inertia and damping term respectively using the rne_python implementation
'''
def mass_matrix(self: RobotProto, q):
    M = np.zeros((self.n, self.n))
    for i in range(self.n):
        qd = np.zeros(self.n)
        qdd = np.zeros(self.n)
        qdd[i] = 1
        M[:, i] = self.rne_python(q, qd, qdd, gravity=np.zeros(3)) # zero gravity is required
    return M

def damping_term(self: RobotProto, q, qd):
    C_qd_plus_gravload = np.zeros(self.n)
    C_qd_plus_gravload = self.rne_python(q, qd, np.zeros(self.n))
    return C_qd_plus_gravload
'''

The printed output:
All output vectors should be equal to zero, otherwise an equation is not satisfied. Here both equations (1) and (2) are satisfied for the python implemented RNE algorithm for the Panda robot but this is not the case for other robot types as the UR5 for example.

C implemented RNE: Y @ theta + RNE_nopay - RNE_pay = [ 1.02928212  4.0434679   2.71876803 -1.74502051 -1.63167266  2.65752592
  4.06248439]

 C implemented RNE: RNE - (M @ qdd + C @ qd + g) = [ 7.31977868e-15 -7.10542736e-15 -3.10862447e-15 -1.99840144e-15
  8.88178420e-16 -3.55271368e-15  8.88178420e-16]

-------------------------------------------------------
Python implemented RNE: Y @ theta + RNE_nopay - RNE_pay = [ 3.10862447e-15 -7.10542736e-15 -1.77635684e-15  1.77635684e-15
  8.88178420e-16  1.77635684e-15 -1.11022302e-16]

 Python implemented RNE: RNE - (M @ qdd + C @ qd + g) = [-1.33226763e-15  7.10542736e-15  3.55271368e-15  1.77635684e-15
 -8.88178420e-16  0.00000000e+00 -5.55111512e-17]