Shortest rotation between two vectors not working like expected

Question

def signed_angle_between_vecs(target_vec, start_vec, plane_normal=None):
    start_vec = np.array(start_vec)
    target_vec = np.array(target_vec)

    start_vec = start_vec/np.linalg.norm(start_vec)
    target_vec = target_vec/np.linalg.norm(target_vec)

    if plane_normal is None:
        arg1 = np.dot(np.cross(start_vec, target_vec), np.cross(start_vec, target_vec))
    else:
        arg1 = np.dot(np.cross(start_vec, target_vec), plane_normal)
    arg2 = np.dot(start_vec, target_vec)
    return np.arctan2(arg1, arg2)


from scipy.spatial.transform import Rotation as R


    world_frame_axis = input_rotation_object.apply(canonical_axis)

    angle = signed_angle_between_vecs(canonical_axis, world_frame_axis)

    axis_angle = np.cross(world_frame_axis, canonical_axis) * angle
    
    C = R.from_rotvec(axis_angle)

    transformed_world_frame_axis_to_canonical = C.apply(world_frame_axis)

I am trying to align world_frame_axis to canonical_axis by performing a rotation around the normal vector generated by the cross product between the two vectors, using the signed angle between the two axes.

However, this code does not work. If you start with some arbitrary rotation as input_rotation_object you will see that transformed_world_frame_axis_to_canonical does not match canonical_axis.

What am I doing wrong?

Spektre Spektre · Accepted Answer · 2021-01-16T10:09:49

not a python coder so I might be wrong but this looks suspicious:

start_vec = start_vec/np.linalg.norm(start_vec)

from the names I would expect that np.linalg.norm normalizes the vector already so the line should be:

start_vec = np.linalg.norm(start_vec)

and all the similar lines too ...

Also the atan2 operands are not looking right to me. I would (using math):

a = start_vec  / |start_vec |  // normalized start
b = target_vec / |target_vec|  // normalized end
u = a                          // normalized one axis of plane
v = cross(u ,b)                
v = cross(v ,u)
v = v / |v|                    // normalized second axis of plane perpendicular to u
dx = dot(u,b)                  // target vector in 2D aligned to start
dy = dot(v,b)
ang = atan2(dy,dx)

beware the ang might negated (depending on your notations) if the case either add minus sign or reverse the order in cross(u,v) to cross(v,u) Also you can do sanity check with comparing result to unsigned:

ang' = acos(dot(a,b))

in absolute values they should be the same (+/- rounding error).

Shortest rotation between two vectors not working like expected

1 Answers