Estimate Pose of Moving Camera Mounted on a Robot: How to get the joint angles information for a UR5

Good afternoon,

I would like to find the transformation between a camera mounted on a UR5 and the TCP of this robot.

I'm using the code from the MATLAB example Estimate Pose of Moving Camera Mounted on a Robot but I'm having a problem with the values for the robot's joint angles. I used the values I obtained from the UR5 register ‘actual_q’. I read this register via an RTDE connection.

When I used the information found in this register with the images taken by my camera, I obtained the following result:

I did some research and discovered that the error was due to the joint robot's joint angles values I had been given in the code.

In the ‘Real-Time Data Exchange (RTDE) Guide’ for UR5 (https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/), it says that ‘actual_q’ is the actual joint position in radians.

I guess I missed a transformation or got the wrong data.

Can you help me understand why this isn't working?