def distance_quat(p1, p2):
if isinstance(p1, PoseStamped):<EOL><INDENT>p1 = pose_to_list(p1)<EOL><DEDENT>if isinstance(p2, PoseStamped):<EOL><INDENT>p2 = pose_to_list(p2)<EOL><DEDENT>if _is_indexable(p1) and _is_indexable(p1[<NUM_LIT:0>]):<EOL><INDENT>p1 = p1[<NUM_LIT:1>]<EOL>p2 = p2[<NUM_LIT:1>]<EOL><DEDENT>dotp = inner(array(p1), array(p2))<EOL>return arccos(<NUM_LIT:2> * dotp * dotp - <NUM_LIT:1>)<EOL>
Returns the angle between two quaternions
http://math.stackexchange.com/a/90098
:param q1, q2: two quaternions [x, y, z, w] or two poses [[x, y, z], [x, y, z, w]] or two PoseStamped
:return: the angle between them (radians)