diff --git a/examples/swerve.rs b/examples/swerve.rs index b2ce0f1d..23aaa4e1 100644 --- a/examples/swerve.rs +++ b/examples/swerve.rs @@ -42,8 +42,8 @@ fn main() { 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_angular_velocity(0, 0.0); - path.wpt_angular_velocity(1, 0.0); + path.wpt_angular_velocity_max_magnitude(0, 0.0); + path.wpt_angular_velocity_max_magnitude(1, 0.0); path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); path.set_control_interval_counts(vec![40]); println!("setup complete"); diff --git a/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp b/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp deleted file mode 100644 index c23c3742..00000000 --- a/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include "trajopt/geometry/Pose2.hpp" -#include "trajopt/geometry/Translation2.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Angular velocity equality constraint. - */ -class TRAJOPT_DLLEXPORT AngularVelocityEqualityConstraint { - public: - /** - * Constructs an AngularVelocityEqualityConstraint. - * - * @param angularVelocity The angular velocity. - */ - explicit AngularVelocityEqualityConstraint(double angularVelocity) - : m_angularVelocity{angularVelocity} {} - - /** - * Applies this constraint to the given problem. - * - * @param problem The optimization problem. - * @param pose The robot's pose. - * @param linearVelocity The robot's linear velocity. - * @param angularVelocity The robot's angular velocity. - * @param linearAcceleration The robot's linear acceleration. - * @param angularAcceleration The robot's angular acceleration. - */ - void Apply(sleipnir::OptimizationProblem& problem, - [[maybe_unused]] const Pose2v& pose, - [[maybe_unused]] const Translation2v& linearVelocity, - const sleipnir::Variable& angularVelocity, - [[maybe_unused]] const Translation2v& linearAcceleration, - [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { - problem.SubjectTo(angularVelocity == m_angularVelocity); - } - - private: - double m_angularVelocity; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/Constraint.hpp b/include/trajopt/constraint/Constraint.hpp index 0c4b875f..b6a21081 100644 --- a/include/trajopt/constraint/Constraint.hpp +++ b/include/trajopt/constraint/Constraint.hpp @@ -9,7 +9,6 @@ #include #include -#include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp" #include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp" #include "trajopt/constraint/LinePointConstraint.hpp" #include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp" @@ -47,7 +46,6 @@ concept ConstraintType = } -> std::same_as; }; -static_assert(ConstraintType); static_assert(ConstraintType); static_assert(ConstraintType); static_assert(ConstraintType); @@ -59,8 +57,7 @@ static_assert(ConstraintType); static_assert(ConstraintType); using Constraint = - std::variant callback); + /** + * Cancel all currently generating SwervePathBuilders. + */ + void CancelAll(); + private: SwervePath path; diff --git a/src/lib.rs b/src/lib.rs index a19fa03c..775c5847 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -88,11 +88,6 @@ mod ffi { index: usize, magnitude: f64, ); - fn wpt_angular_velocity( - self: Pin<&mut SwervePathBuilderImpl>, - index: usize, - angular_velocity: f64, - ); fn wpt_angular_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, index: usize, @@ -118,12 +113,6 @@ mod ffi { to_index: usize, magnitude: f64, ); - fn sgmt_angular_velocity( - self: Pin<&mut SwervePathBuilderImpl>, - from_index: usize, - to_index: usize, - angular_velocity: f64, - ); fn sgmt_angular_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, from_index: usize, @@ -249,14 +238,6 @@ impl SwervePathBuilder { ); } - pub fn wpt_angular_velocity(&mut self, index: usize, angular_velocity: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity( - self.path.pin_mut(), - index, - angular_velocity, - ); - } - pub fn wpt_angular_velocity_max_magnitude(&mut self, index: usize, angular_velocity: f64) { crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude( self.path.pin_mut(), @@ -309,20 +290,6 @@ impl SwervePathBuilder { ); } - pub fn sgmt_angular_velocity( - &mut self, - from_index: usize, - to_index: usize, - angular_velocity: f64, - ) { - crate::ffi::SwervePathBuilderImpl::sgmt_angular_velocity( - self.path.pin_mut(), - from_index, - to_index, - angular_velocity, - ); - } - pub fn sgmt_angular_velocity_max_magnitude( &mut self, from_index: usize, @@ -413,9 +380,6 @@ impl SwervePathBuilder { } } - pub fn cancel_all(&mut self) { - crate::ffi::SwervePathBuilderImpl::cancel_all(self.path.pin_mut()); - } /// /// Add a callback that will be called on each iteration of the solver. /// @@ -428,6 +392,10 @@ impl SwervePathBuilder { pub fn add_progress_callback(&mut self, callback: fn(HolonomicTrajectory, i64)) { crate::ffi::SwervePathBuilderImpl::add_progress_callback(self.path.pin_mut(), callback); } + + pub fn cancel_all(&mut self) { + crate::ffi::SwervePathBuilderImpl::cancel_all(self.path.pin_mut()); + } } impl Default for SwervePathBuilder { diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index 8987080f..f9404981 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -16,10 +16,6 @@ namespace trajopt { -void SwervePathBuilder::CancelAll() { - trajopt::GetCancellationFlag() = 1; -} - SwervePath& SwervePathBuilder::GetPath() { return path; } @@ -202,6 +198,10 @@ void SwervePathBuilder::AddIntermediateCallback( path.callbacks.push_back(callback); } +void SwervePathBuilder::CancelAll() { + trajopt::GetCancellationFlag() = 1; +} + void SwervePathBuilder::NewWpts(size_t finalIndex) { int64_t targetIndex = finalIndex; int64_t greatestIndex = path.waypoints.size() - 1; diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index e061698b..2ea1c8dc 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -10,7 +10,6 @@ #include #include "trajopt/SwerveTrajectoryGenerator.hpp" -#include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp" #include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp" #include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp" #include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp" @@ -129,13 +128,6 @@ void SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); } -void SwervePathBuilderImpl::wpt_angular_velocity(size_t index, - double angular_velocity) { - // this probably ought to be added to SwervePathBuilder in the C++ API - path.WptConstraint( - index, trajopt::AngularVelocityEqualityConstraint{angular_velocity}); -} - void SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude( size_t index, double angular_velocity) { path.WptConstraint( @@ -164,14 +156,6 @@ void SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude( trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); } -void SwervePathBuilderImpl::sgmt_angular_velocity(size_t from_index, - size_t to_index, - double angular_velocity) { - path.SgmtConstraint( - from_index, to_index, - trajopt::AngularVelocityEqualityConstraint{angular_velocity}); -} - void SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude( size_t from_index, size_t to_index, double angular_velocity) { path.SgmtConstraint( diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp index 05637546..6d422dcb 100644 --- a/src/trajoptlibrust.hpp +++ b/src/trajoptlibrust.hpp @@ -33,7 +33,6 @@ class SwervePathBuilderImpl { void wpt_linear_velocity_direction(size_t index, double angle); void wpt_linear_velocity_max_magnitude(size_t index, double magnitude); - void wpt_angular_velocity(size_t index, double angular_velocity); void wpt_angular_velocity_max_magnitude(size_t index, double angular_velocity); void wpt_point_at(size_t index, double field_point_x, double field_point_y, @@ -43,8 +42,6 @@ class SwervePathBuilderImpl { double angle); void sgmt_linear_velocity_max_magnitude(size_t from_index, size_t to_index, double magnitude); - void sgmt_angular_velocity(size_t from_index, size_t to_index, - double angular_velocity); void sgmt_angular_velocity_max_magnitude(size_t from_index, size_t to_index, double angular_velocity); void sgmt_point_at(size_t from_index, size_t to_index, double field_point_x,