Skip to content

Commit

Permalink
fix: fix wrap-around of yaw angles near +- 180 degrees
Browse files Browse the repository at this point in the history
  • Loading branch information
ntamas committed Dec 4, 2024
1 parent 20ab2bd commit 2f51d86
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 0 deletions.
13 changes: 13 additions & 0 deletions src/modules/sbstudio/model/yaw.py
Original file line number Diff line number Diff line change
Expand Up @@ -123,3 +123,16 @@ def simplify(self: C) -> C:
self.setpoints = new_setpoints

return self

def unwrap(self: C, *, threshold: float = 180, full_cycle: float = 360) -> C:
"""Unwraps the yaw angles of the setpoint list "in-place" and ensures
that consecutive sampled angles never have a difference of more than
180 degrees.
"""
for prev, curr in zip(self.setpoints, self.setpoints[1:]):
diff = curr.angle - prev.angle
if diff > threshold or diff < -threshold:
num_cycles = -round(diff / full_cycle)
curr.angle += num_cycles * full_cycle

return self
10 changes: 10 additions & 0 deletions src/modules/sbstudio/plugin/utils/sampling.py
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,11 @@ def sample_positions_and_yaw_of_objects(
YawSetpoint(time, get_xyz_euler_rotation_of_object(obj)[2])
)

# Ensure that the yaw curve makes sense even if the extracted yaw angles
# "wrap around" the boundary between -180 and 180 degrees
for yaw in yaw_setpoints.values():
yaw.unwrap()

if simplify:
return {
key: (
Expand Down Expand Up @@ -315,6 +320,11 @@ def sample_positions_colors_and_yaw_of_objects(
)
yaw_setpoints[key].append(YawSetpoint(time, rotation[2]))

# Ensure that the yaw curve makes sense even if the extracted yaw angles
# "wrap around" the boundary between -180 and 180 degrees
for yaw in yaw_setpoints.values():
yaw.unwrap()

if simplify:
return {
key: (
Expand Down

0 comments on commit 2f51d86

Please sign in to comment.