Add zero angular velocity constraint to zero velocity requested paths

Mechanical-Advantage · Feb 11, 2024 · cd9294d · cd9294d
1 parent 0beef02
commit cd9294d
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp
@@ -113,12 +113,14 @@ class VehicleTrajectoryService final
                             fmt::print("Adding implicit zero velocity constraint to waypoint {}\n",
                                        segment_start_offset + waypoint_idx);
                             builder.WptZeroVelocity(segment_start_offset + waypoint_idx);
+                            builder.WptZeroAngularVelocity(segment_start_offset + waypoint_idx);
                         }
                         break;
                     case vts::Waypoint::kZeroVelocity:
                         fmt::print("Adding zero velocity constraint to waypoint {}\n",
                                    segment_start_offset + waypoint_idx);
                         builder.WptZeroVelocity(segment_start_offset + waypoint_idx);
+                        builder.WptZeroAngularVelocity(segment_start_offset + waypoint_idx);
                         break;
                     case vts::Waypoint::kVehicleVelocity:
                         fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n",