getForwardKinematics(actual_q) not providing the same answer as getActualTCPPose()

Technical Questions Report a Bug
1

Steps to reproduce:

import rtde_control
import rtde_receive
HOST = “127.0.0.1”
control = rtde_control.RTDEControlInterface(HOST)
receive = rtde_receive.RTDEReceiveInterface(HOST)

ZERO_COORD = receive.getActualTCPPose()
ZERO_Q = receive.getActualQ()

ZERO_COORD
[-0.5688240883583726, 0.1385389418035598, 0.38379092026207345, 2.0629827979244912, 2.3351493370908547, -0.010060181623337425]
ZERO_Q
[-0.5432213908580206, 0.394379499143662, -2.184086442468014, 0.24301507990286897, -1.559838562883069, -0.6669604472629826]

test_coord = control.getForwardKinematics(ZERO_Q)
test_q = control.getInverseKinematics(ZERO_COORD)

test_coord
[0.6910947428839915, 1.3530535579213854, 0.3761237227919115, 2.0629827979244912, 2.3351493370908547, -0.010060181623337425]
test_q
[-0.5432213908580206, 0.3943794991436613, -2.184086442468013, 0.24301507990287152, -1.5598385628830682, -0.6669604472629818]

Expected Behavior:
I am expecting that getActualTCPPose and getForwardKinematics(ZERO_Q), where ZERO_Q is the actual joint position (obtained from getActualQ()) both return the same (or very similar) position in the cartesian space (tool space)… because they are the same position

Actual Behavior:
The x, y, z coordinates differ in the getForwardKinematics() calculation. (I have to assume that getActualTCPPose() is giving the right values and it is getForwardKinematics() that is wrong. Here are the differences:

getActualTCPPose() gives [-0.5688240883583726, 0.1385389418035598, 0.38379092026207345, 2.0629827979244912, 2.3351493370908547, -0.010060181623337425]

getForwardKinematics(ZERO_COORD) gives [0.6910947428839915, 1.3530535579213854, 0.3761237227919115, 2.0629827979244912, 2.3351493370908547, -0.010060181623337425]

Affected versions:
Using Linux-based offline simulator version 5.12, on Ubuntu 20.04, running in a conda virtual environment using Python 3.8, with ur_rtde package version 1.5.2

2

Did you find a solution to this? I’m experiencing the same issue.

3

Well I figured out my issue, maybe it can help others. I could call getForwardKinematics and get the correct result, then call getInverseKinematics and get the correct result, then call getForwardKinematics again but this time get the wrong result and it will not be fixed until you reboot. It appears that getInverseKinematics screws up the known position somehow. If instead you getTCPOffset and getForwardKinematics(q_rad, my_tcp_offset), I am continuously getting the correct result.