I have a UR5 CB3 robotic arm that shall be trained to pick up watermelons. For that, I installed a camera that determines the location of the watermelon with respect to the TCP (or end effector / the gripping “hand”). This TCP coordinate system is called K1. I also know the location and orientation of the TCP with respect to the base coordinate system. This base coordinate system is called K0.
In order to simplifiy the problem, I removed all the code used to determine the watermelon location, and just inserted arbitrary coordinates given in the K1 coordinate system, as for example [ 0 , 0 , 0.1]. Now I want to calculate what these coordinates are in the K0 coordinate system. Mathematically, this should be a fairly simple code, I now would use the [roll, pitch, yaw] to calculate the rotation matrix rot_mat_K1_to_K0 and then use matrix multiplication of rot_mat_K1_to_K0 with the vecotor pointing to W in the K1 system. Then I would add the vector from K0 to K1 given in K1 coordinated and the now rotated vector from K1 to W in a simple vector addition.
Then I could send this final vector to my robot to move there.