Polygon obstacle plumbing, obstacles fix

include/trajopt/constraint/LinePointConstraint.h

@@ -26,7 +26,7 @@ struct LinePointConstraint {
   double fieldPointX;
   /// field point y
   double fieldPointY;
-  /// the required minimum distance between the line and point, must be positive
+  /// the allowed distances between the line segment and point
   IntervalSet1d distance;
 };
 

rust/examples/swerve.rs

@@ -38,6 +38,7 @@ fn main() {
 
     let mut path = SwervePathBuilder::new();
     path.set_drivetrain(&drivetrain);
+    path.set_bumpers(1.3, 1.3);
     path.pose_wpt(0, 0.0, 0.0, 0.0);
     path.pose_wpt(1, 1.0, 0.0, 0.0);
     path.wpt_linear_velocity_polar(0, 0.0, 0.0);

rust/include/trajoptlib.h

@@ -50,6 +50,9 @@ class SwervePathBuilderImpl {
 
   void sgmt_circle_obstacle(size_t from_idx, size_t to_idx, double x, double y,
                             double radius);
+  void sgmt_polygon_obstacle(size_t from_idx, size_t to_idx,
+                             rust::Vec<double> x, rust::Vec<double> y,
+                             double radius);
 
   HolonomicTrajectory generate() const;
   void cancel_all();

rust/src/lib.rs

@@ -145,6 +145,15 @@ mod ffi {
             radius: f64,
         );
 
+        fn sgmt_polygon_obstacle(
+            self: Pin<&mut SwervePathBuilderImpl>,
+            from_idx: usize,
+            to_idx: usize,
+            x: Vec<f64>,
+            y: Vec<f64>,
+            radius: f64,
+        );
+
         fn generate(self: &SwervePathBuilderImpl) -> Result<HolonomicTrajectory>;
 
         fn new_swerve_path_builder_impl() -> UniquePtr<SwervePathBuilderImpl>;
@@ -338,6 +347,24 @@ impl SwervePathBuilder {
         );
     }
 
+    pub fn sgmt_polygon_obstacle(
+        &mut self,
+        from_idx: usize,
+        to_idx: usize,
+        x: Vec<f64>,
+        y: Vec<f64>,
+        radius: f64,
+    ) {
+        crate::ffi::SwervePathBuilderImpl::sgmt_polygon_obstacle(
+            self.path.pin_mut(),
+            from_idx,
+            to_idx,
+            x,
+            y,
+            radius,
+        );
+    }
+
     pub fn generate(&self) -> Result<HolonomicTrajectory, String> {
         match self.path.generate() {
             Ok(traj) => Ok(traj),

rust/src/trajoptlib.cc

@@ -97,7 +97,7 @@ void SwervePathBuilderImpl::set_control_interval_counts(
 
 void SwervePathBuilderImpl::set_bumpers(double length, double width) {
   path.AddBumpers(trajopt::Bumpers{
-    .safetyDistance = 0.0,
+    .safetyDistance = 0.01,
     .points = {
       {+length / 2, +width / 2},
       {-length / 2, +width / 2},
@@ -218,6 +218,20 @@ void SwervePathBuilderImpl::sgmt_circle_obstacle(size_t from_idx, size_t to_idx,
   path.SgmtObstacle(from_idx, to_idx, obstacle);
 }
 
+void SwervePathBuilderImpl::sgmt_polygon_obstacle(size_t from_idx, size_t to_idx, const rust::Vec<double> x, const rust::Vec<double> y, double radius) {
+  std::vector<trajopt::ObstaclePoint> points;
+  if (x.size() != y.size()) return;
+  for (size_t i = 0; i < x.size(); i++)
+  {
+    points.push_back({x.at(i), y.at(i)});
+  }
+  auto obstacle = trajopt::Obstacle{
+    .safetyDistance = radius,
+    .points = points
+  };
+  path.SgmtObstacle(from_idx, to_idx, obstacle);
+}
+
 HolonomicTrajectorySample _convert_holonomic_trajectory_sample(const trajopt::HolonomicTrajectorySample& sample) {
   return HolonomicTrajectorySample{
     .timestamp = sample.timestamp,

src/optimization/algorithms/SwerveDiscreteOptimal.inc

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <algorithm>
 #include <iostream>
 #include <string>
 #include <vector>
@@ -79,8 +80,33 @@ SwerveDiscreteOptimal<Expr, Opti>::SwerveDiscreteOptimal(
     }
   }
 
+  double minWidth = INFINITY;
+  for (size_t i = 1; i < path.drivetrain.modules.size(); i++) {
+    if (std::abs(path.drivetrain.modules.at(i - 1).x -
+                 path.drivetrain.modules.at(i).x) != 0) {
+      minWidth =
+          std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).x -
+                                      path.drivetrain.modules.at(i).x));
+    }
+    if (std::abs(path.drivetrain.modules.at(i - 1).y -
+                 path.drivetrain.modules.at(i).y) != 0) {
+      minWidth =
+          std::min(minWidth, std::abs(path.drivetrain.modules.at(i - 1).y -
+                                      path.drivetrain.modules.at(i).y));
+    }
+  }
+
+  std::cout << "Min Width: ";
+  std::cout << minWidth;
+  std::cout << "\n";
+
   for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) {
     dt.emplace_back(opti.DecisionVariable());
+    for (auto module : path.drivetrain.modules) {
+      opti.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius *
+                         module.wheelMaxAngularVelocity <=
+                     minWidth);
+    }
   }
 
   ApplyDiscreteTimeObjective(opti, dt, N);

src/path/SwervePathBuilder.cpp

@@ -200,8 +200,8 @@ void SwervePathBuilder::NewWpts(size_t finalIndex) {
 
 std::vector<HolonomicConstraint> SwervePathBuilder::GetConstraintsForObstacle(
     const Bumpers& bumpers, const Obstacle& obstacle) {
-  auto distConst =
-      IntervalSet1d::LessThan(bumpers.safetyDistance + obstacle.safetyDistance);
+  auto distConst = IntervalSet1d::GreaterThan(bumpers.safetyDistance +
+                                              obstacle.safetyDistance);
 
   size_t bumperCornerCount = bumpers.points.size();
   size_t obstacleCornerCount = obstacle.points.size();
@@ -238,20 +238,27 @@ std::vector<HolonomicConstraint> SwervePathBuilder::GetConstraintsForObstacle(
 
   // obstacle edge to bumper corner constraints
   for (auto& bumperCorner : bumpers.points) {
-    for (size_t obstacleCornerIndex = 0;
-         obstacleCornerIndex < obstacleCornerCount - 1; obstacleCornerIndex++) {
-      constraints.emplace_back(PointLineConstraint{
-          bumperCorner.x, bumperCorner.y,
-          obstacle.points.at(obstacleCornerIndex).x,
-          obstacle.points.at(obstacleCornerIndex).y,
-          obstacle.points.at(obstacleCornerIndex + 1).x,
-          obstacle.points.at(obstacleCornerIndex + 1).y, distConst});
-    }
-    if (obstacleCornerCount >= 3) {
-      constraints.emplace_back(PointLineConstraint{
-          bumperCorner.x, bumperCorner.y,
-          obstacle.points.at(bumperCornerCount - 1).x,
-          obstacle.points.at(bumperCornerCount - 1).y, obstacle.points.at(0).x,
+    if (obstacleCornerCount > 1) {
+      for (size_t obstacleCornerIndex = 0;
+           obstacleCornerIndex < obstacleCornerCount - 1;
+           obstacleCornerIndex++) {
+        constraints.emplace_back(PointLineConstraint{
+            bumperCorner.x, bumperCorner.y,
+            obstacle.points.at(obstacleCornerIndex).x,
+            obstacle.points.at(obstacleCornerIndex).y,
+            obstacle.points.at(obstacleCornerIndex + 1).x,
+            obstacle.points.at(obstacleCornerIndex + 1).y, distConst});
+      }
+      if (obstacleCornerCount >= 3) {
+        constraints.emplace_back(PointLineConstraint{
+            bumperCorner.x, bumperCorner.y,
+            obstacle.points.at(bumperCornerCount - 1).x,
+            obstacle.points.at(bumperCornerCount - 1).y,
+            obstacle.points.at(0).x, obstacle.points.at(0).y, distConst});
+      }
+    } else {
+      constraints.emplace_back(PointPointConstraint{
+          bumperCorner.x, bumperCorner.y, obstacle.points.at(0).x,
           obstacle.points.at(0).y, distConst});
     }
   }

test/src/ObstacleTest.cpp

@@ -0,0 +1,57 @@
+// Copyright (c) TrajoptLib contributors
+
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include "path/InitialGuessPoint.h"
+#include "path/SwervePathBuilder.h"
+
+TEST(ObstacleTest, GenerateLinearInitialGuess) {
+  using namespace trajopt;
+  trajopt::SwervePathBuilder path;
+  path.SetDrivetrain({45, 6,
+                      [{
+                        x : 0.6,
+                        y : 0.6,
+                        wheel_radius : 0.04,
+                        wheel_max_angular_velocity : 70.0,
+                        wheel_max_torque : 2.0,
+                      },
+                       {
+                         x : 0.6,
+                         y : -0.6,
+                         wheel_radius : 0.04,
+                         wheel_max_angular_velocity : 70.0,
+                         wheel_max_torque : 2.0,
+                       },
+                       {
+                         x : -0.6,
+                         y : 0.6,
+                         wheel_radius : 0.04,
+                         wheel_max_angular_velocity : 70.0,
+                         wheel_max_torque : 2.0,
+                       },
+                       {
+                         x : -0.6,
+                         y : -0.6,
+                         wheel_radius : 0.04,
+                         wheel_max_angular_velocity : 70.0,
+                         wheel_max_torque : 2.0,
+                       }]});
+  path.PoseWpt(0, 0.0, 0.0, 0.0);
+  path.PoseWpt(1, 2.0, 2.0, 0.0);
+
+  const length = 0.7;
+  path.AddBumpers(trajopt::Bumpers{.safetyDistance = 0.1,
+                                   .points = {{+length / 2, +width / 2},
+                                              {-length / 2, +width / 2},
+                                              {-length / 2, -width / 2},
+                                              {+length / 2, -width / 2}}});
+
+  path.SgmtObstacle(
+      0, 1, trajopt::Obstacle{.safetyDistance = 1.0, .points = {{1.0, 1.0}}});
+
+  path.ControlIntervalCounts({10});
+  ASSERT_NO_THROW(trajopt::OptimalTrajectoryGenerator::Generate(path));
+}