diff --git a/social_mpc/path.py b/social_mpc/path.py
index 8113dd6d4fafbe11e02b850c4b5b5d4c8431de78..731274f611a9ff77c72fc8e17e19523ee0e99b89 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -76,7 +76,7 @@ class PathPlanner():
 
         if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
             return []
-        d_angle = np.arctan2(vxl[0], -vyl[0]) - pos[2]
+        d_angle = utils.constraint_angle(np.arctan2(vxl[0], -vyl[0]) - pos[2])
         if dl[0] < self.max_d_orientation:  # if within range, set waypoint = target
             wpt_pos = [self.target[0], self.target[1], self.target[2]]
         elif dl[0] < self.max_d: