We know where the coordinates of our vacuum robots charging bay and we know the current special euclidean transform matrix for our helper. How should it turn to point straight towards its destination? That was a slightly dramatized version of a question I got and answered and I decided to put my answer here for reference.

So given that you have your pose T_cw specified with a SE(3) transform and your robots forward direction is in its frame’s x-direction. Use the vector towards your target location and the unit-vector in the direction where the robot is looking, and calculate the angle from their scalar product:

Calculations illustrated with an example below where the vacuum is in the xy-plane at position (3,1,0) and rotated 90 degree in the right-handed positive direction along the z-axis, represented with its SE(3) transform Tcw which transforms points in the world-coordinate system to the robots coordinate system. The unit-vector in the robots x-direction is acquired by taking the difference Tcw*(1,0,0,1)^T – Tcw*(0,0,0,1)^T = (0,1,0,1). The forth coordinate is a homogeneous coordinate to deal with the rotation and the translation with one matrix operation. Using the first three coordinates of this vector together with the vector that you get by taking the destination minus the robot location, you calculate the turn angle from the scalar product above.