From c9f8140e92dff07b2edb9c7034fea4d18dc46c9a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden <70111132+Lewis-Seiden@users.noreply.github.com> Date: Thu, 28 Dec 2023 23:48:46 -0800 Subject: [PATCH] Polygon obstacle plumbing, obstacles fix (#69) * parent 20bb8c2ef016ceedab0dc5c0271ad9b69048039a author Lewy Seiden 1697929321 -0700 committer Lewy Seiden 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 --- .../trajopt/constraint/LinePointConstraint.h | 2 +- rust/examples/swerve.rs | 1 + rust/include/trajoptlib.h | 3 + rust/src/lib.rs | 27 +++++++++ rust/src/trajoptlib.cc | 16 +++++- .../algorithms/SwerveDiscreteOptimal.inc | 26 +++++++++ src/path/SwervePathBuilder.cpp | 39 +++++++------ test/src/ObstacleTest.cpp | 57 +++++++++++++++++++ 8 files changed, 153 insertions(+), 18 deletions(-) create mode 100644 test/src/ObstacleTest.cpp diff --git a/include/trajopt/constraint/LinePointConstraint.h b/include/trajopt/constraint/LinePointConstraint.h index 33d001d6..66206081 100644 --- 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 index a368929f..c777057e 100644 --- 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 index b913e4e7..b17312b3 100644 --- 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 x, rust::Vec y, + double radius); HolonomicTrajectory generate() const; void cancel_all(); diff --git a/rust/src/lib.rs b/rust/src/lib.rs index 4cc43e42..3e660ddf 100644 --- 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, + y: Vec, + radius: f64, + ); + fn generate(self: &SwervePathBuilderImpl) -> Result; fn new_swerve_path_builder_impl() -> UniquePtr; @@ -338,6 +347,24 @@ impl SwervePathBuilder { ); } + pub fn sgmt_polygon_obstacle( + &mut self, + from_idx: usize, + to_idx: usize, + x: Vec, + y: Vec, + radius: f64, + ) { + crate::ffi::SwervePathBuilderImpl::sgmt_polygon_obstacle( + self.path.pin_mut(), + from_idx, + to_idx, + x, + y, + radius, + ); + } + pub fn generate(&self) -> Result { match self.path.generate() { Ok(traj) => Ok(traj), diff --git a/rust/src/trajoptlib.cc b/rust/src/trajoptlib.cc index ce85d1b7..f9b90994 100644 --- 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 x, const rust::Vec y, double radius) { + std::vector 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 index 7fecf038..679486d2 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.inc +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.inc @@ -2,6 +2,7 @@ #pragma once +#include #include #include #include @@ -79,8 +80,33 @@ SwerveDiscreteOptimal::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 index abaded7f..adba18b4 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -200,8 +200,8 @@ void SwervePathBuilder::NewWpts(size_t finalIndex) { std::vector 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 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 new file mode 100644 index 00000000..4223e952 --- /dev/null +++ b/test/src/ObstacleTest.cpp @@ -0,0 +1,57 @@ +// Copyright (c) TrajoptLib contributors + +#include + +#include + +#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)); +}