Apologies in advance my Git-Fu is weak. I'm sure there's a better way to do this...
I've added "roll" to my version of your code. I'm using it to move a mechatronic head so needed full range of movement in 3D.
ray_left_right = deque(maxlen=filter_length)
.....
ray_left_right.append(right_axis)
avg_left_right = np.mean(ray_left_right, axis=0)
avg_left_right /= np.linalg.norm(avg_left_right)
# Tilt (roll) angle from reference (project onto XY plane)
xy_proj = np.array([avg_left_right[0], avg_left_right[1], 0])
xy_proj /= np.linalg.norm(xy_proj)
roll_rad = math.acos(np.clip(np.dot(reference_left_right, xy_proj), -1.0, 1.0))
if avg_left_right[1] < 0:
roll_rad = -roll_rad # clockwise positive
Apologies in advance my Git-Fu is weak. I'm sure there's a better way to do this...
I've added "roll" to my version of your code. I'm using it to move a mechatronic head so needed full range of movement in 3D.