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)); +}