Polygon obstacle plumbing, obstacles fix (#69)

* parent 20bb8c2 author Lewy Seiden <[email protected]> 1697929321 -0700 committer Lewy Seiden <[email protected]> 1703659757 -0800 started playing with dt removed cmake cache made solution return actual dts unfucked(?) variable dt added min time to dt stopped double constraining dt changed kinematics constraint dt sampling to current sample instead of prev changed dt sample in discrete time goal added upper bound to dt increased maximum dt fixed control interval jumping Run wpiformat fixed swerve obstacles added polygon plumbing reduced max dt Run wpiformat reverted var-dt changes * added max dt based on robot frame size * ran wpiformat * fixed format violation * removed cmake generated files --------- Co-authored-by: Lewy Seiden <[email protected]>
SleipnirGroup · Dec 29, 2023 · c9f8140 · c9f8140
1 parent a2f290d
commit c9f8140
Show file tree

Hide file tree

Showing 8 changed files with 153 additions and 18 deletions.
diff --git a/include/trajopt/constraint/LinePointConstraint.h b/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;
 };
 

diff --git a/rust/examples/swerve.rs b/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);

diff --git a/rust/include/trajoptlib.h b/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();

diff --git a/rust/src/lib.rs b/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),

diff --git a/rust/src/trajoptlib.cc b/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,

diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.inc b/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);

diff --git a/src/path/SwervePathBuilder.cpp b/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});
     }
   }

diff --git a/test/src/ObstacleTest.cpp b/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));
+}