From 239cd0bb0b5be086342cc7c34bb49b7ccc6aa7bf Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 24 Jun 2024 09:39:01 -0700 Subject: [PATCH] Spell out index instead of using idx abbreviation (#208) --- include/trajopt/path/SwervePathBuilder.hpp | 122 +++++----- src/lib.rs | 212 ++++++++++-------- src/optimization/SwerveTrajoptUtil.hpp | 76 ++++--- src/optimization/TrajoptUtil.hpp | 90 ++++---- .../algorithms/SwerveDiscreteOptimal.hpp | 69 +++--- src/path/SwervePathBuilder.cpp | 115 +++++----- src/trajoptlibrust.cpp | 111 ++++----- src/trajoptlibrust.hpp | 53 ++--- test/src/TrajoptUtilTest.cpp | 10 +- 9 files changed, 448 insertions(+), 410 deletions(-) diff --git a/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp index fd8cbd8b..7258c6b5 100644 --- a/include/trajopt/path/SwervePathBuilder.hpp +++ b/include/trajopt/path/SwervePathBuilder.hpp @@ -50,12 +50,12 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * position and heading of the robot at the waypoint must be fixed at the * values provided. * - * @param idx index of the pose waypoint + * @param index index of the pose waypoint * @param x the x * @param y the y * @param heading the heading */ - void PoseWpt(size_t idx, double x, double y, double heading); + void PoseWpt(size_t index, double x, double y, double heading); /** * Create a translation waypoint constraint on the waypoint at the @@ -63,150 +63,150 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * This specifies that the position of the robot at the waypoint must be fixed * at the value provided. * - * @param idx index of the pose waypoint + * @param index index of the pose waypoint * @param x the x * @param y the y * @param headingGuess optionally, an initial guess of the heading */ - void TranslationWpt(size_t idx, double x, double y, + void TranslationWpt(size_t index, double x, double y, double headingGuess = 0.0); /** * Provide a guess of the instantaneous pose of the robot at a waypoint. * - * @param wptIdx the waypoint to apply the guess to + * @param wptIndex the waypoint to apply the guess to * @param poseGuess the guess of the robot's pose */ - void WptInitialGuessPoint(size_t wptIdx, const Pose2d& poseGuess); + void WptInitialGuessPoint(size_t wptIndex, const Pose2d& poseGuess); /** * Add a sequence of initial guess points between two waypoints. The points - * are inserted between the waypoints at fromIdx and fromIdx + 1. Linear + * are inserted between the waypoints at fromIndex and fromIndex + 1. Linear * interpolation between the waypoint initial guess points and these segment * initial guess points is used as the initial guess of the robot's pose over * the trajectory. * - * @param fromIdx index of the waypoint the initial guess point + * @param fromIndex index of the waypoint the initial guess point * comes immediately after * @param sgmtPoseGuess the sequence of initial guess points */ - void SgmtInitialGuessPoints(size_t fromIdx, + void SgmtInitialGuessPoints(size_t fromIndex, const std::vector& sgmtPoseGuess); /** * Specify the required direction of the velocity vector of the robot * at a waypoint. * - * @param idx index of the waypoint + * @param index index of the waypoint * @param angle the polar angle of the required direction */ - void WptVelocityDirection(size_t idx, double angle); + void WptVelocityDirection(size_t index, double angle); /** * Specify the required maximum magnitude of the velocity vector of the * robot at a waypoint. * - * @param idx index of the waypoint + * @param index index of the waypoint * @param v the maximum velocity magnitude */ - void WptVelocityMagnitude(size_t idx, double v); + void WptVelocityMagnitude(size_t index, double v); /** * Specify the required velocity vector of the robot at a waypoint to * be zero. * - * @param idx index of the waypoint + * @param index index of the waypoint */ - void WptZeroVelocity(size_t idx); + void WptZeroVelocity(size_t index); /** * Specify the exact required velocity vector of the robot at a * waypoint. * - * @param idx index of the waypoint + * @param index index of the waypoint * @param vr velocity vector magnitude * @param vtheta velocity vector polar angle */ - void WptVelocityPolar(size_t idx, double vr, double vtheta); + void WptVelocityPolar(size_t index, double vr, double vtheta); /** * Specify the required angular velocity of the robot at a waypoint * - * @param idx index of the waypoint + * @param index index of the waypoint * @param angular_velocity the angular velocity */ - void WptAngularVelocity(size_t idx, double angular_velocity); + void WptAngularVelocity(size_t index, double angular_velocity); /** * Specify the required angular velocity of the robot at a waypoint * - * @param idx index of the waypoint + * @param index index of the waypoint * @param angular_velocity the maximum angular velocity magnitude */ - void WptAngularVelocityMaxMagnitude(size_t idx, double angular_velocity); + void WptAngularVelocityMaxMagnitude(size_t index, double angular_velocity); /** * Specify the required angular velocity of the robot to be zero * at a waypoint * - * @param idx index of the waypoint + * @param index index of the waypoint */ - void WptZeroAngularVelocity(size_t idx); + void WptZeroAngularVelocity(size_t index); /** * Specify the required direction of the velocity vector of the robot * for the continuum of robot state between two waypoints. * - * @param fromIdx the waypoint at the beginning of the continuum - * @param toIdx the waypoint at the end of the continuum + * @param fromIndex the waypoint at the beginning of the continuum + * @param toIndex the waypoint at the end of the continuum * @param angle the polar angle of the required direction * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtVelocityDirection(size_t fromIdx, size_t toIdx, double angle, + void SgmtVelocityDirection(size_t fromIndex, size_t toIndex, double angle, bool includeWpts = true); /** * Specify the required maximum magnitude of the velocity vector of the * robot for the continuum of robot state between two waypoints. * - * @param fromIdx the waypoint at the beginning of the continuum - * @param toIdx the waypoint at the end of the continuum + * @param fromIndex the waypoint at the beginning of the continuum + * @param toIndex the waypoint at the end of the continuum * @param v the maximum velocity magnitude * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, double v, + void SgmtVelocityMagnitude(size_t fromIndex, size_t toIndex, double v, bool includeWpts = true); /** * Specify the required angular velocity of the robot for the continuum * of robot state between two waypoints. * - * @param fromIdx index of the waypoint at the beginning of the continuum - * @param toIdx index of the waypoint at the end of the continuum + * @param fromIndex index of the waypoint at the beginning of the continuum + * @param toIndex index of the waypoint at the end of the continuum * @param angular_velocity the angular velocity * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtAngularVelocity(size_t fromIdx, size_t toIdx, + void SgmtAngularVelocity(size_t fromIndex, size_t toIndex, double angular_velocity, bool includeWpts = true); /** * Specify the required angular velocity magnitude of the robot for the * continuum of robot state between two waypoints. * - * @param fromIdx index of the waypoint at the beginning of the continuum - * @param toIdx index of the waypoint at the end of the continuum + * @param fromIndex index of the waypoint at the beginning of the continuum + * @param toIndex index of the waypoint at the end of the continuum * @param angular_velocity the maximum angular velocity magnitudeks * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtAngularVelocityMaxMagnitude(size_t fromIdx, size_t toIdx, + void SgmtAngularVelocityMaxMagnitude(size_t fromIndex, size_t toIndex, double angular_velocity, bool includeWpts = true); @@ -214,34 +214,34 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * Specify the required angular velocity of the robot to be zero * for the continuum of robot state between two waypoints. * - * @param fromIdx index of the waypoint at the beginning of the continuum - * @param toIdx index of the waypoint at the end of the continuum + * @param fromIndex index of the waypoint at the beginning of the continuum + * @param toIndex index of the waypoint at the end of the continuum * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtZeroAngularVelocity(size_t fromIdx, size_t toIdx, + void SgmtZeroAngularVelocity(size_t fromIndex, size_t toIndex, bool includeWpts = true); /** * Apply a custom holonomic constraint at a waypoint * - * @param idx index of the waypoint + * @param index index of the waypoint * @param constraint the constraint to be applied */ - void WptConstraint(size_t idx, const HolonomicConstraint& constraint); + void WptConstraint(size_t index, const HolonomicConstraint& constraint); /** * Apply a custom holonomic constraint to the continuum of state * between two waypoints. * - * @param fromIdx index of the waypoint at the beginning of the continuum - * @param toIdx index of the waypoint at the end of the continuum + * @param fromIndex index of the waypoint at the beginning of the continuum + * @param toIndex index of the waypoint at the end of the continuum * @param constraint the custom constraint to be applied * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the waypoints at fromIdx and toIdx + * constraint at the waypoints at fromIndex and toIndex */ - void SgmtConstraint(size_t fromIdx, size_t toIdx, + void SgmtConstraint(size_t fromIndex, size_t toIndex, const HolonomicConstraint& constraint, bool includeWpts = true); @@ -256,23 +256,23 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { /** * Apply an obstacle constraint to a waypoint. * - * @param idx index of the waypoint + * @param index index of the waypoint * @param obstacle the obstacle */ - void WptObstacle(size_t idx, const Obstacle& obstacle); + void WptObstacle(size_t index, const Obstacle& obstacle); /** * Apply an obstacle constraint to the continuum of state between two * waypoints. * - * @param fromIdx index of the waypoint at the beginning of the continuum - * @param toIdx index of the waypoint at the end of the continuum + * @param fromIndex index of the waypoint at the beginning of the continuum + * @param toIndex index of the waypoint at the end of the continuum * @param obstacle the obstacle * @param includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx + * constraint at the instantaneous state at waypoints at indices fromIndex and + * toIndex */ - void SgmtObstacle(size_t fromIdx, size_t toIdx, const Obstacle& obstacle, + void SgmtObstacle(size_t fromIndex, size_t toIndex, const Obstacle& obstacle, bool includeWpts = true); /** diff --git a/src/lib.rs b/src/lib.rs index cd392419..3ac81567 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -53,21 +53,21 @@ mod ffi { fn pose_wpt( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, x: f64, y: f64, heading: f64, ); fn translation_wpt( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, x: f64, y: f64, heading_guess: f64, ); fn empty_wpt( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, x_guess: f64, y_guess: f64, heading_guess: f64, @@ -75,42 +75,42 @@ mod ffi { fn sgmt_initial_guess_points( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, + from_index: usize, guess_points: &Vec, ); fn wpt_linear_velocity_direction( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, angle: f64, ); fn wpt_linear_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, magnitude: f64, ); fn wpt_linear_velocity_polar( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, magnitude: f64, angle: f64, ); fn wpt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, angular_velocity: f64, ); fn wpt_angular_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, angular_velocity: f64, ); - fn wpt_x(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, x: f64); - fn wpt_y(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, y: f64); - fn wpt_heading(self: Pin<&mut SwervePathBuilderImpl>, idx: usize, heading: f64); + fn wpt_x(self: Pin<&mut SwervePathBuilderImpl>, index: usize, x: f64); + fn wpt_y(self: Pin<&mut SwervePathBuilderImpl>, index: usize, y: f64); + fn wpt_heading(self: Pin<&mut SwervePathBuilderImpl>, index: usize, heading: f64); fn wpt_point_at( self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, + index: usize, field_point_x: f64, field_point_y: f64, heading_tolerance: f64, @@ -118,47 +118,57 @@ mod ffi { fn sgmt_linear_velocity_direction( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, angle: f64, ); fn sgmt_linear_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, magnitude: f64, ); fn sgmt_linear_velocity_polar( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, magnitude: f64, angle: f64, ); fn sgmt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, angular_velocity: f64, ); fn sgmt_angular_velocity_max_magnitude( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, angular_velocity: f64, ); - fn sgmt_x(self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, to_idx: usize, x: f64); - fn sgmt_y(self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, to_idx: usize, y: f64); + fn sgmt_x( + self: Pin<&mut SwervePathBuilderImpl>, + from_index: usize, + to_index: usize, + x: f64, + ); + fn sgmt_y( + self: Pin<&mut SwervePathBuilderImpl>, + from_index: usize, + to_index: usize, + y: f64, + ); fn sgmt_heading( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, heading: f64, ); fn sgmt_point_at( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, field_point_x: f64, field_point_y: f64, heading_tolerance: f64, @@ -166,8 +176,8 @@ mod ffi { fn sgmt_circle_obstacle( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, x: f64, y: f64, radius: f64, @@ -175,8 +185,8 @@ mod ffi { fn sgmt_polygon_obstacle( self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, x: Vec, y: Vec, radius: f64, @@ -219,24 +229,24 @@ impl SwervePathBuilder { crate::ffi::SwervePathBuilderImpl::set_control_interval_counts(self.path.pin_mut(), counts); } - pub fn pose_wpt(&mut self, idx: usize, x: f64, y: f64, heading: f64) { - crate::ffi::SwervePathBuilderImpl::pose_wpt(self.path.pin_mut(), idx, x, y, heading); + pub fn pose_wpt(&mut self, index: usize, x: f64, y: f64, heading: f64) { + crate::ffi::SwervePathBuilderImpl::pose_wpt(self.path.pin_mut(), index, x, y, heading); } - pub fn translation_wpt(&mut self, idx: usize, x: f64, y: f64, heading_guess: f64) { + pub fn translation_wpt(&mut self, index: usize, x: f64, y: f64, heading_guess: f64) { crate::ffi::SwervePathBuilderImpl::translation_wpt( self.path.pin_mut(), - idx, + index, x, y, heading_guess, ); } - pub fn empty_wpt(&mut self, idx: usize, x_guess: f64, y_guess: f64, heading_guess: f64) { + pub fn empty_wpt(&mut self, index: usize, x_guess: f64, y_guess: f64, heading_guess: f64) { crate::ffi::SwervePathBuilderImpl::empty_wpt( self.path.pin_mut(), - idx, + index, x_guess, y_guess, heading_guess, @@ -245,176 +255,186 @@ impl SwervePathBuilder { pub fn sgmt_initial_guess_points( &mut self, - from_idx: usize, + from_index: usize, guess_points: &Vec, ) { crate::ffi::SwervePathBuilderImpl::sgmt_initial_guess_points( self.path.pin_mut(), - from_idx, + from_index, guess_points, ); } - pub fn wpt_linear_velocity_direction(&mut self, idx: usize, angle: f64) { + pub fn wpt_linear_velocity_direction(&mut self, index: usize, angle: f64) { crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_direction( self.path.pin_mut(), - idx, + index, angle, ); } - pub fn wpt_linear_velocity_max_magnitude(&mut self, idx: usize, magnitude: f64) { + pub fn wpt_linear_velocity_max_magnitude(&mut self, index: usize, magnitude: f64) { crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( self.path.pin_mut(), - idx, + index, magnitude, ); } - pub fn wpt_linear_velocity_polar(&mut self, idx: usize, magnitude: f64, angle: f64) { + pub fn wpt_linear_velocity_polar(&mut self, index: usize, magnitude: f64, angle: f64) { crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_polar( self.path.pin_mut(), - idx, + index, magnitude, angle, ); } - pub fn wpt_angular_velocity(&mut self, idx: usize, angular_velocity: f64) { + pub fn wpt_angular_velocity(&mut self, index: usize, angular_velocity: f64) { crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity( self.path.pin_mut(), - idx, + index, angular_velocity, ); } - pub fn wpt_angular_velocity_max_magnitude(&mut self, idx: usize, angular_velocity: f64) { + 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(), - idx, + index, angular_velocity, ); } - pub fn wpt_x(&mut self, idx: usize, x: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_x(self.path.pin_mut(), idx, x); + pub fn wpt_x(&mut self, index: usize, x: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_x(self.path.pin_mut(), index, x); } - pub fn wpt_y(&mut self, idx: usize, y: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_y(self.path.pin_mut(), idx, y); + pub fn wpt_y(&mut self, index: usize, y: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_y(self.path.pin_mut(), index, y); } - pub fn wpt_heading(&mut self, idx: usize, heading: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), idx, heading); + pub fn wpt_heading(&mut self, index: usize, heading: f64) { + crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), index, heading); } pub fn wpt_point_at( &mut self, - idx: usize, + index: usize, field_point_x: f64, field_point_y: f64, heading_tolerance: f64, ) { crate::ffi::SwervePathBuilderImpl::wpt_point_at( self.path.pin_mut(), - idx, + index, field_point_x, field_point_y, heading_tolerance, ) } - pub fn sgmt_linear_velocity_direction(&mut self, from_idx: usize, to_idx: usize, angle: f64) { + pub fn sgmt_linear_velocity_direction( + &mut self, + from_index: usize, + to_index: usize, + angle: f64, + ) { crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_direction( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, angle, ); } pub fn sgmt_linear_velocity_max_magnitude( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, magnitude: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, magnitude, ); } pub fn sgmt_linear_velocity_polar( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, magnitude: f64, angle: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_polar( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, magnitude, angle, ); } - pub fn sgmt_angular_velocity(&mut self, from_idx: usize, to_idx: usize, angular_velocity: f64) { + 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_idx, - to_idx, + from_index, + to_index, angular_velocity, ); } pub fn sgmt_angular_velocity_max_magnitude( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, angular_velocity: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, angular_velocity, ); } - pub fn sgmt_x(&mut self, from_idx: usize, to_idx: usize, x: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_x(self.path.pin_mut(), from_idx, to_idx, x); + pub fn sgmt_x(&mut self, from_index: usize, to_index: usize, x: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_x(self.path.pin_mut(), from_index, to_index, x); } - pub fn sgmt_y(&mut self, from_idx: usize, to_idx: usize, y: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_y(self.path.pin_mut(), from_idx, to_idx, y); + pub fn sgmt_y(&mut self, from_index: usize, to_index: usize, y: f64) { + crate::ffi::SwervePathBuilderImpl::sgmt_y(self.path.pin_mut(), from_index, to_index, y); } - pub fn sgmt_heading(&mut self, from_idx: usize, to_idx: usize, heading: f64) { + pub fn sgmt_heading(&mut self, from_index: usize, to_index: usize, heading: f64) { crate::ffi::SwervePathBuilderImpl::sgmt_heading( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, heading, ); } pub fn sgmt_point_at( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, field_point_x: f64, field_point_y: f64, heading_tolerance: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_point_at( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, field_point_x, field_point_y, heading_tolerance, @@ -423,16 +443,16 @@ impl SwervePathBuilder { pub fn sgmt_circle_obstacle( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, x: f64, y: f64, radius: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_circle_obstacle( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, x, y, radius, @@ -441,16 +461,16 @@ impl SwervePathBuilder { pub fn sgmt_polygon_obstacle( &mut self, - from_idx: usize, - to_idx: usize, + from_index: usize, + to_index: usize, x: Vec, y: Vec, radius: f64, ) { crate::ffi::SwervePathBuilderImpl::sgmt_polygon_obstacle( self.path.pin_mut(), - from_idx, - to_idx, + from_index, + to_index, x, y, radius, diff --git a/src/optimization/SwerveTrajoptUtil.hpp b/src/optimization/SwerveTrajoptUtil.hpp index 2aa04ad7..49191485 100644 --- a/src/optimization/SwerveTrajoptUtil.hpp +++ b/src/optimization/SwerveTrajoptUtil.hpp @@ -29,13 +29,14 @@ inline sleipnir::Variable SolveNetTorque( const std::vector& swerveModules) { sleipnir::Variable tau_net = 0; - for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) { - auto& swerveModule = swerveModules.at(moduleIdx); + for (size_t moduleIndex = 0; moduleIndex < swerveModules.size(); + ++moduleIndex) { + auto& swerveModule = swerveModules.at(moduleIndex); const auto& [x_m, y_m] = swerveModule.translation.RotateBy(theta); Translation2v r{x_m, y_m}; - Translation2v F{Fx.at(moduleIdx), Fy.at(moduleIdx)}; + Translation2v F{Fx.at(moduleIndex), Fy.at(moduleIndex)}; tau_net += r.Cross(F); } @@ -58,26 +59,26 @@ inline void ApplyKinematicsConstraints( const std::vector& dt, const std::vector& N) { size_t wptCnt = N.size() + 1; - for (size_t wptIdx = 1; wptIdx < wptCnt; ++wptIdx) { - size_t N_sgmt = N.at(wptIdx - 1); - auto dt_sgmt = dt.at(wptIdx - 1); - for (size_t sampIdx = 0; sampIdx < N_sgmt; ++sampIdx) { - size_t idx = GetIdx(N, wptIdx, sampIdx); - auto x_n = x.at(idx); - auto x_n_1 = x.at(idx - 1); - auto y_n = y.at(idx); - auto y_n_1 = y.at(idx - 1); - Rotation2v theta_n{thetacos.at(idx), thetasin.at(idx)}; - Rotation2v theta_n_1{thetacos.at(idx - 1), thetasin.at(idx - 1)}; - auto vx_n = vx.at(idx); - auto vx_n_1 = vx.at(idx - 1); - auto vy_n = vy.at(idx); - auto vy_n_1 = vy.at(idx - 1); - auto omega_n = omega.at(idx); - auto omega_n_1 = omega.at(idx - 1); - auto ax_n = ax.at(idx); - auto ay_n = ay.at(idx); - auto alpha_n = alpha.at(idx); + for (size_t wptIndex = 1; wptIndex < wptCnt; ++wptIndex) { + size_t N_sgmt = N.at(wptIndex - 1); + auto dt_sgmt = dt.at(wptIndex - 1); + for (size_t sampIndex = 0; sampIndex < N_sgmt; ++sampIndex) { + size_t index = GetIndex(N, wptIndex, sampIndex); + auto x_n = x.at(index); + auto x_n_1 = x.at(index - 1); + auto y_n = y.at(index); + auto y_n_1 = y.at(index - 1); + Rotation2v theta_n{thetacos.at(index), thetasin.at(index)}; + Rotation2v theta_n_1{thetacos.at(index - 1), thetasin.at(index - 1)}; + auto vx_n = vx.at(index); + auto vx_n_1 = vx.at(index - 1); + auto vy_n = vy.at(index); + auto vy_n_1 = vy.at(index - 1); + auto omega_n = omega.at(index); + auto omega_n_1 = omega.at(index - 1); + auto ax_n = ax.at(index); + auto ay_n = ay.at(index); + auto alpha_n = alpha.at(index); problem.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n); problem.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n); @@ -108,9 +109,9 @@ inline void ApplyKinematicsConstraints( problem.SubjectTo(vy_n_1 + ay_n * dt_sgmt == vy_n); problem.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); } - size_t lastIdx = GetIdx(N, wptIdx, N_sgmt - 1); - problem.SubjectTo(thetacos.at(lastIdx) * thetacos.at(lastIdx) + - thetasin.at(lastIdx) * thetasin.at(lastIdx) == + size_t lastIndex = GetIndex(N, wptIndex, N_sgmt - 1); + problem.SubjectTo(thetacos.at(lastIndex) * thetacos.at(lastIndex) + + thetasin.at(lastIndex) * thetasin.at(lastIndex) == 1); } } @@ -169,21 +170,22 @@ inline void ApplyPowerConstraints(sleipnir::OptimizationProblem& problem, vx_m.reserve(moduleCount); vy_m.reserve(moduleCount); - for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { - const auto& [x_m, y_m] = swerveDrivetrain.modules.at(moduleIdx).translation; + for (size_t moduleIndex = 0; moduleIndex < moduleCount; ++moduleIndex) { + const auto& [x_m, y_m] = + swerveDrivetrain.modules.at(moduleIndex).translation; vx_m.emplace_back(vx_prime - y_m * omega); vy_m.emplace_back(vy_prime + x_m * omega); } - for (size_t moduleIdx = 0; moduleIdx < moduleCount; ++moduleIdx) { - auto& _module = swerveDrivetrain.modules.at(moduleIdx); + for (size_t moduleIndex = 0; moduleIndex < moduleCount; ++moduleIndex) { + auto& _module = swerveDrivetrain.modules.at(moduleIndex); double maxWheelVelocity = _module.wheelRadius * _module.wheelMaxAngularVelocity; double maxForce = _module.wheelMaxTorque / _module.wheelRadius; - auto _vx_m = vx_m.at(moduleIdx); - auto _vy_m = vy_m.at(moduleIdx); - auto Fx_m = Fx.at(moduleIdx); - auto Fy_m = Fy.at(moduleIdx); + auto _vx_m = vx_m.at(moduleIndex); + auto _vy_m = vy_m.at(moduleIndex); + auto Fx_m = Fx.at(moduleIndex); + auto Fy_m = Fy.at(moduleIndex); problem.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <= maxWheelVelocity * maxWheelVelocity); @@ -202,9 +204,9 @@ inline SwerveSolution ConstructSwerveSolution( std::vector>& Fy, std::vector& dt, const std::vector& N) { std::vector dtPerSamp; - for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { - size_t N_sgmt = N.at(sgmtIdx); - sleipnir::Variable dt_sgmt = dt.at(sgmtIdx); + for (size_t sgmtIndex = 0; sgmtIndex < N.size(); ++sgmtIndex) { + size_t N_sgmt = N.at(sgmtIndex); + sleipnir::Variable dt_sgmt = dt.at(sgmtIndex); double dt_val = dt_sgmt.Value(); for (size_t i = 0; i < N_sgmt; ++i) { dtPerSamp.push_back(dt_val); diff --git a/src/optimization/TrajoptUtil.hpp b/src/optimization/TrajoptUtil.hpp index 2f48ec14..4c2aab82 100644 --- a/src/optimization/TrajoptUtil.hpp +++ b/src/optimization/TrajoptUtil.hpp @@ -43,32 +43,32 @@ inline sleipnir::Variable min(const sleipnir::Variable& a, * for the initial sample point ("dt" does not, "x" does). * * @param ctrlIntCnts the control interval counts of each segment, in order - * @param wpIdx the waypoint index (1 + segment index) - * @param sampIdx the sample index within the segment + * @param wpIndex the waypoint index (1 + segment index) + * @param sampIndex the sample index within the segment * @param hasInitialSamp whether this decision variable includes a value for * the initial sample * @return the index in the array */ -inline size_t GetIdx(const std::vector& N, size_t wptIdx, - size_t sampIdx = 0) { - size_t idx = 0; - if (wptIdx > 0) { - ++idx; +inline size_t GetIndex(const std::vector& N, size_t wptIndex, + size_t sampIndex = 0) { + size_t index = 0; + if (wptIndex > 0) { + ++index; } - for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { - idx += N.at(_wptIdx - 1); + for (size_t _wptIndex = 1; _wptIndex < wptIndex; ++_wptIndex) { + index += N.at(_wptIndex - 1); } - idx += sampIdx; - return idx; + index += sampIndex; + return index; } inline void ApplyDiscreteTimeObjective(sleipnir::OptimizationProblem& problem, std::vector& dt, const std::vector N) { sleipnir::Variable T_tot = 0; - for (size_t sgmtIdx = 0; sgmtIdx < N.size(); ++sgmtIdx) { - auto& dt_sgmt = dt.at(sgmtIdx); - auto N_sgmt = N.at(sgmtIdx); + for (size_t sgmtIndex = 0; sgmtIndex < N.size(); ++sgmtIndex) { + auto& dt_sgmt = dt.at(sgmtIndex); + auto N_sgmt = N.at(sgmtIndex); auto T_sgmt = dt_sgmt * static_cast(N_sgmt); T_tot += T_sgmt; @@ -288,7 +288,7 @@ inline Solution GenerateLinearInitialGuess( const std::vector>& initialGuessPoints, const std::vector controlIntervalCounts) { size_t wptCnt = controlIntervalCounts.size() + 1; - size_t sampTot = GetIdx(controlIntervalCounts, wptCnt, 0); + size_t sampTot = GetIndex(controlIntervalCounts, wptCnt, 0); Solution initialGuess; @@ -309,45 +309,47 @@ inline Solution GenerateLinearInitialGuess( initialGuess.dt.push_back((wptCnt * 5.0) / sampTot); } - for (size_t wptIdx = 1; wptIdx < wptCnt; wptIdx++) { - size_t N_sgmt = controlIntervalCounts.at(wptIdx - 1); - size_t guessPointCount = initialGuessPoints.at(wptIdx).size(); + for (size_t wptIndex = 1; wptIndex < wptCnt; wptIndex++) { + size_t N_sgmt = controlIntervalCounts.at(wptIndex - 1); + size_t guessPointCount = initialGuessPoints.at(wptIndex).size(); size_t N_guessSgmt = N_sgmt / guessPointCount; append_vector( initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx - 1).back().X(), - initialGuessPoints.at(wptIdx).front().X(), N_guessSgmt)); + Linspace(initialGuessPoints.at(wptIndex - 1).back().X(), + initialGuessPoints.at(wptIndex).front().X(), N_guessSgmt)); append_vector( initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx - 1).back().Y(), - initialGuessPoints.at(wptIdx).front().Y(), N_guessSgmt)); + Linspace(initialGuessPoints.at(wptIndex - 1).back().Y(), + initialGuessPoints.at(wptIndex).front().Y(), N_guessSgmt)); auto wptThetas = AngleLinspace( - initialGuessPoints.at(wptIdx - 1).back().Rotation().Radians(), - initialGuessPoints.at(wptIdx).front().Rotation().Radians(), + initialGuessPoints.at(wptIndex - 1).back().Rotation().Radians(), + initialGuessPoints.at(wptIndex).front().Rotation().Radians(), N_guessSgmt); for (auto theta : wptThetas) { initialGuess.thetacos.push_back(std::cos(theta)); initialGuess.thetasin.push_back(std::sin(theta)); } - for (size_t guessPointIdx = 1; guessPointIdx < guessPointCount - 1; - guessPointIdx++) { // if three or more guess points + for (size_t guessPointIndex = 1; guessPointIndex < guessPointCount - 1; + guessPointIndex++) { // if three or more guess points append_vector( initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).X(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).X(), + Linspace(initialGuessPoints.at(wptIndex).at(guessPointIndex - 1).X(), + initialGuessPoints.at(wptIndex).at(guessPointIndex).X(), N_guessSgmt)); append_vector( initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).Y(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).Y(), + Linspace(initialGuessPoints.at(wptIndex).at(guessPointIndex - 1).Y(), + initialGuessPoints.at(wptIndex).at(guessPointIndex).Y(), N_guessSgmt)); - auto guessThetas = AngleLinspace( - initialGuessPoints.at(wptIdx) - .at(guessPointIdx - 1) - .Rotation() - .Radians(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).Rotation().Radians(), - N_guessSgmt); + auto guessThetas = AngleLinspace(initialGuessPoints.at(wptIndex) + .at(guessPointIndex - 1) + .Rotation() + .Radians(), + initialGuessPoints.at(wptIndex) + .at(guessPointIndex) + .Rotation() + .Radians(), + N_guessSgmt); for (auto theta : guessThetas) { initialGuess.thetacos.push_back(std::cos(theta)); initialGuess.thetasin.push_back(std::sin(theta)); @@ -357,18 +359,20 @@ inline Solution GenerateLinearInitialGuess( size_t N_lastGuessSgmt = N_sgmt - (guessPointCount - 1) * N_guessSgmt; append_vector( initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).X(), - initialGuessPoints.at(wptIdx).back().X(), N_lastGuessSgmt)); + Linspace(initialGuessPoints.at(wptIndex).at(guessPointCount - 2).X(), + initialGuessPoints.at(wptIndex).back().X(), + N_lastGuessSgmt)); append_vector( initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).Y(), - initialGuessPoints.at(wptIdx).back().Y(), N_lastGuessSgmt)); + Linspace(initialGuessPoints.at(wptIndex).at(guessPointCount - 2).Y(), + initialGuessPoints.at(wptIndex).back().Y(), + N_lastGuessSgmt)); auto lastThetas = AngleLinspace( - initialGuessPoints.at(wptIdx) + initialGuessPoints.at(wptIndex) .at(guessPointCount - 2) .Rotation() .Radians(), - initialGuessPoints.at(wptIdx).back().Rotation().Radians(), + initialGuessPoints.at(wptIndex).back().Rotation().Radians(), N_lastGuessSgmt); for (auto theta : lastThetas) { initialGuess.thetacos.push_back(std::cos(theta)); diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp index 1bfad996..86b0a5e7 100644 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp +++ b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp @@ -51,7 +51,7 @@ class SwerveDiscreteOptimal { }); size_t wptCnt = 1 + N.size(); size_t sgmtCnt = N.size(); - size_t sampTot = GetIdx(N, wptCnt, 0); + size_t sampTot = GetIndex(N, wptCnt, 0); size_t moduleCnt = path.drivetrain.modules.size(); x.reserve(sampTot); @@ -67,7 +67,7 @@ class SwerveDiscreteOptimal { Fx.reserve(sampTot); Fy.reserve(sampTot); - for (size_t sampIdx = 0; sampIdx < sampTot; ++sampIdx) { + for (size_t sampIndex = 0; sampIndex < sampTot; ++sampIndex) { auto& _Fx = Fx.emplace_back(); auto& _Fy = Fy.emplace_back(); _Fx.reserve(moduleCnt); @@ -76,7 +76,7 @@ class SwerveDiscreteOptimal { dt.reserve(sgmtCnt); - for (size_t idx = 0; idx < sampTot; ++idx) { + for (size_t index = 0; index < sampTot; ++index) { x.emplace_back(problem.DecisionVariable()); y.emplace_back(problem.DecisionVariable()); thetacos.emplace_back(problem.DecisionVariable()); @@ -88,9 +88,9 @@ class SwerveDiscreteOptimal { ay.emplace_back(problem.DecisionVariable()); alpha.emplace_back(problem.DecisionVariable()); - for (size_t moduleIdx = 0; moduleIdx < moduleCnt; ++moduleIdx) { - Fx.at(idx).emplace_back(problem.DecisionVariable()); - Fy.at(idx).emplace_back(problem.DecisionVariable()); + for (size_t moduleIndex = 0; moduleIndex < moduleCnt; ++moduleIndex) { + Fx.at(index).emplace_back(problem.DecisionVariable()); + Fy.at(index).emplace_back(problem.DecisionVariable()); } } @@ -112,10 +112,10 @@ class SwerveDiscreteOptimal { } } - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) { dt.emplace_back(problem.DecisionVariable()); for (auto module : path.drivetrain.modules) { - problem.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * + problem.SubjectTo(dt.at(sgmtIndex) * module.wheelRadius * module.wheelMaxAngularVelocity <= minWidth); } @@ -125,44 +125,49 @@ class SwerveDiscreteOptimal { ApplyKinematicsConstraints(problem, x, y, thetacos, thetasin, vx, vy, omega, ax, ay, alpha, dt, N); - for (size_t idx = 0; idx < sampTot; ++idx) { - auto [Fx_net, Fy_net] = SolveNetForce(Fx.at(idx), Fy.at(idx)); + for (size_t index = 0; index < sampTot; ++index) { + auto [Fx_net, Fy_net] = SolveNetForce(Fx.at(index), Fy.at(index)); ApplyDynamicsConstraints( - problem, ax.at(idx), ay.at(idx), alpha.at(idx), Fx_net, Fy_net, - SolveNetTorque({thetacos.at(idx), thetasin.at(idx)}, Fx.at(idx), - Fy.at(idx), path.drivetrain.modules), + problem, ax.at(index), ay.at(index), alpha.at(index), Fx_net, Fy_net, + SolveNetTorque({thetacos.at(index), thetasin.at(index)}, Fx.at(index), + Fy.at(index), path.drivetrain.modules), path.drivetrain.mass, path.drivetrain.moi); ApplyPowerConstraints(problem, - Rotation2v{thetacos.at(idx), thetasin.at(idx)}, - {vx.at(idx), vy.at(idx)}, omega.at(idx), Fx.at(idx), - Fy.at(idx), path.drivetrain); + Rotation2v{thetacos.at(index), thetasin.at(index)}, + {vx.at(index), vy.at(index)}, omega.at(index), + Fx.at(index), Fy.at(index), path.drivetrain); } - for (size_t wptIdx = 0; wptIdx < wptCnt; ++wptIdx) { - for (auto& constraint : path.waypoints.at(wptIdx).waypointConstraints) { - size_t idx = GetIdx(N, wptIdx + 1, 0) - 1; // first idx of next wpt - 1 - ApplyHolonomicConstraint( - problem, - {x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}, - {vx.at(idx), vy.at(idx)}, omega.at(idx), {ax.at(idx), ay.at(idx)}, - alpha.at(idx), constraint); + for (size_t wptIndex = 0; wptIndex < wptCnt; ++wptIndex) { + for (auto& constraint : path.waypoints.at(wptIndex).waypointConstraints) { + size_t index = + GetIndex(N, wptIndex + 1, 0) - 1; // first index of next wpt - 1 + ApplyHolonomicConstraint(problem, + {x.at(index), + y.at(index), + {thetacos.at(index), thetasin.at(index)}}, + {vx.at(index), vy.at(index)}, omega.at(index), + {ax.at(index), ay.at(index)}, alpha.at(index), + constraint); } } // TODO: try changing the path struct so instead of having waypoint objects // it's just two vectors of waypoint constraints and segment // constraints, the waypoint one would be one larger by size - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + for (size_t sgmtIndex = 0; sgmtIndex < sgmtCnt; ++sgmtIndex) { for (auto& constraint : - path.waypoints.at(sgmtIdx + 1).segmentConstraints) { - size_t startIdx = GetIdx(N, sgmtIdx + 1, 0); - size_t endIdx = GetIdx(N, sgmtIdx + 2, 0); - for (size_t idx = startIdx; idx < endIdx; ++idx) { + path.waypoints.at(sgmtIndex + 1).segmentConstraints) { + size_t startIndex = GetIndex(N, sgmtIndex + 1, 0); + size_t endIndex = GetIndex(N, sgmtIndex + 2, 0); + for (size_t index = startIndex; index < endIndex; ++index) { ApplyHolonomicConstraint( problem, - {x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}, - {vx.at(idx), vy.at(idx)}, omega.at(idx), {ax.at(idx), ay.at(idx)}, - alpha.at(idx), constraint); + {x.at(index), + y.at(index), + {thetacos.at(index), thetasin.at(index)}}, + {vx.at(index), vy.at(index)}, omega.at(index), + {ax.at(index), ay.at(index)}, alpha.at(index), constraint); } } } diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index a60356bf..57fee847 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -49,123 +49,126 @@ void SwervePathBuilder::TranslationWpt(size_t index, double x, double y, WptInitialGuessPoint(index, {x, y, {headingGuess}}); } -void SwervePathBuilder::WptInitialGuessPoint(size_t wptIdx, +void SwervePathBuilder::WptInitialGuessPoint(size_t wptIndex, const Pose2d& poseGuess) { - NewWpts(wptIdx); - initialGuessPoints.at(wptIdx).back() = poseGuess; + NewWpts(wptIndex); + initialGuessPoints.at(wptIndex).back() = poseGuess; } void SwervePathBuilder::SgmtInitialGuessPoints( - size_t fromIdx, const std::vector& sgmtPoseGuess) { - NewWpts(fromIdx + 1); + size_t fromIndex, const std::vector& sgmtPoseGuess) { + NewWpts(fromIndex + 1); std::vector& toInitialGuessPoints = - initialGuessPoints.at(fromIdx + 1); + initialGuessPoints.at(fromIndex + 1); toInitialGuessPoints.insert(toInitialGuessPoints.begin(), sgmtPoseGuess.begin(), sgmtPoseGuess.end()); } -void SwervePathBuilder::WptVelocityDirection(size_t idx, double angle) { - WptConstraint(idx, HolonomicVelocityConstraint{LinearSet2d{angle}, - CoordinateSystem::kField}); +void SwervePathBuilder::WptVelocityDirection(size_t index, double angle) { + WptConstraint(index, HolonomicVelocityConstraint{LinearSet2d{angle}, + CoordinateSystem::kField}); } -void SwervePathBuilder::WptVelocityMagnitude(size_t idx, double v) { +void SwervePathBuilder::WptVelocityMagnitude(size_t index, double v) { if (std::abs(v) < 1e-4) { - WptZeroVelocity(idx); + WptZeroVelocity(index); } else { - WptConstraint(idx, + WptConstraint(index, HolonomicVelocityConstraint{EllipticalSet2d::CircularSet2d(v), CoordinateSystem::kField}); } } -void SwervePathBuilder::WptZeroVelocity(size_t idx) { - WptConstraint(idx, HolonomicVelocityConstraint{RectangularSet2d{0.0, 0.0}, - CoordinateSystem::kField}); +void SwervePathBuilder::WptZeroVelocity(size_t index) { + WptConstraint(index, HolonomicVelocityConstraint{RectangularSet2d{0.0, 0.0}, + CoordinateSystem::kField}); } -void SwervePathBuilder::WptVelocityPolar(size_t idx, double vr, double vtheta) { - WptConstraint(idx, HolonomicVelocityConstraint{ - RectangularSet2d::PolarExactSet2d(vr, vtheta), - CoordinateSystem::kField}); +void SwervePathBuilder::WptVelocityPolar(size_t index, double vr, + double vtheta) { + WptConstraint(index, HolonomicVelocityConstraint{ + RectangularSet2d::PolarExactSet2d(vr, vtheta), + CoordinateSystem::kField}); } -void SwervePathBuilder::WptAngularVelocity(size_t idx, +void SwervePathBuilder::WptAngularVelocity(size_t index, double angular_velocity) { - WptConstraint(idx, AngularVelocityConstraint{angular_velocity}); + WptConstraint(index, AngularVelocityConstraint{angular_velocity}); } void SwervePathBuilder::WptAngularVelocityMaxMagnitude( - size_t idx, double angular_velocity) { - WptConstraint(idx, AngularVelocityConstraint{ - IntervalSet1d{-angular_velocity, angular_velocity}}); + size_t index, double angular_velocity) { + WptConstraint(index, AngularVelocityConstraint{ + IntervalSet1d{-angular_velocity, angular_velocity}}); } -void SwervePathBuilder::WptZeroAngularVelocity(size_t idx) { - WptConstraint(idx, AngularVelocityConstraint{0.0}); +void SwervePathBuilder::WptZeroAngularVelocity(size_t index) { + WptConstraint(index, AngularVelocityConstraint{0.0}); } -void SwervePathBuilder::SgmtVelocityDirection(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtVelocityDirection(size_t fromIndex, size_t toIndex, double angle, bool includeWpts) { SgmtConstraint( - fromIdx, toIdx, + fromIndex, toIndex, HolonomicVelocityConstraint{LinearSet2d{angle}, CoordinateSystem::kField}, includeWpts); } -void SwervePathBuilder::SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtVelocityMagnitude(size_t fromIndex, size_t toIndex, double v, bool includeWpts) { Set2d set = EllipticalSet2d{v, v, EllipticalSet2d::Direction::kInside}; if (std::abs(v) < 1e-4) { set = RectangularSet2d{0.0, 0.0}; } - SgmtConstraint(fromIdx, toIdx, + SgmtConstraint(fromIndex, toIndex, HolonomicVelocityConstraint{set, CoordinateSystem::kField}, includeWpts); } -void SwervePathBuilder::SgmtAngularVelocity(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtAngularVelocity(size_t fromIndex, size_t toIndex, double angular_velocity, bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, AngularVelocityConstraint{angular_velocity}, - includeWpts); + SgmtConstraint(fromIndex, toIndex, + AngularVelocityConstraint{angular_velocity}, includeWpts); } -void SwervePathBuilder::SgmtAngularVelocityMaxMagnitude(size_t fromIdx, - size_t toIdx, +void SwervePathBuilder::SgmtAngularVelocityMaxMagnitude(size_t fromIndex, + size_t toIndex, double angular_velocity, bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, + SgmtConstraint(fromIndex, toIndex, AngularVelocityConstraint{ IntervalSet1d{-angular_velocity, angular_velocity}}, includeWpts); } -void SwervePathBuilder::SgmtZeroAngularVelocity(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtZeroAngularVelocity(size_t fromIndex, + size_t toIndex, bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, AngularVelocityConstraint{0.0}, includeWpts); + SgmtConstraint(fromIndex, toIndex, AngularVelocityConstraint{0.0}, + includeWpts); } -void SwervePathBuilder::WptConstraint(size_t idx, +void SwervePathBuilder::WptConstraint(size_t index, const HolonomicConstraint& constraint) { - NewWpts(idx); - path.waypoints.at(idx).waypointConstraints.push_back(constraint); + NewWpts(index); + path.waypoints.at(index).waypointConstraints.push_back(constraint); } -void SwervePathBuilder::SgmtConstraint(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtConstraint(size_t fromIndex, size_t toIndex, const HolonomicConstraint& constraint, bool includeWpts) { - assert(fromIdx < toIdx); + assert(fromIndex < toIndex); - NewWpts(toIdx); + NewWpts(toIndex); if (includeWpts) { - path.waypoints.at(fromIdx).waypointConstraints.push_back(constraint); + path.waypoints.at(fromIndex).waypointConstraints.push_back(constraint); } - for (size_t idx = fromIdx + 1; idx <= toIdx; idx++) { + for (size_t index = fromIndex + 1; index <= toIndex; index++) { if (includeWpts) { - path.waypoints.at(idx).waypointConstraints.push_back(constraint); + path.waypoints.at(index).waypointConstraints.push_back(constraint); } - path.waypoints.at(idx).segmentConstraints.push_back(constraint); + path.waypoints.at(index).segmentConstraints.push_back(constraint); } } @@ -173,22 +176,22 @@ void SwervePathBuilder::AddBumpers(Bumpers&& newBumpers) { bumpers.emplace_back(std::move(newBumpers)); } -void SwervePathBuilder::WptObstacle(size_t idx, const Obstacle& obstacle) { +void SwervePathBuilder::WptObstacle(size_t index, const Obstacle& obstacle) { for (auto& _bumpers : bumpers) { auto constraints = GetConstraintsForObstacle(_bumpers, obstacle); for (auto& constraint : constraints) { - WptConstraint(idx, constraint); + WptConstraint(index, constraint); } } } -void SwervePathBuilder::SgmtObstacle(size_t fromIdx, size_t toIdx, +void SwervePathBuilder::SgmtObstacle(size_t fromIndex, size_t toIndex, const Obstacle& obstacle, bool includeWpts) { for (auto& _bumpers : bumpers) { auto constraints = GetConstraintsForObstacle(_bumpers, obstacle); for (auto& constraint : constraints) { - SgmtConstraint(fromIdx, toIdx, constraint, includeWpts); + SgmtConstraint(fromIndex, toIndex, constraint, includeWpts); } } } @@ -206,10 +209,10 @@ Solution SwervePathBuilder::CalculateInitialGuess() const { } void SwervePathBuilder::NewWpts(size_t finalIndex) { - int64_t targetIdx = finalIndex; - int64_t greatestIdx = path.waypoints.size() - 1; - if (targetIdx > greatestIdx) { - for (int64_t i = greatestIdx + 1; i <= targetIdx; ++i) { + int64_t targetIndex = finalIndex; + int64_t greatestIndex = path.waypoints.size() - 1; + if (targetIndex > greatestIndex) { + for (int64_t i = greatestIndex + 1; i <= targetIndex; ++i) { path.waypoints.emplace_back(SwerveWaypoint{}); initialGuessPoints.emplace_back(std::vector{{0.0, 0.0, {0.0}}}); if (i != 0) { diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index 12b75de7..09b8e289 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -90,153 +90,156 @@ void SwervePathBuilderImpl::set_bumpers(double length, double width) { {+length / 2, -width / 2}}}); } -void SwervePathBuilderImpl::pose_wpt(size_t idx, double x, double y, +void SwervePathBuilderImpl::pose_wpt(size_t index, double x, double y, double heading) { - path.PoseWpt(idx, x, y, heading); + path.PoseWpt(index, x, y, heading); } -void SwervePathBuilderImpl::translation_wpt(size_t idx, double x, double y, +void SwervePathBuilderImpl::translation_wpt(size_t index, double x, double y, double heading_guess) { - path.TranslationWpt(idx, x, y, heading_guess); + path.TranslationWpt(index, x, y, heading_guess); } -void SwervePathBuilderImpl::empty_wpt(size_t idx, double x_guess, +void SwervePathBuilderImpl::empty_wpt(size_t index, double x_guess, double y_guess, double heading_guess) { - path.WptInitialGuessPoint(idx, {x_guess, y_guess, heading_guess}); + path.WptInitialGuessPoint(index, {x_guess, y_guess, heading_guess}); } void SwervePathBuilderImpl::sgmt_initial_guess_points( - size_t from_idx, const rust::Vec& guess_points) { + size_t from_index, const rust::Vec& guess_points) { std::vector convertedGuessPoints = _rust_vec_to_cpp_vector(guess_points); - path.SgmtInitialGuessPoints(from_idx, convertedGuessPoints); + path.SgmtInitialGuessPoints(from_index, convertedGuessPoints); } -void SwervePathBuilderImpl::wpt_linear_velocity_direction(size_t idx, +void SwervePathBuilderImpl::wpt_linear_velocity_direction(size_t index, double angle) { - path.WptVelocityDirection(idx, angle); + path.WptVelocityDirection(index, angle); } void SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( - size_t idx, double magnitude) { - path.WptVelocityMagnitude(idx, magnitude); + size_t index, double magnitude) { + path.WptVelocityMagnitude(index, magnitude); } -void SwervePathBuilderImpl::wpt_linear_velocity_polar(size_t idx, +void SwervePathBuilderImpl::wpt_linear_velocity_polar(size_t index, double magnitude, double angle) { - path.WptVelocityPolar(idx, magnitude, angle); + path.WptVelocityPolar(index, magnitude, angle); } -void SwervePathBuilderImpl::wpt_angular_velocity(size_t idx, +void SwervePathBuilderImpl::wpt_angular_velocity(size_t index, double angular_velocity) { // this probably ought to be added to SwervePathBuilder in the C++ API - path.WptAngularVelocity(idx, angular_velocity); + path.WptAngularVelocity(index, angular_velocity); } void SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude( - size_t idx, double angular_velocity) { - path.WptAngularVelocityMaxMagnitude(idx, angular_velocity); + size_t index, double angular_velocity) { + path.WptAngularVelocityMaxMagnitude(index, angular_velocity); } -void SwervePathBuilderImpl::wpt_x(size_t idx, double x) { - path.WptConstraint(idx, +void SwervePathBuilderImpl::wpt_x(size_t index, double x) { + path.WptConstraint(index, trajopt::TranslationConstraint{trajopt::RectangularSet2d{ .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); } -void SwervePathBuilderImpl::wpt_y(size_t idx, double y) { - path.WptConstraint(idx, +void SwervePathBuilderImpl::wpt_y(size_t index, double y) { + path.WptConstraint(index, trajopt::TranslationConstraint{trajopt::RectangularSet2d{ .xBound = trajopt::IntervalSet1d::R1(), .yBound = y}}); } -void SwervePathBuilderImpl::wpt_heading(size_t idx, double heading) { - path.WptConstraint(idx, trajopt::HeadingConstraint{heading}); +void SwervePathBuilderImpl::wpt_heading(size_t index, double heading) { + path.WptConstraint(index, trajopt::HeadingConstraint{heading}); } -void SwervePathBuilderImpl::wpt_point_at(size_t idx, double field_point_x, +void SwervePathBuilderImpl::wpt_point_at(size_t index, double field_point_x, double field_point_y, double heading_tolerance) { - path.WptConstraint(idx, + path.WptConstraint(index, trajopt::PointAtConstraint{ trajopt::Translation2d{field_point_x, field_point_y}, heading_tolerance}); } -void SwervePathBuilderImpl::sgmt_linear_velocity_direction(size_t from_idx, - size_t to_idx, +void SwervePathBuilderImpl::sgmt_linear_velocity_direction(size_t from_index, + size_t to_index, double angle) { - path.SgmtVelocityDirection(from_idx, to_idx, angle); + path.SgmtVelocityDirection(from_index, to_index, angle); } void SwervePathBuilderImpl::sgmt_linear_velocity_max_magnitude( - size_t from_idx, size_t to_idx, double magnitude) { - path.SgmtVelocityMagnitude(from_idx, to_idx, magnitude); + size_t from_index, size_t to_index, double magnitude) { + path.SgmtVelocityMagnitude(from_index, to_index, magnitude); } -void SwervePathBuilderImpl::sgmt_linear_velocity_polar(size_t from_idx, - size_t to_idx, +void SwervePathBuilderImpl::sgmt_linear_velocity_polar(size_t from_index, + size_t to_index, double magnitude, double angle) { path.SgmtConstraint( - from_idx, to_idx, + from_index, to_index, trajopt::HolonomicVelocityConstraint{ trajopt::RectangularSet2d::PolarExactSet2d(magnitude, angle), trajopt::CoordinateSystem::kField}); } -void SwervePathBuilderImpl::sgmt_angular_velocity(size_t from_idx, - size_t to_idx, +void SwervePathBuilderImpl::sgmt_angular_velocity(size_t from_index, + size_t to_index, double angular_velocity) { - path.SgmtAngularVelocity(from_idx, to_idx, angular_velocity); + path.SgmtAngularVelocity(from_index, to_index, angular_velocity); } void SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude( - size_t from_idx, size_t to_idx, double angular_velocity) { - path.SgmtAngularVelocityMaxMagnitude(from_idx, to_idx, angular_velocity); + size_t from_index, size_t to_index, double angular_velocity) { + path.SgmtAngularVelocityMaxMagnitude(from_index, to_index, angular_velocity); } -void SwervePathBuilderImpl::sgmt_x(size_t from_idx, size_t to_idx, double x) { +void SwervePathBuilderImpl::sgmt_x(size_t from_index, size_t to_index, + double x) { path.SgmtConstraint( - from_idx, to_idx, + from_index, to_index, trajopt::TranslationConstraint{trajopt::RectangularSet2d{ .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); } -void SwervePathBuilderImpl::sgmt_y(size_t from_idx, size_t to_idx, double y) { +void SwervePathBuilderImpl::sgmt_y(size_t from_index, size_t to_index, + double y) { path.SgmtConstraint( - from_idx, to_idx, + from_index, to_index, trajopt::TranslationConstraint{trajopt::RectangularSet2d{ .xBound = trajopt::IntervalSet1d::R1(), .yBound = y}}); } -void SwervePathBuilderImpl::sgmt_heading(size_t from_idx, size_t to_idx, +void SwervePathBuilderImpl::sgmt_heading(size_t from_index, size_t to_index, double heading) { - path.SgmtConstraint(from_idx, to_idx, trajopt::HeadingConstraint{heading}); + path.SgmtConstraint(from_index, to_index, + trajopt::HeadingConstraint{heading}); } -void SwervePathBuilderImpl::sgmt_point_at(size_t from_idx, size_t to_idx, +void SwervePathBuilderImpl::sgmt_point_at(size_t from_index, size_t to_index, double field_point_x, double field_point_y, double heading_tolerance) { - path.SgmtConstraint(from_idx, to_idx, + path.SgmtConstraint(from_index, to_index, trajopt::PointAtConstraint{ trajopt::Translation2d{field_point_x, field_point_y}, heading_tolerance}); } -void SwervePathBuilderImpl::sgmt_circle_obstacle(size_t from_idx, size_t to_idx, - double x, double y, - double radius) { +void SwervePathBuilderImpl::sgmt_circle_obstacle(size_t from_index, + size_t to_index, double x, + double y, double radius) { auto obstacle = trajopt::Obstacle{.safetyDistance = radius, .points = {{x, y}}}; - path.SgmtObstacle(from_idx, to_idx, obstacle); + path.SgmtObstacle(from_index, to_index, obstacle); } -void SwervePathBuilderImpl::sgmt_polygon_obstacle(size_t from_idx, - size_t to_idx, +void SwervePathBuilderImpl::sgmt_polygon_obstacle(size_t from_index, + size_t to_index, const rust::Vec x, const rust::Vec y, double radius) { @@ -248,7 +251,7 @@ void SwervePathBuilderImpl::sgmt_polygon_obstacle(size_t from_idx, points.push_back({x.at(i), y.at(i)}); } auto obstacle = trajopt::Obstacle{.safetyDistance = radius, .points = points}; - path.SgmtObstacle(from_idx, to_idx, obstacle); + path.SgmtObstacle(from_index, to_index, obstacle); } HolonomicTrajectorySample _convert_holonomic_trajectory_sample( diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp index 73005112..9bc55c02 100644 --- a/src/trajoptlibrust.hpp +++ b/src/trajoptlibrust.hpp @@ -21,44 +21,45 @@ class SwervePathBuilderImpl { void set_bumpers(double length, double width); void set_control_interval_counts(const rust::Vec counts); - void pose_wpt(size_t idx, double x, double y, double heading); - void translation_wpt(size_t idx, double x, double y, double heading_guess); - void empty_wpt(size_t idx, double x_guess, double y_guess, + void pose_wpt(size_t index, double x, double y, double heading); + void translation_wpt(size_t index, double x, double y, double heading_guess); + void empty_wpt(size_t index, double x_guess, double y_guess, double heading_guess); void sgmt_initial_guess_points( - size_t from_idx, const rust::Vec& guess_points); - - void wpt_linear_velocity_direction(size_t idx, double angle); - void wpt_linear_velocity_max_magnitude(size_t idx, double magnitude); - void wpt_linear_velocity_polar(size_t idx, double magnitude, double angle); - void wpt_angular_velocity(size_t idx, double angular_velocity); - void wpt_angular_velocity_max_magnitude(size_t idx, double angular_velocity); - void wpt_x(size_t idx, double x); - void wpt_y(size_t idx, double y); - void wpt_heading(size_t idx, double heading); - void wpt_point_at(size_t idx, double field_point_x, double field_point_y, + size_t from_index, const rust::Vec& guess_points); + + void wpt_linear_velocity_direction(size_t index, double angle); + void wpt_linear_velocity_max_magnitude(size_t index, double magnitude); + void wpt_linear_velocity_polar(size_t index, double magnitude, double angle); + void wpt_angular_velocity(size_t index, double angular_velocity); + void wpt_angular_velocity_max_magnitude(size_t index, + double angular_velocity); + void wpt_x(size_t index, double x); + void wpt_y(size_t index, double y); + void wpt_heading(size_t index, double heading); + void wpt_point_at(size_t index, double field_point_x, double field_point_y, double heading_tolerance); - void sgmt_linear_velocity_direction(size_t from_idx, size_t to_idx, + void sgmt_linear_velocity_direction(size_t from_index, size_t to_index, double angle); - void sgmt_linear_velocity_max_magnitude(size_t from_idx, size_t to_idx, + void sgmt_linear_velocity_max_magnitude(size_t from_index, size_t to_index, double magnitude); - void sgmt_linear_velocity_polar(size_t from_idx, size_t to_idx, + void sgmt_linear_velocity_polar(size_t from_index, size_t to_index, double magnitude, double angle); - void sgmt_angular_velocity(size_t from_idx, size_t to_idx, + void sgmt_angular_velocity(size_t from_index, size_t to_index, double angular_velocity); - void sgmt_angular_velocity_max_magnitude(size_t from_idx, size_t to_idx, + void sgmt_angular_velocity_max_magnitude(size_t from_index, size_t to_index, double angular_velocity); - void sgmt_x(size_t from_idx, size_t to_idx, double x); - void sgmt_y(size_t from_idx, size_t to_idx, double y); - void sgmt_heading(size_t from_idx, size_t to_idx, double heading); - void sgmt_point_at(size_t from_idx, size_t to_idx, double field_point_x, + void sgmt_x(size_t from_index, size_t to_index, double x); + void sgmt_y(size_t from_index, size_t to_index, double y); + void sgmt_heading(size_t from_index, size_t to_index, double heading); + void sgmt_point_at(size_t from_index, size_t to_index, double field_point_x, double field_point_y, double heading_tolerance); - 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, + void sgmt_circle_obstacle(size_t from_index, size_t to_index, double x, + double y, double radius); + void sgmt_polygon_obstacle(size_t from_index, size_t to_index, rust::Vec x, rust::Vec y, double radius); // TODO: Return std::expected instead of diff --git a/test/src/TrajoptUtilTest.cpp b/test/src/TrajoptUtilTest.cpp index a6cab965..f26be898 100644 --- a/test/src/TrajoptUtilTest.cpp +++ b/test/src/TrajoptUtilTest.cpp @@ -7,11 +7,11 @@ #include "optimization/TrajoptUtil.hpp" -TEST_CASE("TrajoptUtil - GetIdx()", "[TrajoptUtil]") { - auto result0 = trajopt::GetIdx({2, 3}, 0, 0); - auto result1 = trajopt::GetIdx({2, 3}, 1, 1); - auto result2 = trajopt::GetIdx({2, 3}, 2, 2); - auto result3 = trajopt::GetIdx({2, 3}, 3, 0); +TEST_CASE("TrajoptUtil - GetIndex()", "[TrajoptUtil]") { + auto result0 = trajopt::GetIndex({2, 3}, 0, 0); + auto result1 = trajopt::GetIndex({2, 3}, 1, 1); + auto result2 = trajopt::GetIndex({2, 3}, 2, 2); + auto result3 = trajopt::GetIndex({2, 3}, 3, 0); CHECK(result0 == 0); CHECK(result1 == 2); CHECK(result2 == 5);