fix trajectory tracking calculation

kerikun11 · Apr 25, 2021 · 9cf7da4 · 9cf7da4
1 parent d8a96a9
commit 9cf7da4
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/include/ctrl/trajectory_tracker.h b/include/ctrl/trajectory_tracker.h
@@ -150,7 +150,7 @@ class TrajectoryTracker {
                      k1 * (cos_theta * (x_r - x) + sin_theta * (y_r - y));
       const auto w = w_d +
                      k2 * v_d * sinc(th_r - theta) *
-                         (cos_theta * (x_r - x) - sin_theta * (y_r - y)) +
+                         (-sin_theta * (x_r - x) + cos_theta * (y_r - y)) +
                      k3 * (th_r - theta);
       res.v = v;
       res.w = w;