From 58c2347963a16d2d4c8c5b520cc28de7536a09d8 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 22 Jun 2024 16:16:14 -0700 Subject: [PATCH] Rewrite trajectory generator and constraint handling OptimalTrajectoryGenerator and SwerveDiscreteOptimal were merged into a new class SwerveTrajectoryGenerator. The constraints were rewritten based on the SwervePathBuilder interface. The geometry classes now overload operator== to produce equality constraints. --- build.rs | 1 + examples/Swerve/src/Main.cpp | 8 +- examples/swerve.rs | 2 - .../trajopt/OptimalTrajectoryGenerator.hpp | 38 -- include/trajopt/SwerveTrajectoryGenerator.hpp | 80 ++++ .../constraint/AngularVelocityConstraint.hpp | 18 - .../AngularVelocityEqualityConstraint.hpp | 45 ++ .../AngularVelocityMaxMagnitudeConstraint.hpp | 55 +++ include/trajopt/constraint/Constraint.hpp | 58 ++- .../trajopt/constraint/HeadingConstraint.hpp | 18 - .../constraint/LinePointConstraint.hpp | 63 ++- .../LinearVelocityDirectionConstraint.hpp | 45 ++ .../LinearVelocityMaxMagnitudeConstraint.hpp | 56 +++ .../trajopt/constraint/PointAtConstraint.hpp | 55 ++- .../constraint/PointLineConstraint.hpp | 66 ++- .../constraint/PointPointConstraint.hpp | 57 ++- .../constraint/PoseEqualityConstraint.hpp | 46 ++ .../constraint/TranslationConstraint.hpp | 18 - .../TranslationEqualityConstraint.hpp | 44 ++ .../constraint/detail/LinePointDistance.hpp | 37 ++ ...ntialCentripetalAccelerationConstraint.hpp | 18 - .../differential/DifferentialConstraint.hpp | 18 - ...fferentialTangentialVelocityConstraint.hpp | 18 - .../holonomic/HolonomicConstraint.hpp | 17 - .../holonomic/HolonomicVelocityConstraint.hpp | 22 - include/trajopt/geometry/Pose2.hpp | 10 + include/trajopt/geometry/Rotation2.hpp | 26 ++ include/trajopt/geometry/Translation2.hpp | 35 ++ include/trajopt/path/Path.hpp | 44 +- include/trajopt/path/SwervePathBuilder.hpp | 229 +++------ include/trajopt/set/ConeSet2d.hpp | 32 -- include/trajopt/set/EllipticalSet2d.hpp | 60 --- include/trajopt/set/IntervalSet1d.hpp | 117 ----- include/trajopt/set/LinearSet2d.hpp | 17 - include/trajopt/set/ManifoldIntervalSet2d.hpp | 44 -- include/trajopt/set/RectangularSet2d.hpp | 40 -- include/trajopt/set/Set2d.hpp | 39 -- include/trajopt/set/setnotation.txt | 110 ----- .../trajopt/solution/DifferentialSolution.hpp | 25 +- .../trajopt/solution/HolonomicSolution.hpp | 35 -- include/trajopt/solution/Solution.hpp | 30 -- include/trajopt/solution/SwerveSolution.hpp | 36 +- .../trajectory/HolonomicTrajectory.hpp | 20 +- .../trajopt/util}/Cancellation.hpp | 4 +- .../util/GenerateLinearInitialGuess.hpp | 167 +++++++ include/trajopt/util/VariantCat.hpp | 32 -- include/trajopt/{ => util}/expected | 0 src/OptimalTrajectoryGenerator.cpp | 16 - src/SwerveTrajectoryGenerator.cpp | 384 +++++++++++++++ src/lib.rs | 82 +--- src/optimization/HolonomicTrajoptUtil.hpp | 62 --- src/optimization/SwerveTrajoptUtil.hpp | 226 --------- src/optimization/TrajoptUtil.hpp | 440 ------------------ .../algorithms/SwerveDiscreteOptimal.hpp | 238 ---------- src/path/SwervePathBuilder.cpp | 359 ++++++-------- src/trajoptlibrust.cpp | 145 +++--- src/trajoptlibrust.hpp | 53 +-- src/{optimization => util}/Cancellation.cpp | 4 +- test/src/ObstacleTest.cpp | 5 +- test/src/SwerveTrajoptUtilTest.cpp | 38 -- test/src/TrajoptUtilTest.cpp | 35 -- test/src/geometry/Translation2dTest.cpp | 16 + 62 files changed, 1637 insertions(+), 2521 deletions(-) delete mode 100644 include/trajopt/OptimalTrajectoryGenerator.hpp create mode 100644 include/trajopt/SwerveTrajectoryGenerator.hpp delete mode 100644 include/trajopt/constraint/AngularVelocityConstraint.hpp create mode 100644 include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp create mode 100644 include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp delete mode 100644 include/trajopt/constraint/HeadingConstraint.hpp create mode 100644 include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp create mode 100644 include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp create mode 100644 include/trajopt/constraint/PoseEqualityConstraint.hpp delete mode 100644 include/trajopt/constraint/TranslationConstraint.hpp create mode 100644 include/trajopt/constraint/TranslationEqualityConstraint.hpp create mode 100644 include/trajopt/constraint/detail/LinePointDistance.hpp delete mode 100644 include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.hpp delete mode 100644 include/trajopt/constraint/differential/DifferentialConstraint.hpp delete mode 100644 include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.hpp delete mode 100644 include/trajopt/constraint/holonomic/HolonomicConstraint.hpp delete mode 100644 include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp delete mode 100644 include/trajopt/set/ConeSet2d.hpp delete mode 100644 include/trajopt/set/EllipticalSet2d.hpp delete mode 100644 include/trajopt/set/IntervalSet1d.hpp delete mode 100644 include/trajopt/set/LinearSet2d.hpp delete mode 100644 include/trajopt/set/ManifoldIntervalSet2d.hpp delete mode 100644 include/trajopt/set/RectangularSet2d.hpp delete mode 100644 include/trajopt/set/Set2d.hpp delete mode 100644 include/trajopt/set/setnotation.txt delete mode 100644 include/trajopt/solution/HolonomicSolution.hpp delete mode 100644 include/trajopt/solution/Solution.hpp rename {src/optimization => include/trajopt/util}/Cancellation.hpp (54%) create mode 100644 include/trajopt/util/GenerateLinearInitialGuess.hpp delete mode 100644 include/trajopt/util/VariantCat.hpp rename include/trajopt/{ => util}/expected (100%) delete mode 100644 src/OptimalTrajectoryGenerator.cpp create mode 100644 src/SwerveTrajectoryGenerator.cpp delete mode 100644 src/optimization/HolonomicTrajoptUtil.hpp delete mode 100644 src/optimization/SwerveTrajoptUtil.hpp delete mode 100644 src/optimization/TrajoptUtil.hpp delete mode 100644 src/optimization/algorithms/SwerveDiscreteOptimal.hpp rename src/{optimization => util}/Cancellation.cpp (75%) delete mode 100644 test/src/SwerveTrajoptUtilTest.cpp delete mode 100644 test/src/TrajoptUtilTest.cpp diff --git a/build.rs b/build.rs index 42fd9985..bd8ed72f 100644 --- a/build.rs +++ b/build.rs @@ -23,6 +23,7 @@ fn main() { .file("src/trajoptlibrust.cpp") .include("src") .include(format!("{}/include", cmake_dest.display())) + .include(format!("{}/include/eigen3", cmake_dest.display())) .std("c++20"); bridge_build.compile("trajoptrust"); diff --git a/examples/Swerve/src/Main.cpp b/examples/Swerve/src/Main.cpp index 93074931..92160969 100644 --- a/examples/Swerve/src/Main.cpp +++ b/examples/Swerve/src/Main.cpp @@ -1,6 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include +#include int main() { trajopt::SwerveDrivetrain swerveDrivetrain{ @@ -19,11 +19,11 @@ int main() { path.SetDrivetrain(swerveDrivetrain); path.PoseWpt(0, 0.0, 0.0, 0.0); path.PoseWpt(1, 5.0, 0.0, 0.0); - path.WptZeroVelocity(0); - path.WptZeroVelocity(1); path.ControlIntervalCounts({4}); + trajopt::SwerveTrajectoryGenerator generator{path}; + // SOLVE [[maybe_unused]] - auto solution = trajopt::OptimalTrajectoryGenerator::Generate(path, true); + auto solution = generator.Generate(true); } diff --git a/examples/swerve.rs b/examples/swerve.rs index ae0cb119..71bc8eac 100644 --- a/examples/swerve.rs +++ b/examples/swerve.rs @@ -42,8 +42,6 @@ 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_linear_velocity_polar(0, 0.0, 0.0); - // path.wpt_linear_velocity_polar(1, 0.0, 0.0); // path.wpt_angular_velocity(0, 0.0); // path.wpt_angular_velocity(1, 0.0); // path.sgmt_circle_obstacle(0, 1, 0.5, 0.1, 0.2); diff --git a/include/trajopt/OptimalTrajectoryGenerator.hpp b/include/trajopt/OptimalTrajectoryGenerator.hpp deleted file mode 100644 index 606191ca..00000000 --- a/include/trajopt/OptimalTrajectoryGenerator.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include - -#include "trajopt/expected" -#include "trajopt/path/SwervePathBuilder.hpp" -#include "trajopt/solution/SwerveSolution.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * @brief This trajectory generator class contains functions to generate - * time-optimal trajectories for several drivetrain types. - */ -class TRAJOPT_DLLEXPORT OptimalTrajectoryGenerator { - public: - OptimalTrajectoryGenerator() = delete; - - /** - * Initializes and solves an optimization problem for a swerve drivetrain. - * - * @param path the path - * @param diagnostics Enables diagnostic prints. - * @param handle an identifier for state callbacks - * @return The optimized swerve trajectory solution on success, or a string - * containing the failure reason. - */ - static expected Generate( - const SwervePathBuilder& path, bool diagnostics = false, - int64_t handle = 0); -}; - -} // namespace trajopt diff --git a/include/trajopt/SwerveTrajectoryGenerator.hpp b/include/trajopt/SwerveTrajectoryGenerator.hpp new file mode 100644 index 00000000..4e31baf5 --- /dev/null +++ b/include/trajopt/SwerveTrajectoryGenerator.hpp @@ -0,0 +1,80 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include +#include +#include + +#include + +#include "trajopt/path/SwervePathBuilder.hpp" +#include "trajopt/solution/SwerveSolution.hpp" +#include "trajopt/util/SymbolExports.hpp" +#include "trajopt/util/expected" + +namespace trajopt { + +/** + * This trajectory generator class contains functions to generate + * time-optimal trajectories for several drivetrain types. + */ +class TRAJOPT_DLLEXPORT SwerveTrajectoryGenerator { + public: + /** + * Construct a new swerve trajectory optimization problem. + * + * @param pathBuilder The path builder. + * @param handle An identifier for state callbacks. + */ + explicit SwerveTrajectoryGenerator(SwervePathBuilder pathBuilder, + int64_t handle = 0); + + /** + * Generates an optimal trajectory. + * + * This function may take a long time to complete. + * + * @param diagnostics Enables diagnostic prints. + * @return Returns a holonomic trajectory on success, or a string containing a + * failure reason. + */ + expected Generate(bool diagnostics = false); + + private: + /// Swerve path + SwervePath path; + + /// State Variables + std::vector x; + std::vector y; + std::vector thetacos; + std::vector thetasin; + std::vector vx; + std::vector vy; + std::vector omega; + std::vector ax; + std::vector ay; + std::vector alpha; + + /// Input Variables + std::vector> Fx; + std::vector> Fy; + + /// Time Variables + std::vector dt; + + /// Discretization Constants + std::vector N; + + sleipnir::OptimizationProblem problem; + std::vector> callbacks; + + void ApplyInitialGuess(const SwerveSolution& solution); + + SwerveSolution ConstructSwerveSolution(); +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/AngularVelocityConstraint.hpp b/include/trajopt/constraint/AngularVelocityConstraint.hpp deleted file mode 100644 index 053274e0..00000000 --- a/include/trajopt/constraint/AngularVelocityConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Angular velocity constraint. - */ -struct TRAJOPT_DLLEXPORT AngularVelocityConstraint { - /// The angular velocity bounds (rad/s) - IntervalSet1d angularVelocityBound; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp b/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp new file mode 100644 index 00000000..3c9fb21a --- /dev/null +++ b/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp @@ -0,0 +1,45 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#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. + */ + void Apply(sleipnir::OptimizationProblem& problem, + [[maybe_unused]] const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + const sleipnir::Variable& angularVelocity) { + problem.SubjectTo(angularVelocity == m_angularVelocity); + } + + private: + double m_angularVelocity; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp new file mode 100644 index 00000000..41a3b869 --- /dev/null +++ b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp @@ -0,0 +1,55 @@ +// 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 max magnitude inequality constraint. + */ +class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint { + public: + /** + * Constructs an AngularVelocityMaxMagnitudeConstraint. + * + * @param maxMagnitude The maximum angular velocity magnitude. Must be + * nonnegative. + */ + explicit AngularVelocityMaxMagnitudeConstraint(double maxMagnitude) + : m_maxMagnitude{maxMagnitude} { + assert(maxMagnitude >= 0.0); + } + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, + [[maybe_unused]] const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + const sleipnir::Variable& angularVelocity) { + if (m_maxMagnitude == 0.0) { + problem.SubjectTo(angularVelocity == 0.0); + } else { + problem.SubjectTo(angularVelocity >= -m_maxMagnitude); + problem.SubjectTo(angularVelocity <= m_maxMagnitude); + } + } + + private: + double m_maxMagnitude; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/Constraint.hpp b/include/trajopt/constraint/Constraint.hpp index 3f81de88..eeb07fdd 100644 --- a/include/trajopt/constraint/Constraint.hpp +++ b/include/trajopt/constraint/Constraint.hpp @@ -2,50 +2,44 @@ #pragma once +#include #include -#include "trajopt/constraint/HeadingConstraint.hpp" +#include + +#include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp" +#include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp" #include "trajopt/constraint/LinePointConstraint.hpp" +#include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp" +#include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp" +#include "trajopt/constraint/PointAtConstraint.hpp" #include "trajopt/constraint/PointLineConstraint.hpp" #include "trajopt/constraint/PointPointConstraint.hpp" -#include "trajopt/constraint/TranslationConstraint.hpp" +#include "trajopt/constraint/PoseEqualityConstraint.hpp" +#include "trajopt/constraint/TranslationEqualityConstraint.hpp" +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/geometry/Translation2.hpp" namespace trajopt { /** - * @brief Enumerates the various coordinate systems used in trajectory - * optimization. The field coordinate system is the base system that is fixed to - * the field with some arbitrary center. Other coordinate systems defined do not - * have a relative velocity to the field coordinate system, but their position - * and orientation depends on the current position of the robot. For example, if - * the robot is spinning, then the robot has angular velocity relative to all - * the systems defined here, and the magnitude of the robot's velocity is the - * same in all the systems but the direction varies. - * - * We define these other systems: the robot coordinate system, the - * robot nonrotating coordinate system, and the robot velocity coordinate - * system. The robot coordinate system is centered on and oriented with the - * front of the robot towards the x-axis. The robot nonrotating coordinate - * system is centered on the robot but is oriented with the field. The robot - * velocity coordinate system is centered on the robot and it is oriented with - * the robot's velocity in the x-direction. - * - * Note that in differential drivetrains, the robot coordinate system - * is equivalent ot the robot velocity coordinate system. + * ConstraintType concept. */ -enum class CoordinateSystem { - /** - * @brief the coordinate system of the field - */ - kField, - /** - * @brief the coordinate system of the robot - */ - kRobot, +template +concept ConstraintType = requires(T t) { + { + t.Apply(std::declval(), + std::declval(), std::declval(), + std::declval()) + }; // NOLINT(readability/braces) }; using Constraint = - std::variant; + std::variant; } // namespace trajopt diff --git a/include/trajopt/constraint/HeadingConstraint.hpp b/include/trajopt/constraint/HeadingConstraint.hpp deleted file mode 100644 index 617b0564..00000000 --- a/include/trajopt/constraint/HeadingConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/set/ManifoldIntervalSet2d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Heading constraint. - */ -struct TRAJOPT_DLLEXPORT HeadingConstraint { - /// The heading bound (radians) - ManifoldIntervalSet2d headingBound; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/LinePointConstraint.hpp b/include/trajopt/constraint/LinePointConstraint.hpp index 36545c74..744d89e3 100644 --- a/include/trajopt/constraint/LinePointConstraint.hpp +++ b/include/trajopt/constraint/LinePointConstraint.hpp @@ -2,28 +2,69 @@ #pragma once +#include +#include + +#include + +#include "trajopt/constraint/detail/LinePointDistance.hpp" +#include "trajopt/geometry/Pose2.hpp" #include "trajopt/geometry/Translation2.hpp" -#include "trajopt/set/IntervalSet1d.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { /** + * Line-point constraint. + * * Specifies the required minimum distance between a line segment on the * robot's frame and a point on the field. */ -struct TRAJOPT_DLLEXPORT LinePointConstraint { - /// Robot line start (meters). - Translation2d robotLineStart; - - /// Robot line end (meters). - Translation2d robotLineEnd; +class TRAJOPT_DLLEXPORT LinePointConstraint { + public: + /** + * Constructs a LinePointConstraint. + * + * @param robotLineStart Robot line start. + * @param robotLineEnd Robot line end. + * @param fieldPoint Field point. + * @param minDistance Minimum distance between robot line and field point. + * Must be nonnegative. + */ + explicit LinePointConstraint(Translation2d robotLineStart, + Translation2d robotLineEnd, + Translation2d fieldPoint, double minDistance) + : m_robotLineStart{std::move(robotLineStart)}, + m_robotLineEnd{std::move(robotLineEnd)}, + m_fieldPoint{std::move(fieldPoint)}, + m_minDistance{minDistance} { + assert(minDistance >= 0.0); + } - /// Field point (meters). - Translation2d fieldPoint; + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + auto lineStart = + pose.Translation() + m_robotLineStart.RotateBy(pose.Rotation()); + auto lineEnd = + pose.Translation() + m_robotLineEnd.RotateBy(pose.Rotation()); + auto dist = detail::LinePointDistance(lineStart, lineEnd, m_fieldPoint); + problem.SubjectTo(dist * dist >= m_minDistance * m_minDistance); + } - /// The allowed distances (meters) between the line segment and point. - IntervalSet1d distance; + private: + Translation2d m_robotLineStart; + Translation2d m_robotLineEnd; + Translation2d m_fieldPoint; + double m_minDistance; }; } // namespace trajopt diff --git a/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp b/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp new file mode 100644 index 00000000..34683583 --- /dev/null +++ b/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp @@ -0,0 +1,45 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/geometry/Translation2.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +/** + * Linear velocity direction equality constraint. + */ +class TRAJOPT_DLLEXPORT LinearVelocityDirectionConstraint { + public: + /** + * Constructs a LinearVelocityDirectionConstraint. + * + * @param angle The angle (radians). + */ + explicit LinearVelocityDirectionConstraint(double angle) : m_angle{angle} {} + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, + [[maybe_unused]] const Pose2v& pose, + const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + problem.SubjectTo(Rotation2v{linearVelocity.X(), linearVelocity.Y()} == + m_angle); + } + + private: + trajopt::Rotation2d m_angle; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp new file mode 100644 index 00000000..8ce14bc5 --- /dev/null +++ b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp @@ -0,0 +1,56 @@ +// 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 { + +/** + * Linear velocity max magnitude inequality constraint. + */ +class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint { + public: + /** + * Constructs a LinearVelocityMaxMagnitudeConstraint. + * + * @param maxMagnitude The maximum linear velocity magnitude. Must be + * nonnegative. + */ + explicit LinearVelocityMaxMagnitudeConstraint(double maxMagnitude) + : m_maxMagnitude{maxMagnitude} { + assert(maxMagnitude >= 0.0); + } + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, + [[maybe_unused]] const Pose2v& pose, + const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + if (m_maxMagnitude == 0.0) { + problem.SubjectTo(linearVelocity.X() == 0.0); + problem.SubjectTo(linearVelocity.Y() == 0.0); + } else { + problem.SubjectTo(linearVelocity.SquaredNorm() <= + m_maxMagnitude * m_maxMagnitude); + } + } + + private: + double m_maxMagnitude; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/PointAtConstraint.hpp b/include/trajopt/constraint/PointAtConstraint.hpp index e9118be3..c2b2801a 100644 --- a/include/trajopt/constraint/PointAtConstraint.hpp +++ b/include/trajopt/constraint/PointAtConstraint.hpp @@ -2,20 +2,65 @@ #pragma once +#include +#include + +#include + +#include "trajopt/geometry/Pose2.hpp" #include "trajopt/geometry/Translation2.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { /** + * Point-at constraint. + * * Specifies a point on the field at which the robot should point. */ -struct TRAJOPT_DLLEXPORT PointAtConstraint { - /// Field point (meters). - Translation2d fieldPoint; +class TRAJOPT_DLLEXPORT PointAtConstraint { + public: + /** + * Cosntructs a PointAtConstraint. + * + * @param fieldPoint Field point. + * @param headingTolerance The allowed robot heading tolerance (radians). Must + * be nonnegative. + */ + explicit PointAtConstraint(Translation2d fieldPoint, double headingTolerance) + : m_fieldPoint{std::move(fieldPoint)}, + m_headingTolerance{headingTolerance} { + assert(m_headingTolerance >= 0.0); + } + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + // dx,dy = desired heading + // ux,uy = unit vector of desired heading + // hx,hy = heading + // dot = dot product of ux,uy and hx,hy + // + // constrain dot to cos(1.0), which is colinear + // and cos(thetaTolerance) + auto dx = m_fieldPoint.X() - pose.X(); + auto dy = m_fieldPoint.Y() - pose.Y(); + auto dot = pose.Rotation().Cos() * dx + pose.Rotation().Sin() * dy; + problem.SubjectTo(dot >= + std::cos(m_headingTolerance) * sleipnir::hypot(dx, dy)); + } - /// The allowed robot heading tolerance (radians), must be positive. - double headingTolerance; + private: + Translation2d m_fieldPoint; + double m_headingTolerance; }; } // namespace trajopt diff --git a/include/trajopt/constraint/PointLineConstraint.hpp b/include/trajopt/constraint/PointLineConstraint.hpp index d3d6e7ea..4cd5c7d3 100644 --- a/include/trajopt/constraint/PointLineConstraint.hpp +++ b/include/trajopt/constraint/PointLineConstraint.hpp @@ -2,29 +2,67 @@ #pragma once +#include +#include + +#include + +#include "trajopt/constraint/detail/LinePointDistance.hpp" +#include "trajopt/geometry/Pose2.hpp" #include "trajopt/geometry/Translation2.hpp" -#include "trajopt/set/IntervalSet1d.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { /** - * Specifies the required minimum distance between a point on the robot's - * frame and a line segment on the field. + * Point-line constraint. + * + * Specifies the required minimum distance between a point on the robot's frame + * and a line segment on the field. */ -struct TRAJOPT_DLLEXPORT PointLineConstraint { - /// Robot point (meters). - Translation2d robotPoint; - - /// Field line start (meters). - Translation2d fieldLineStart; +class TRAJOPT_DLLEXPORT PointLineConstraint { + public: + /** + * Constructs a PointLineConstraint. + * + * @param robotPoint Robot point. + * @param fieldLineStart Field line start. + * @param fieldLineEnd Field line end. + * @param minDistance Minimum distance between robot point and field line. + * Must be nonnegative. + */ + explicit PointLineConstraint(Translation2d robotPoint, + Translation2d fieldLineStart, + Translation2d fieldLineEnd, double minDistance) + : m_robotPoint{std::move(robotPoint)}, + m_fieldLineStart{std::move(fieldLineStart)}, + m_fieldLineEnd{std::move(fieldLineEnd)}, + m_minDistance{minDistance} { + assert(minDistance >= 0.0); + } - /// Field line end (meters). - Translation2d fieldLineEnd; + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + auto point = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); + auto dist = + detail::LinePointDistance(m_fieldLineStart, m_fieldLineEnd, point); + problem.SubjectTo(dist * dist >= m_minDistance * m_minDistance); + } - /// The required minimum distance (meters) between the point and line. Must be - /// positive. - IntervalSet1d distance; + private: + Translation2d m_robotPoint; + Translation2d m_fieldLineStart; + Translation2d m_fieldLineEnd; + double m_minDistance; }; } // namespace trajopt diff --git a/include/trajopt/constraint/PointPointConstraint.hpp b/include/trajopt/constraint/PointPointConstraint.hpp index a1029828..81506356 100644 --- a/include/trajopt/constraint/PointPointConstraint.hpp +++ b/include/trajopt/constraint/PointPointConstraint.hpp @@ -2,26 +2,63 @@ #pragma once +#include +#include + +#include + +#include "trajopt/geometry/Pose2.hpp" #include "trajopt/geometry/Translation2.hpp" -#include "trajopt/set/IntervalSet1d.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { /** - * Specifies the required distance between a point on the robot's frame + * Point-point constraint. + * + * Specifies the required minimum distance between a point on the robot's frame * and a point on the field. */ -struct TRAJOPT_DLLEXPORT PointPointConstraint { - /// Robot point (meters). - Translation2d robotPoint; +class TRAJOPT_DLLEXPORT PointPointConstraint { + public: + /** + * Constructs a LinePointConstraint. + * + * @param robotPoint Robot point. + * @param fieldPoint Field point. + * @param minDistance Minimum distance between robot line and field point. + * Must be nonnegative. + */ + explicit PointPointConstraint(Translation2d robotPoint, + Translation2d fieldPoint, double minDistance) + : m_robotPoint{std::move(robotPoint)}, + m_fieldPoint{std::move(fieldPoint)}, + m_minDistance{minDistance} { + assert(minDistance >= 0.0); + } - /// Field point (meters). - Translation2d fieldPoint; + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + auto bumperCorner = + pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); + auto dx = m_fieldPoint.X() - bumperCorner.X(); + auto dy = m_fieldPoint.Y() - bumperCorner.Y(); + problem.SubjectTo(dx * dx + dy * dy >= m_minDistance * m_minDistance); + } - /// The required distance (meters) between the robot point and field point. - /// Must be positive. - IntervalSet1d distance; + private: + Translation2d m_robotPoint; + Translation2d m_fieldPoint; + double m_minDistance; }; } // namespace trajopt diff --git a/include/trajopt/constraint/PoseEqualityConstraint.hpp b/include/trajopt/constraint/PoseEqualityConstraint.hpp new file mode 100644 index 00000000..d1670a71 --- /dev/null +++ b/include/trajopt/constraint/PoseEqualityConstraint.hpp @@ -0,0 +1,46 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/geometry/Translation2.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +/** + * Pose equality constraint. + */ +class TRAJOPT_DLLEXPORT PoseEqualityConstraint { + public: + /** + * Constructs a PoseEqualityConstraint. + * + * @param x The robot's x position. + * @param y The robot's y position. + * @param heading The robot's heading. + */ + PoseEqualityConstraint(double x, double y, double heading) + : m_pose{x, y, heading} {} + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + problem.SubjectTo(pose == m_pose); + } + + private: + trajopt::Pose2d m_pose; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/TranslationConstraint.hpp b/include/trajopt/constraint/TranslationConstraint.hpp deleted file mode 100644 index 8664d354..00000000 --- a/include/trajopt/constraint/TranslationConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/set/Set2d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Translation constraint. - */ -struct TRAJOPT_DLLEXPORT TranslationConstraint { - /// Translation bound. - Set2d translationBound; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/TranslationEqualityConstraint.hpp b/include/trajopt/constraint/TranslationEqualityConstraint.hpp new file mode 100644 index 00000000..567872b7 --- /dev/null +++ b/include/trajopt/constraint/TranslationEqualityConstraint.hpp @@ -0,0 +1,44 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/geometry/Translation2.hpp" +#include "trajopt/util/SymbolExports.hpp" + +namespace trajopt { + +/** + * Translation equality constraint. + */ +class TRAJOPT_DLLEXPORT TranslationEqualityConstraint { + public: + /** + * Constructs a TranslationEqualityConstraint. + * + * @param x The robot's x position. + * @param y The robot's y position. + */ + TranslationEqualityConstraint(double x, double y) : m_translation{x, y} {} + + /** + * 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. + */ + void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, + [[maybe_unused]] const Translation2v& linearVelocity, + [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + problem.SubjectTo(pose.Translation() == m_translation); + } + + private: + trajopt::Translation2d m_translation; +}; + +} // namespace trajopt diff --git a/include/trajopt/constraint/detail/LinePointDistance.hpp b/include/trajopt/constraint/detail/LinePointDistance.hpp new file mode 100644 index 00000000..87c57516 --- /dev/null +++ b/include/trajopt/constraint/detail/LinePointDistance.hpp @@ -0,0 +1,37 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include + +#include "trajopt/geometry/Translation2.hpp" + +namespace trajopt::detail { + +// https://www.desmos.com/calculator/cqmc1tjtsv +template +decltype(auto) LinePointDistance(const Translation2& lineStart, + const Translation2& lineEnd, + const Translation2& point) { + using R = decltype(std::declval() + std::declval()); + + auto max = [](R a, R b) { + return +0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + a; + }; + auto min = [](R a, R b) { + return -0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + b; + }; + auto Lerp = [](R a, R b, R t) { return a + t * (b - a); }; + + Translation2 l{lineEnd.X() - lineStart.X(), lineEnd.Y() - lineStart.Y()}; + Translation2 v{point.X() - lineStart.X(), point.Y() - lineStart.Y()}; + + auto t = v.Dot(l) / l.SquaredNorm(); + auto tBounded = max(min(t, 1), 0); // NOLINT + + Translation2 i{Lerp(lineStart.X(), lineEnd.X(), tBounded), + Lerp(lineStart.Y(), lineEnd.Y(), tBounded)}; + return (i - point).SquaredNorm(); +} + +} // namespace trajopt::detail diff --git a/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.hpp b/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.hpp deleted file mode 100644 index b8a4235d..00000000 --- a/include/trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Differential centripetal acceleration constraint - */ -struct TRAJOPT_DLLEXPORT DifferentialCentripetalAccelerationConstraint { - /// Acceleration bound. - IntervalSet1d accelerationBound; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/differential/DifferentialConstraint.hpp b/include/trajopt/constraint/differential/DifferentialConstraint.hpp deleted file mode 100644 index f48ca16a..00000000 --- a/include/trajopt/constraint/differential/DifferentialConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/constraint/AngularVelocityConstraint.hpp" -#include "trajopt/constraint/Constraint.hpp" -#include "trajopt/constraint/differential/DifferentialCentripetalAccelerationConstraint.hpp" -#include "trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.hpp" -#include "trajopt/util/VariantCat.hpp" - -namespace trajopt { - -using DifferentialConstraint = - variant_cat_t; - -} // namespace trajopt diff --git a/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.hpp b/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.hpp deleted file mode 100644 index b44b8b1b..00000000 --- a/include/trajopt/constraint/differential/DifferentialTangentialVelocityConstraint.hpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Differential Tangential Velocity constraint. - */ -struct TRAJOPT_DLLEXPORT DifferentialTangentialVelocityConstraint { - /// Velocity bound. - IntervalSet1d velocityBound; -}; - -} // namespace trajopt diff --git a/include/trajopt/constraint/holonomic/HolonomicConstraint.hpp b/include/trajopt/constraint/holonomic/HolonomicConstraint.hpp deleted file mode 100644 index b70618a2..00000000 --- a/include/trajopt/constraint/holonomic/HolonomicConstraint.hpp +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/constraint/AngularVelocityConstraint.hpp" -#include "trajopt/constraint/Constraint.hpp" -#include "trajopt/constraint/PointAtConstraint.hpp" -#include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp" -#include "trajopt/util/VariantCat.hpp" - -namespace trajopt { - -using HolonomicConstraint = - variant_cat_t; - -} // namespace trajopt diff --git a/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp b/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp deleted file mode 100644 index 4e261f6a..00000000 --- a/include/trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/constraint/Constraint.hpp" -#include "trajopt/set/Set2d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Velocity constraint. - */ -struct TRAJOPT_DLLEXPORT HolonomicVelocityConstraint { - /// Velocity bound. - Set2d velocityBound; - - /// Coordinate system. - CoordinateSystem coordinateSystem; -}; - -} // namespace trajopt diff --git a/include/trajopt/geometry/Pose2.hpp b/include/trajopt/geometry/Pose2.hpp index 3a91967f..19c74290 100644 --- a/include/trajopt/geometry/Pose2.hpp +++ b/include/trajopt/geometry/Pose2.hpp @@ -2,9 +2,11 @@ #pragma once +#include #include #include +#include #include "trajopt/geometry/Rotation2.hpp" #include "trajopt/geometry/Translation2.hpp" @@ -99,4 +101,12 @@ class Pose2 { using Pose2d = Pose2; using Pose2v = Pose2; +template + requires std::convertible_to || std::convertible_to +sleipnir::EqualityConstraints operator==(const Pose2& lhs, + const Pose2& rhs) { + return {lhs.Translation() == rhs.Translation(), + lhs.Rotation() == rhs.Rotation()}; +} + } // namespace trajopt diff --git a/include/trajopt/geometry/Rotation2.hpp b/include/trajopt/geometry/Rotation2.hpp index d6c1f260..14b9c85b 100644 --- a/include/trajopt/geometry/Rotation2.hpp +++ b/include/trajopt/geometry/Rotation2.hpp @@ -7,6 +7,7 @@ #include #include +#include namespace trajopt { @@ -131,4 +132,29 @@ class Rotation2 { using Rotation2d = Rotation2; using Rotation2v = Rotation2; +template +sleipnir::EqualityConstraints operator==(const Rotation2& lhs, + const Rotation2& rhs) { + // Constrain angle equality on manifold. + // + // Let lhs = . NOLINT + // Let rhs = . NOLINT + // + // If the angles are equal, the angle between the unit vectors should be + // zero. + // + // lhs x rhs = ||lhs|| ||rhs|| sin(angleBetween) NOLINT + // = 1 * 1 * 0 + // = 0 + // + // NOTE: angleBetween = π rad would be another solution + sleipnir::VariableMatrix lhsVar{ + {lhs.Cos() * rhs.Sin() - lhs.Sin() * rhs.Cos()}, + // Require that lhs and rhs are unit vectors + {lhs.Cos() * lhs.Cos() + lhs.Sin() * lhs.Sin()}, + {rhs.Cos() * rhs.Cos() + rhs.Sin() * rhs.Sin()}}; + sleipnir::VariableMatrix rhsVar{{0.0}, {1.0}, {1.0}}; + return lhsVar == rhsVar; +} + } // namespace trajopt diff --git a/include/trajopt/geometry/Translation2.hpp b/include/trajopt/geometry/Translation2.hpp index 5c035f99..045e36b4 100644 --- a/include/trajopt/geometry/Translation2.hpp +++ b/include/trajopt/geometry/Translation2.hpp @@ -7,6 +7,7 @@ #include #include +#include #include "trajopt/geometry/Rotation2.hpp" @@ -102,6 +103,32 @@ class Translation2 { */ constexpr Translation2 operator-() const { return {-m_x, -m_y}; } + /** + * Returns the translation multiplied by a scalar. + * + * @param scalar The scalar to multiply by. + * + * @return The scaled translation. + */ + template + constexpr auto operator*(const U& scalar) const { + using R = decltype(std::declval() + std::declval()); + return Translation2{m_x * scalar, m_y * scalar}; + } + + /** + * Returns the translation divided by a scalar. + * + * @param scalar The scalar to divide by. + * + * @return The scaled translation. + */ + template + constexpr auto operator/(const U& scalar) const { + using R = decltype(std::declval() + std::declval()); + return Translation2{m_x / scalar, m_y / scalar}; + } + /** * Applies a rotation to the translation in 2D space. * @@ -192,6 +219,14 @@ constexpr decltype(auto) get(const trajopt::Translation2& translation) { using Translation2d = Translation2; using Translation2v = Translation2; +template +sleipnir::EqualityConstraints operator==(const Translation2& lhs, + const Translation2& rhs) { + sleipnir::VariableMatrix lhsVar{{lhs.X()}, {lhs.Y()}}; + sleipnir::VariableMatrix rhsVar{{rhs.X()}, {rhs.Y()}}; + return lhsVar == rhsVar; +} + } // namespace trajopt namespace std { diff --git a/include/trajopt/path/Path.hpp b/include/trajopt/path/Path.hpp index fb9c4fe5..9231e7c9 100644 --- a/include/trajopt/path/Path.hpp +++ b/include/trajopt/path/Path.hpp @@ -7,8 +7,7 @@ #include #include -#include "trajopt/constraint/differential/DifferentialConstraint.hpp" -#include "trajopt/constraint/holonomic/HolonomicConstraint.hpp" +#include "trajopt/constraint/Constraint.hpp" #include "trajopt/drivetrain/DifferentialDrivetrain.hpp" #include "trajopt/drivetrain/SwerveDrivetrain.hpp" #include "trajopt/solution/SwerveSolution.hpp" @@ -17,46 +16,39 @@ namespace trajopt { /** - * A swerve path waypoint + * A path waypoint. */ -struct TRAJOPT_DLLEXPORT SwerveWaypoint { - /// instantaneous constraints at the waypoint - std::vector waypointConstraints; - /// continuous constraints along the segment - std::vector segmentConstraints; -}; +struct TRAJOPT_DLLEXPORT Waypoint { + /// Instantaneous constraints at the waypoint. + std::vector waypointConstraints; -/** - * A differential path waypoint - */ -struct TRAJOPT_DLLEXPORT DifferentialWaypoint { - /// instantaneous constraints at the waypoint - std::vector waypointConstraints; - /// continuous constraints along the segment - std::vector segmentConstraints; + /// Continuous constraints along the segment. + std::vector segmentConstraints; }; /** - * Swerve path + * Swerve path. */ struct TRAJOPT_DLLEXPORT SwervePath { - /// waypoints along the path - std::vector waypoints; - /// drivetrain of the robot + /// Waypoints along the path. + std::vector waypoints; + + /// Drivetrain of the robot. SwerveDrivetrain drivetrain; /// A vector of callbacks to be called with the intermediate SwerveSolution - /// and a user-specified handle at every iteration of the solver + /// and a user-specified handle at every iteration of the solver. std::vector> callbacks; }; /** - * Differential path + * Differential path. */ struct TRAJOPT_DLLEXPORT DifferentialPath { - /// waypoints along the path - std::vector waypoints; - /// drivetrain of the robot + /// Waypoints along the path. + std::vector waypoints; + + /// Drivetrain of the robot. DifferentialDrivetrain drivetrain; }; diff --git a/include/trajopt/path/SwervePathBuilder.hpp b/include/trajopt/path/SwervePathBuilder.hpp index fd8cbd8b..f04c64df 100644 --- a/include/trajopt/path/SwervePathBuilder.hpp +++ b/include/trajopt/path/SwervePathBuilder.hpp @@ -4,16 +4,18 @@ #include +#include #include #include +#include #include +#include "trajopt/constraint/Constraint.hpp" #include "trajopt/drivetrain/SwerveDrivetrain.hpp" #include "trajopt/geometry/Pose2.hpp" #include "trajopt/obstacle/Bumpers.hpp" #include "trajopt/obstacle/Obstacle.hpp" #include "trajopt/path/Path.hpp" -#include "trajopt/solution/Solution.hpp" #include "trajopt/solution/SwerveSolution.hpp" namespace trajopt { @@ -21,7 +23,7 @@ namespace trajopt { /** * Builds a swerve path using information about how the robot * must travel through a series of waypoints. This path can be converted - * to a trajectory using OptimalTrajectoryGenerator. + * to a trajectory using SwerveTrajectoryGenerator. */ class TRAJOPT_DLLEXPORT SwervePathBuilder { public: @@ -35,7 +37,7 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * * @return the path */ - const SwervePath& GetPath() const; + SwervePath& GetPath(); /** * Set the Drivetrain object @@ -50,12 +52,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,188 +65,36 @@ 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 angle the polar angle of the required direction - */ - void WptVelocityDirection(size_t idx, double angle); - - /** - * Specify the required maximum magnitude of the velocity vector of the - * robot at a waypoint. - * - * @param idx index of the waypoint - * @param v the maximum velocity magnitude - */ - void WptVelocityMagnitude(size_t idx, double v); - - /** - * Specify the required velocity vector of the robot at a waypoint to - * be zero. - * - * @param idx index of the waypoint - */ - void WptZeroVelocity(size_t idx); - - /** - * Specify the exact required velocity vector of the robot at a - * waypoint. - * - * @param idx index of the waypoint - * @param vr velocity vector magnitude - * @param vtheta velocity vector polar angle - */ - void WptVelocityPolar(size_t idx, double vr, double vtheta); - - /** - * Specify the required angular velocity of the robot at a waypoint - * - * @param idx index of the waypoint - * @param angular_velocity the angular velocity - */ - void WptAngularVelocity(size_t idx, double angular_velocity); - - /** - * Specify the required angular velocity of the robot at a waypoint - * - * @param idx index of the waypoint - * @param angular_velocity the maximum angular velocity magnitude - */ - void WptAngularVelocityMaxMagnitude(size_t idx, double angular_velocity); - - /** - * Specify the required angular velocity of the robot to be zero - * at a waypoint - * - * @param idx index of the waypoint - */ - void WptZeroAngularVelocity(size_t idx); - - /** - * 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 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 - */ - void SgmtVelocityDirection(size_t fromIdx, size_t toIdx, 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 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 - */ - void SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, 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 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 - */ - void SgmtAngularVelocity(size_t fromIdx, size_t toIdx, - 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 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 - */ - void SgmtAngularVelocityMaxMagnitude(size_t fromIdx, size_t toIdx, - double angular_velocity, - bool includeWpts = true); - - /** - * 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 includeWpts if using a discrete algorithm, false does not apply the - * constraint at the instantaneous state at waypoints at indices fromIdx and - * toIdx - */ - void SgmtZeroAngularVelocity(size_t fromIdx, size_t toIdx, - bool includeWpts = true); - - /** - * Apply a custom holonomic constraint at a waypoint - * - * @param idx index of the waypoint - * @param constraint the constraint to be applied - */ - void WptConstraint(size_t idx, 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 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 - */ - void SgmtConstraint(size_t fromIdx, size_t toIdx, - const HolonomicConstraint& constraint, - bool includeWpts = true); - /** * Add polygon or circle shaped bumpers to a list used when applying * obstacle constraints. @@ -256,24 +106,51 @@ 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 */ - void SgmtObstacle(size_t fromIdx, size_t toIdx, const Obstacle& obstacle, - bool includeWpts = true); + void SgmtObstacle(size_t fromIndex, size_t toIndex, const Obstacle& obstacle); + + /** + * Apply a constraint at a waypoint. + * + * @param index Index of the waypoint. + * @param constraint The constraint. + */ + void WptConstraint(size_t index, const Constraint& constraint) { + NewWpts(index); + path.waypoints.at(index).waypointConstraints.push_back(constraint); + } + + /** + * Apply a custom holonomic constraint to the continuum of state between two + * waypoints. + * + * @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 constraint. + */ + void SgmtConstraint(size_t fromIndex, size_t toIndex, + const Constraint& constraint) { + assert(fromIndex < toIndex); + + NewWpts(toIndex); + path.waypoints.at(fromIndex).waypointConstraints.push_back(constraint); + for (size_t index = fromIndex + 1; index <= toIndex; ++index) { + path.waypoints.at(index).waypointConstraints.push_back(constraint); + path.waypoints.at(index).segmentConstraints.push_back(constraint); + } + } /** * If using a discrete algorithm, specify the number of discrete @@ -297,7 +174,7 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { * * @return the initial guess, as a solution */ - Solution CalculateInitialGuess() const; + SwerveSolution CalculateInitialGuess() const; /** * Add a callback to retrieve the state of the solver as a SwerveSolution. @@ -319,8 +196,6 @@ class TRAJOPT_DLLEXPORT SwervePathBuilder { std::vector controlIntervalCounts; void NewWpts(size_t finalIndex); - - static std::vector GetConstraintsForObstacle( - const Bumpers& bumpers, const Obstacle& obstacle); }; + } // namespace trajopt diff --git a/include/trajopt/set/ConeSet2d.hpp b/include/trajopt/set/ConeSet2d.hpp deleted file mode 100644 index 9b73506c..00000000 --- a/include/trajopt/set/ConeSet2d.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Conical 2D set. - */ -struct TRAJOPT_DLLEXPORT ConeSet2d { - /// The heading bounds of the cone. - IntervalSet1d thetaBound; - - /** - * Constructs a ConeSet2d. - * - * @param thetaBound The internal angle of the cone tip. Must be within (0, - * π]. - */ - explicit constexpr ConeSet2d(const IntervalSet1d& thetaBound) - : thetaBound{thetaBound} { - assert(thetaBound.Range() > 0.0 && thetaBound.Range() <= std::numbers::pi); - } -}; - -} // namespace trajopt diff --git a/include/trajopt/set/EllipticalSet2d.hpp b/include/trajopt/set/EllipticalSet2d.hpp deleted file mode 100644 index 2782a625..00000000 --- a/include/trajopt/set/EllipticalSet2d.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Elliptical 2D set. - */ -struct TRAJOPT_DLLEXPORT EllipticalSet2d { - /** - * The directionality of the set. - */ - enum class Direction { - /// The set is every point inside the ellipse. - kInside, - /// The set is every point on the border of the ellipse. - kCentered, - /// The set is every point outside the ellipse. - kOutside - }; - - /// The x radius. - double xRadius; - - /// The y radius. - double yRadius; - - /// The set direction. - Direction direction; - - /** - * Construct an EllipticalSet2d. - * - * @param xRadius The ellipse's x radius. Must be greater than zero. - * @param yRadius The ellipse's y radius. Must be greater than zero. - * @param direction The set direction. - */ - constexpr EllipticalSet2d(double xRadius, double yRadius, Direction direction) - : xRadius{xRadius}, yRadius{yRadius}, direction{direction} { - assert(xRadius > 0.0 && yRadius > 0.0); - } - - /** - * Construct a circular EllipticalSet2d from a radius. - * - * @param radius The radius. - * @param direction The direction. - */ - static constexpr EllipticalSet2d CircularSet2d( - double radius, Direction direction = Direction::kInside) { - return EllipticalSet2d{radius, radius, direction}; - } -}; - -} // namespace trajopt diff --git a/include/trajopt/set/IntervalSet1d.hpp b/include/trajopt/set/IntervalSet1d.hpp deleted file mode 100644 index 48b24681..00000000 --- a/include/trajopt/set/IntervalSet1d.hpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * This class represents a bounded region of abstract 1D space. The bound - * is specified by an inclusive inequality between two numbers. - */ -struct TRAJOPT_DLLEXPORT IntervalSet1d { - /// The lower bound. - double lower; - - /// The upper bound. - double upper; - - /** - * Construct a scalar bound between a lower and upper bound. - * - * @param lower The lower bound. Must be less than or equal to upper bound. - * @param upper The upper bound. Must be greater than or equal to lower bound. - */ - constexpr IntervalSet1d(double lower, double upper) - : lower(lower), upper(upper) { - assert(lower <= upper); - } - - /** - * Construct a scalar bound that represents the interval [value, value]. - * - * @param value the value to bound the number between. - */ - constexpr IntervalSet1d(double value) // NOLINT - : IntervalSet1d{value, value} {} - - constexpr IntervalSet1d() = default; - - /** - * Returns an IntervalSet1d spanning R¹. - */ - static constexpr IntervalSet1d R1() { - return IntervalSet1d(-std::numeric_limits::infinity(), - +std::numeric_limits::infinity()); - } - - /** - * Returns an IntervalSet1d that contains all the real numbers less than or - * equal to a maximum value - * - * @param max the maximum value - * @return [-∞, max] - */ - static constexpr IntervalSet1d LessThan(double max) { - return IntervalSet1d(-std::numeric_limits::infinity(), max); - } - - /** - * Returns an IntervalSet1d that contains all the real numbers greater than or - * equal to a minimum value - * - * @param min the minimum value - * @return [min, ∞] - */ - static constexpr IntervalSet1d GreaterThan(double min) { - return IntervalSet1d(min, +std::numeric_limits::infinity()); - } - - /** - * Check if this scalar bound is equivalent to another scalar bound. - * Two scalar bounds are equivalent if their upper and lower bounds are equal. - * This operator can also be used to check if this bound is equal to a number, - * meaning the upper and lower bounds both equal the given number. - * - * @param other the other scalar bound - * @return lower == other.lower && upper == other.upper - */ - constexpr bool operator==(const IntervalSet1d& other) const = default; - - /** - * Calculate the range of this scalar bound, which is the difference - * between the upper and lower bounds. An infinite scalar bound has a range - * of positive infinity. - * - * @return upper - lower - */ - constexpr double Range() const noexcept { return upper - lower; } - - /** - * Check if this scalar bound only contains one point. This only - * occurs when lower == upper. - * - * @return lower == upper - */ - constexpr bool IsExact() const noexcept { return lower == upper; } - - /** - * Returns true if this IntervalSet1d has a lower bound. - */ - constexpr bool IsLowerBounded() const noexcept { - return lower > -std::numeric_limits::infinity(); - } - - /** - * Returns true if this IntervalSet1d has an upper bound. - */ - constexpr bool IsUpperBounded() const noexcept { - return upper < +std::numeric_limits::infinity(); - } -}; - -} // namespace trajopt diff --git a/include/trajopt/set/LinearSet2d.hpp b/include/trajopt/set/LinearSet2d.hpp deleted file mode 100644 index 468b017a..00000000 --- a/include/trajopt/set/LinearSet2d.hpp +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Linear 2D set. - */ -struct TRAJOPT_DLLEXPORT LinearSet2d { - /// The direction in which to constrain a 2D vector. - double theta; -}; - -} // namespace trajopt diff --git a/include/trajopt/set/ManifoldIntervalSet2d.hpp b/include/trajopt/set/ManifoldIntervalSet2d.hpp deleted file mode 100644 index 658b1e35..00000000 --- a/include/trajopt/set/ManifoldIntervalSet2d.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Manifold 2D set for angles represented as cos-sin pairs. - */ -struct TRAJOPT_DLLEXPORT ManifoldIntervalSet2d { - /// The angle at the midpoint of the range. - double middle; - - /// Half of the width of the valid range. - double tolerance; - - /** - * Construct a manifold vector bound to a particular value +- a tolerance. - * - * @param middle The angle at the midpoint of the valid range - * @param tolerance The tolerance. Must be positive. Tolerances greater than - * pi, (half the period) do not constrain the vector at all. - */ - constexpr ManifoldIntervalSet2d(double middle, double tolerance) - : middle(middle), tolerance(tolerance) { - assert(tolerance >= 0); - } - - /** - * Construct a manifold vector bound that represents the given angle exactly. - * - * @param angle the angle of the vector to. - */ - constexpr ManifoldIntervalSet2d(double angle) // NOLINT - : ManifoldIntervalSet2d(angle, 0) {} - - constexpr ManifoldIntervalSet2d() = default; -}; - -} // namespace trajopt diff --git a/include/trajopt/set/RectangularSet2d.hpp b/include/trajopt/set/RectangularSet2d.hpp deleted file mode 100644 index 4e809e51..00000000 --- a/include/trajopt/set/RectangularSet2d.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * Rectangular 2D set. - */ -struct TRAJOPT_DLLEXPORT RectangularSet2d { - /// The x boundary. - IntervalSet1d xBound; - - /// The y boundary. - IntervalSet1d yBound; - - /** - * Construct a RectangularSet2d from polar coordinates. - * - * @param r The distance. Must be greater than zero. - * @param theta The heading. - */ - static RectangularSet2d PolarExactSet2d(double r, double theta) { - return RectangularSet2d{r * std::cos(theta), r * std::sin(theta)}; - } - - /** - * Construct a RectangularSet2d spanning R². - */ - static constexpr RectangularSet2d R2() { - return RectangularSet2d{IntervalSet1d::R1(), IntervalSet1d::R1()}; - } -}; - -} // namespace trajopt diff --git a/include/trajopt/set/Set2d.hpp b/include/trajopt/set/Set2d.hpp deleted file mode 100644 index 461687ae..00000000 --- a/include/trajopt/set/Set2d.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/set/ConeSet2d.hpp" -#include "trajopt/set/EllipticalSet2d.hpp" -#include "trajopt/set/LinearSet2d.hpp" -#include "trajopt/set/ManifoldIntervalSet2d.hpp" -#include "trajopt/set/RectangularSet2d.hpp" - -namespace trajopt { - -/** - * @brief This class represents a bounded region of abstract 2D space. The - * bounds are specified with inequalities for each coordinate, a0 and a1. The - * interpretation of the abstract coordinates a0 and a1 changes when - * coordinateType changes. If coordinateType is kRectangular, then a0 and a1 - * represent the rectangular x-coordinate and y-coordinate bounds, respectively. - * If coordinateType is kPolar, then a0 and a1 represent the polar r-coordinate - * and theta-coordinate bounds, respectively. Polar theta coordinates must be - * specified between -pi and pi, inclusive, and all lower bounds must be less - * than or equal to their respective upper bounds. - * - * The following are some common use cases: - * A straight line theta = theta_0 bound can be created as a polar bound - * bounding theta within [theta_0, theta_0], and a straight line x = x_0 or y = - * y_0 bound can be created by bounding x within [x_0, x_0] or y within [y_0, - * y_0]. Bounding the magnitude of a vector can be achieved by bounding r within - * [r_lower, r_upper]. - * - * Bounding theta within [-pi, -pi] or within [pi, pi] is equivalent; both will - * force the vector to be colinear with the x-axis. - */ -using Set2d = std::variant; - -} // namespace trajopt diff --git a/include/trajopt/set/setnotation.txt b/include/trajopt/set/setnotation.txt deleted file mode 100644 index f7903f8c..00000000 --- a/include/trajopt/set/setnotation.txt +++ /dev/null @@ -1,110 +0,0 @@ -Interval Set 1d: - format_string = - if exact: - "= {lower}" - else: - "∈ [{lower}, {upper}]" - violation_string = "= {scalar}" - -Rectangular Set 2d: - format_string = "x {xBound}, y {yBound}"" - voilation_string = - if xBound violated: - "x {xBound violation_string}" - else if yBound violated: - "y {yBound violation_string}" - -Linear Set 2d: - format_string = "polar line: θ = {theta}" - violation_string = "(r, θ) = ({hypot(xComp, yComp)}, {atan2(yComp, xComp)}" - -Cone Set 2d: - format_string = "cone: θ {thetaBound}" - violation_string = "(r, θ) = ({hypot(xComp, yComp)}, {atan2(yComp, xComp)}" - -Elliptical Set 2d: - format_string = ": , rₓ = {xRadius}, rᵧ = {yRadius}" - = circle, ellipse - = inside, centered, outside - violation_string = "(x, y) = ({xComp}, {yComp})" - -Set 2d: - format_string = "2d {set2d}" - violation_string = "{set2d violation_string} - -HeadingConstraint: - format_string = "heading {headingBound}" - violation_string = "heading {headingBound violation_string}" - -TranslationConstraint: - format_string = "translation {translationBound}" - violation_string = "translation {translationBound violation_string}" - -PoseConstraint: - format_string = "{translationBound}, {headingBound}" - violation_string = - if heading violated: "{heading violation_string}" - else if translation violated: "{translation violation_string}" - -VelocityHolonomicConstraint: - format_string = - "constraint (velocity {velocityBound}) violated: velocity = ({velocityX}, {velocityY})" - -AngularVelocityHolonmicConstraint: - format_string = "angular velocity {angularVelocityBound}" - violation_string = " - -Constraint: - format_string = "constraint: {constraint}" - violation_string = "({constraint}) violated: {constraint violation_string}" - - -Other ways of formatting/examples: - -"translation constraint: position x ∈ [1, 2], y ∈ [1, 2]" applied to waypoint index 0 violated: position x = 6 is outside the interval [1, 2]. -translation constraint applied to segment index 0 violated: - -position x of 3 is outside bound [{}, {}] - -Set 1d: - Interval Set 1d: - if lower == upper: - "= {lower}" - else: - "∈ [{lower}, {upper}]" - -Set 2d: - - - Rectangular Set 2d: - "x-component {xBound}, y-component {yBound}"" - - Linear Set 2d: - (x-component, y-component) must be on the polar line θ = {theta} - Cone Set 2d: - (x-component, y-component) must be within the polar cone r ≥ 0, {} ≤ θ ≤ {} - - Elliptical Set 2d: - "(x-component, y-component) must be " - = - if inside: - "on or within" - else if centered: - "on" - else if outside: - "on or outside" - = - if circular: - "a circle of radius = {xRadius}" - else: - "an ellipse of rₓ = {xRadius}, rᵧ = {yRadius}" - - -Constraint: - HeadingConstraint: - "constraint heading {headingBound} violated: theta = {theta}" - TranslationConstraint: - PoseConstraint: - - -heading constraint: θ diff --git a/include/trajopt/solution/DifferentialSolution.hpp b/include/trajopt/solution/DifferentialSolution.hpp index 9d49cdbd..f77535e1 100644 --- a/include/trajopt/solution/DifferentialSolution.hpp +++ b/include/trajopt/solution/DifferentialSolution.hpp @@ -4,7 +4,6 @@ #include -#include "trajopt/solution/Solution.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { @@ -12,14 +11,32 @@ namespace trajopt { /** * The holonomic trajectory optimization solution. */ -struct TRAJOPT_DLLEXPORT DifferentialSolution : public Solution { +struct TRAJOPT_DLLEXPORT DifferentialSolution { + /// Times between samples. + std::vector dt; + + /// X positions. + std::vector x; + + /// Y positions. + std::vector y; + + /// Heading cosine. + std::vector thetacos; + + /// Heading sine. + std::vector thetasin; + /// The x velocities. std::vector vL; + /// The y velocities. std::vector vR; - /// the torque of the left driverail wheels + + /// The torque of the left driverail wheels. std::vector tauL; - /// the torque of the right driverail wheels + + /// The torque of the right driverail wheels. std::vector tauR; }; diff --git a/include/trajopt/solution/HolonomicSolution.hpp b/include/trajopt/solution/HolonomicSolution.hpp deleted file mode 100644 index 994397a6..00000000 --- a/include/trajopt/solution/HolonomicSolution.hpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/solution/Solution.hpp" -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * The holonomic trajectory optimization solution. - */ -struct TRAJOPT_DLLEXPORT HolonomicSolution : public Solution { - /// The x velocities. - std::vector vx; - - /// The y velocities. - std::vector vy; - - /// The angular velocities. - std::vector omega; - - /// The x accelerations. - std::vector ax; - - /// The y accelerations. - std::vector ay; - - /// The angular accelerations. - std::vector alpha; -}; - -} // namespace trajopt diff --git a/include/trajopt/solution/Solution.hpp b/include/trajopt/solution/Solution.hpp deleted file mode 100644 index 2759390e..00000000 --- a/include/trajopt/solution/Solution.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "trajopt/util/SymbolExports.hpp" - -namespace trajopt { - -/** - * The trajectory optimization solution. - */ -struct TRAJOPT_DLLEXPORT Solution { - /// Times between samples. - std::vector dt; - - /// X positions. - std::vector x; - - /// Y positions. - std::vector y; - - /// Heading cosine. - std::vector thetacos; - /// Heading sine. - std::vector thetasin; -}; - -} // namespace trajopt diff --git a/include/trajopt/solution/SwerveSolution.hpp b/include/trajopt/solution/SwerveSolution.hpp index 5c4cfee6..c079a805 100644 --- a/include/trajopt/solution/SwerveSolution.hpp +++ b/include/trajopt/solution/SwerveSolution.hpp @@ -4,7 +4,6 @@ #include -#include "trajopt/solution/HolonomicSolution.hpp" #include "trajopt/util/SymbolExports.hpp" namespace trajopt { @@ -12,7 +11,40 @@ namespace trajopt { /** * The swerve drive trajectory optimization solution. */ -struct TRAJOPT_DLLEXPORT SwerveSolution : HolonomicSolution { +struct TRAJOPT_DLLEXPORT SwerveSolution { + /// Times between samples. + std::vector dt; + + /// X positions. + std::vector x; + + /// Y positions. + std::vector y; + + /// Heading cosine. + std::vector thetacos; + + /// Heading sine. + std::vector thetasin; + + /// The x velocities. + std::vector vx; + + /// The y velocities. + std::vector vy; + + /// The angular velocities. + std::vector omega; + + /// The x accelerations. + std::vector ax; + + /// The y accelerations. + std::vector ay; + + /// The angular accelerations. + std::vector alpha; + /// The x forces for each module. std::vector> moduleFX; diff --git a/include/trajopt/trajectory/HolonomicTrajectory.hpp b/include/trajopt/trajectory/HolonomicTrajectory.hpp index f0203b97..dab90cc9 100644 --- a/include/trajopt/trajectory/HolonomicTrajectory.hpp +++ b/include/trajopt/trajectory/HolonomicTrajectory.hpp @@ -2,10 +2,10 @@ #pragma once +#include #include #include -#include "trajopt/solution/HolonomicSolution.hpp" #include "trajopt/solution/SwerveSolution.hpp" #include "trajopt/trajectory/HolonomicTrajectorySample.hpp" #include "trajopt/util/SymbolExports.hpp" @@ -30,24 +30,6 @@ class TRAJOPT_DLLEXPORT HolonomicTrajectory { explicit HolonomicTrajectory(std::vector samples) : samples{std::move(samples)} {} - /** - * Construct a HolonomicTrajectory from a solution. - * - * @param solution The solution. - */ - explicit HolonomicTrajectory(const HolonomicSolution& solution) { - double ts = 0.0; - for (size_t samp = 0; samp < solution.x.size(); ++samp) { - if (samp != 0) { - ts += solution.dt[samp - 1]; - } - samples.emplace_back( - ts, solution.x[samp], solution.y[samp], - std::atan2(solution.thetasin[samp], solution.thetacos[samp]), - solution.vx[samp], solution.vy[samp], solution.omega[samp]); - } - } - /** * Construct a HolonomicTrajectory from a swerve solution. * diff --git a/src/optimization/Cancellation.hpp b/include/trajopt/util/Cancellation.hpp similarity index 54% rename from src/optimization/Cancellation.hpp rename to include/trajopt/util/Cancellation.hpp index 8740cbbf..ad78584d 100644 --- a/src/optimization/Cancellation.hpp +++ b/include/trajopt/util/Cancellation.hpp @@ -4,8 +4,10 @@ #include +#include "trajopt/util/SymbolExports.hpp" + namespace trajopt { -std::atomic& GetCancellationFlag(); +TRAJOPT_DLLEXPORT std::atomic& GetCancellationFlag(); } // namespace trajopt diff --git a/include/trajopt/util/GenerateLinearInitialGuess.hpp b/include/trajopt/util/GenerateLinearInitialGuess.hpp new file mode 100644 index 00000000..04e475aa --- /dev/null +++ b/include/trajopt/util/GenerateLinearInitialGuess.hpp @@ -0,0 +1,167 @@ +// Copyright (c) TrajoptLib contributors + +#pragma once + +#include +#include + +#include "trajopt/geometry/Pose2.hpp" +#include "trajopt/geometry/Translation2.hpp" + +namespace trajopt { + +/** + * Get the index of an item in a decision variable array, given the waypoint and + * sample indices and whether this array includes an entry for the initial + * sample point ("dt" does not, "x" does). + * + * @param N The control interval counts of each segment, in order. + * @param wptIdx The waypoint index (1 + segment index). + * @param sampIdx The sample index within the segment. + * @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; + } + for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { + idx += N.at(_wptIdx - 1); + } + idx += sampIdx; + return idx; +} + +inline std::vector Linspace(double startValue, double endValue, + size_t numSamples) { + std::vector result; + double delta = (endValue - startValue) / numSamples; + for (size_t index = 1; index <= numSamples; index++) { + result.push_back(startValue + index * delta); + } + return result; +} + +inline std::vector AngleLinspace(double startValue, double endValue, + size_t numSamples) { + auto diff = endValue - startValue; + // angleModulus + const double modulus = 2 * std::numbers::pi; + const double minimumInput = -std::numbers::pi; + const double maximumInput = std::numbers::pi; + // Wrap input if it's above the maximum input + const double numMax = std::trunc((diff - minimumInput) / modulus); + diff -= numMax * modulus; + + // Wrap input if it's below the minimum input + const double numMin = std::trunc((diff - maximumInput) / modulus); + diff -= numMin * modulus; + + return Linspace(startValue, startValue + diff, numSamples); +} + +template +inline void append_vector(std::vector& base, + const std::vector& newItems) { + base.insert(base.end(), newItems.begin(), newItems.end()); +} + +template +Solution GenerateLinearInitialGuess( + const std::vector>& initialGuessPoints, + const std::vector controlIntervalCounts) { + size_t wptCnt = controlIntervalCounts.size() + 1; + size_t sampTot = GetIdx(controlIntervalCounts, wptCnt, 0); + + Solution initialGuess; + + initialGuess.x.reserve(sampTot); + initialGuess.y.reserve(sampTot); + initialGuess.thetacos.reserve(sampTot); + initialGuess.thetasin.reserve(sampTot); + initialGuess.dt.reserve(sampTot); + + initialGuess.x.push_back(initialGuessPoints.front().front().X()); + initialGuess.y.push_back(initialGuessPoints.front().front().Y()); + initialGuess.thetacos.push_back( + initialGuessPoints.front().front().Rotation().Cos()); + initialGuess.thetasin.push_back( + initialGuessPoints.front().front().Rotation().Sin()); + + for (size_t i = 0; i < sampTot; i++) { + 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(); + 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)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx - 1).back().Y(), + initialGuessPoints.at(wptIdx).front().Y(), N_guessSgmt)); + auto wptThetas = AngleLinspace( + initialGuessPoints.at(wptIdx - 1).back().Rotation().Radians(), + initialGuessPoints.at(wptIdx).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 + append_vector( + initialGuess.x, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).X(), + initialGuessPoints.at(wptIdx).at(guessPointIdx).X(), + N_guessSgmt)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).Y(), + initialGuessPoints.at(wptIdx).at(guessPointIdx).Y(), + N_guessSgmt)); + auto guessThetas = AngleLinspace( + initialGuessPoints.at(wptIdx) + .at(guessPointIdx - 1) + .Rotation() + .Radians(), + initialGuessPoints.at(wptIdx).at(guessPointIdx).Rotation().Radians(), + N_guessSgmt); + for (auto theta : guessThetas) { + initialGuess.thetacos.push_back(std::cos(theta)); + initialGuess.thetasin.push_back(std::sin(theta)); + } + } + if (guessPointCount > 1) { // if two or more guess points + 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)); + append_vector( + initialGuess.y, + Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).Y(), + initialGuessPoints.at(wptIdx).back().Y(), N_lastGuessSgmt)); + auto lastThetas = AngleLinspace( + initialGuessPoints.at(wptIdx) + .at(guessPointCount - 2) + .Rotation() + .Radians(), + initialGuessPoints.at(wptIdx).back().Rotation().Radians(), + N_lastGuessSgmt); + for (auto theta : lastThetas) { + initialGuess.thetacos.push_back(std::cos(theta)); + initialGuess.thetasin.push_back(std::sin(theta)); + } + } + } + + return initialGuess; +} + +} // namespace trajopt diff --git a/include/trajopt/util/VariantCat.hpp b/include/trajopt/util/VariantCat.hpp deleted file mode 100644 index f289e47a..00000000 --- a/include/trajopt/util/VariantCat.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -namespace trajopt { - -namespace detail { - -//! @cond Doxygen_Suppress - -template -struct variant_cat; - -template -struct variant_cat, Args1...> { - using type = std::variant; -}; - -//! @endcond - -} // namespace detail - -/** - * Defines a new variant type that appends the types Args... to the variant type - * Variant. - */ -template -using variant_cat_t = typename detail::variant_cat::type; - -} // namespace trajopt diff --git a/include/trajopt/expected b/include/trajopt/util/expected similarity index 100% rename from include/trajopt/expected rename to include/trajopt/util/expected diff --git a/src/OptimalTrajectoryGenerator.cpp b/src/OptimalTrajectoryGenerator.cpp deleted file mode 100644 index c81dbd7f..00000000 --- a/src/OptimalTrajectoryGenerator.cpp +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include "trajopt/OptimalTrajectoryGenerator.hpp" - -#include "optimization/algorithms/SwerveDiscreteOptimal.hpp" - -namespace trajopt { - -expected OptimalTrajectoryGenerator::Generate( - const SwervePathBuilder& path, bool diagnostics, int64_t handle) { - SwerveDiscreteOptimal problem(path.GetPath(), path.GetControlIntervalCounts(), - path.CalculateInitialGuess(), handle); - return problem.Generate(diagnostics); -} - -} // namespace trajopt diff --git a/src/SwerveTrajectoryGenerator.cpp b/src/SwerveTrajectoryGenerator.cpp new file mode 100644 index 00000000..84d279bf --- /dev/null +++ b/src/SwerveTrajectoryGenerator.cpp @@ -0,0 +1,384 @@ +// Copyright (c) TrajoptLib contributors + +#include "trajopt/SwerveTrajectoryGenerator.hpp" + +#include + +#include +#include +#include +#include + +#include + +#include "trajopt/path/SwervePathBuilder.hpp" +#include "trajopt/solution/SwerveSolution.hpp" +#include "trajopt/util/Cancellation.hpp" + +namespace trajopt { + +/** + * Get the index of an item in a decision variable array, given the waypoint and + * sample indices and whether this array includes an entry for the initial + * sample point ("dt" does not, "x" does). + * + * @param N The control interval counts of each segment, in order. + * @param wptIdx The waypoint index (1 + segment index). + * @param sampIdx The sample index within the segment. + * @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; + } + for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { + idx += N.at(_wptIdx - 1); + } + idx += sampIdx; + return idx; +} + +inline std::vector RowSolutionValue( + std::vector& rowVector) { + std::vector valueRowVector; + valueRowVector.reserve(rowVector.size()); + for (auto& expression : rowVector) { + valueRowVector.push_back(expression.Value()); + } + return valueRowVector; +} + +inline std::vector> MatrixSolutionValue( + std::vector>& matrix) { + std::vector> valueMatrix; + valueMatrix.reserve(matrix.size()); + for (auto& row : matrix) { + valueMatrix.push_back(RowSolutionValue(row)); + } + return valueMatrix; +} + +SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( + SwervePathBuilder pathBuilder, int64_t handle) + : path(pathBuilder.GetPath()), N(pathBuilder.GetControlIntervalCounts()) { + auto initialGuess = pathBuilder.CalculateInitialGuess(); + + callbacks.emplace_back([this, handle = handle] { + constexpr int fps = 60; + constexpr std::chrono::duration timePerFrame{1.0 / fps}; + + // FPS limit on sending updates + static auto lastFrameTime = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + if (now - lastFrameTime < timePerFrame) { + return; + } + + lastFrameTime = now; + + auto soln = ConstructSwerveSolution(); + for (auto& callback : this->path.callbacks) { + callback(soln, handle); + } + }); + size_t wptCnt = 1 + N.size(); + size_t sgmtCnt = N.size(); + size_t sampTot = GetIdx(N, wptCnt, 0); + size_t moduleCnt = path.drivetrain.modules.size(); + + x.reserve(sampTot); + y.reserve(sampTot); + thetacos.reserve(sampTot); + thetasin.reserve(sampTot); + vx.reserve(sampTot); + vy.reserve(sampTot); + omega.reserve(sampTot); + ax.reserve(sampTot); + ay.reserve(sampTot); + alpha.reserve(sampTot); + + Fx.reserve(sampTot); + Fy.reserve(sampTot); + for (size_t sampIdx = 0; sampIdx < sampTot; ++sampIdx) { + auto& _Fx = Fx.emplace_back(); + auto& _Fy = Fy.emplace_back(); + _Fx.reserve(moduleCnt); + _Fy.reserve(moduleCnt); + } + + dt.reserve(sgmtCnt); + + for (size_t idx = 0; idx < sampTot; ++idx) { + x.emplace_back(problem.DecisionVariable()); + y.emplace_back(problem.DecisionVariable()); + thetacos.emplace_back(problem.DecisionVariable()); + thetasin.emplace_back(problem.DecisionVariable()); + vx.emplace_back(problem.DecisionVariable()); + vy.emplace_back(problem.DecisionVariable()); + omega.emplace_back(problem.DecisionVariable()); + ax.emplace_back(problem.DecisionVariable()); + 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()); + } + } + + double minWidth = INFINITY; + for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { + if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() - + path.drivetrain.modules.at(i).translation.X()) != 0) { + minWidth = std::min( + minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.X() - + path.drivetrain.modules.at(i).translation.X())); + } + if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - + path.drivetrain.modules.at(i).translation.Y()) != 0) { + minWidth = std::min( + minWidth, std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - + path.drivetrain.modules.at(i).translation.Y())); + } + } + + for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { + dt.emplace_back(problem.DecisionVariable()); + for (auto module : path.drivetrain.modules) { + problem.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * + module.wheelMaxAngularVelocity <= + minWidth); + } + } + + // Minimize total time + 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); + auto T_sgmt = dt_sgmt * static_cast(N_sgmt); + T_tot += T_sgmt; + + problem.SubjectTo(dt_sgmt >= 0); + dt_sgmt.SetValue(5.0 / N_sgmt); + } + problem.Minimize(std::move(T_tot)); + + // Apply kinematics constraints + 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); + + Translation2v x_n{x.at(idx), y.at(idx)}; + Translation2v x_n_1{x.at(idx - 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)}; + + Translation2v v_n{vx.at(idx), vy.at(idx)}; + Translation2v v_n_1{vx.at(idx - 1), vy.at(idx - 1)}; + + auto omega_n = omega.at(idx); + auto omega_n_1 = omega.at(idx - 1); + + Translation2v a_n{ax.at(idx), ay.at(idx)}; + auto alpha_n = alpha.at(idx); + + problem.SubjectTo(x_n_1 + v_n * dt_sgmt == x_n); + problem.SubjectTo((theta_n - theta_n_1) == Rotation2v{omega_n * dt_sgmt}); + problem.SubjectTo(v_n_1 + a_n * dt_sgmt == v_n); + problem.SubjectTo(omega_n_1 + alpha_n * dt_sgmt == omega_n); + } + } + + for (size_t idx = 0; idx < sampTot; ++idx) { + Rotation2v theta{thetacos.at(idx), thetasin.at(idx)}; + Translation2v v{vx.at(idx), vy.at(idx)}; + + // Solve for net force + auto Fx_net = std::accumulate(Fx.at(idx).begin(), Fx.at(idx).end(), + sleipnir::Variable{0.0}); + auto Fy_net = std::accumulate(Fy.at(idx).begin(), Fy.at(idx).end(), + sleipnir::Variable{0.0}); + + // Solve for net torque + sleipnir::Variable tau_net = 0.0; + for (size_t moduleIdx = 0; moduleIdx < path.drivetrain.modules.size(); + ++moduleIdx) { + const auto& [translation, wheelRadius, wheelMaxAngularVelocity, + wheelMaxTorque] = path.drivetrain.modules.at(moduleIdx); + + auto r = translation.RotateBy(theta); + Translation2v F{Fx.at(idx).at(moduleIdx), Fy.at(idx).at(moduleIdx)}; + + tau_net += r.Cross(F); + } + + // Apply module power constraints + auto vWrtRobot = v.RotateBy(-theta); + for (size_t moduleIdx = 0; moduleIdx < path.drivetrain.modules.size(); + ++moduleIdx) { + const auto& [translation, wheelRadius, wheelMaxAngularVelocity, + wheelMaxTorque] = path.drivetrain.modules.at(moduleIdx); + + Translation2v vWheelWrtRobot{ + vWrtRobot.X() - translation.Y() * omega.at(idx), + vWrtRobot.Y() + translation.X() * omega.at(idx)}; + double maxWheelVelocity = wheelRadius * wheelMaxAngularVelocity; + problem.SubjectTo(vWheelWrtRobot.SquaredNorm() <= + maxWheelVelocity * maxWheelVelocity); + + Translation2v moduleF{Fx.at(idx).at(moduleIdx), Fy.at(idx).at(moduleIdx)}; + double maxForce = wheelMaxTorque / wheelRadius; + problem.SubjectTo(moduleF.SquaredNorm() <= maxForce * maxForce); + } + + // Apply dynamics constraints + problem.SubjectTo(Fx_net == path.drivetrain.mass * ax.at(idx)); + problem.SubjectTo(Fy_net == path.drivetrain.mass * ay.at(idx)); + problem.SubjectTo(tau_net == path.drivetrain.moi * alpha.at(idx)); + } + + 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 + + Pose2v pose{x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}; + Translation2v linearVelocity{vx.at(idx), vy.at(idx)}; + auto angularVelocity = omega.at(idx); + + std::visit( + [&](auto&& arg) { + arg.Apply(problem, pose, linearVelocity, angularVelocity); + }, + 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 (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) { + Pose2v pose{x.at(idx), y.at(idx), {thetacos.at(idx), thetasin.at(idx)}}; + Translation2v linearVelocity{vx.at(idx), vy.at(idx)}; + auto angularVelocity = omega.at(idx); + + std::visit( + [&](auto&& arg) { + arg.Apply(problem, pose, linearVelocity, angularVelocity); + }, + constraint); + } + } + } + + ApplyInitialGuess(initialGuess); +} + +expected SwerveTrajectoryGenerator::Generate( + bool diagnostics) { + GetCancellationFlag() = 0; + problem.Callback([this](const sleipnir::SolverIterationInfo&) -> bool { + for (auto& callback : callbacks) { + callback(); + } + return trajopt::GetCancellationFlag(); + }); + + // tolerance of 1e-4 is 0.1 mm + auto status = problem.Solve({.tolerance = 1e-4, .diagnostics = diagnostics}); + + if (static_cast(status.exitCondition) < 0 || + status.exitCondition == + sleipnir::SolverExitCondition::kCallbackRequestedStop) { + return unexpected{std::string{sleipnir::ToMessage(status.exitCondition)}}; + } else { + return ConstructSwerveSolution(); + } +} + +void SwerveTrajectoryGenerator::ApplyInitialGuess( + const SwerveSolution& solution) { + size_t sampleTotal = x.size(); + for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { + x[sampleIndex].SetValue(solution.x[sampleIndex]); + y[sampleIndex].SetValue(solution.y[sampleIndex]); + thetacos[sampleIndex].SetValue(solution.thetacos[sampleIndex]); + thetasin[sampleIndex].SetValue(solution.thetasin[sampleIndex]); + } + + vx[0].SetValue(0.0); + vy[0].SetValue(0.0); + omega[0].SetValue(0.0); + ax[0].SetValue(0.0); + ay[0].SetValue(0.0); + alpha[0].SetValue(0.0); + + for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { + vx[sampleIndex].SetValue( + (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / + solution.dt[sampleIndex]); + vy[sampleIndex].SetValue( + (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / + solution.dt[sampleIndex]); + + double thetacos = solution.thetacos[sampleIndex]; + double thetasin = solution.thetasin[sampleIndex]; + double last_thetacos = solution.thetacos[sampleIndex - 1]; + double last_thetasin = solution.thetasin[sampleIndex - 1]; + + omega[sampleIndex].SetValue( + Rotation2d{thetacos, thetasin} + .RotateBy(-Rotation2d{last_thetacos, last_thetasin}) + .Radians() / + solution.dt[sampleIndex]); + + ax[sampleIndex].SetValue( + (vx[sampleIndex].Value() - vx[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + ay[sampleIndex].SetValue( + (vy[sampleIndex].Value() - vy[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + alpha[sampleIndex].SetValue( + (omega[sampleIndex].Value() - omega[sampleIndex - 1].Value()) / + solution.dt[sampleIndex]); + } +} + +SwerveSolution SwerveTrajectoryGenerator::ConstructSwerveSolution() { + 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); + double dt_val = dt_sgmt.Value(); + for (size_t i = 0; i < N_sgmt; ++i) { + dtPerSamp.push_back(dt_val); + } + } + + return SwerveSolution{dtPerSamp, + RowSolutionValue(x), + RowSolutionValue(y), + RowSolutionValue(thetacos), + RowSolutionValue(thetasin), + RowSolutionValue(vx), + RowSolutionValue(vy), + RowSolutionValue(omega), + RowSolutionValue(ax), + RowSolutionValue(ay), + RowSolutionValue(alpha), + MatrixSolutionValue(Fx), + MatrixSolutionValue(Fy)}; +} + +} // namespace trajopt diff --git a/src/lib.rs b/src/lib.rs index cd392419..a8c020ef 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -46,7 +46,6 @@ mod ffi { type SwervePathBuilderImpl; - fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); fn set_drivetrain(self: Pin<&mut SwervePathBuilderImpl>, drivetrain: &SwerveDrivetrain); fn set_bumpers(self: Pin<&mut SwervePathBuilderImpl>, length: f64, width: f64); fn set_control_interval_counts(self: Pin<&mut SwervePathBuilderImpl>, counts: Vec); @@ -89,12 +88,6 @@ mod ffi { idx: usize, magnitude: f64, ); - fn wpt_linear_velocity_polar( - self: Pin<&mut SwervePathBuilderImpl>, - idx: usize, - magnitude: f64, - angle: f64, - ); fn wpt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, idx: usize, @@ -105,9 +98,6 @@ mod ffi { idx: 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_point_at( self: Pin<&mut SwervePathBuilderImpl>, idx: usize, @@ -128,13 +118,6 @@ mod ffi { to_idx: usize, magnitude: f64, ); - fn sgmt_linear_velocity_polar( - self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, - magnitude: f64, - angle: f64, - ); fn sgmt_angular_velocity( self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, @@ -147,14 +130,6 @@ mod ffi { to_idx: 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_heading( - self: Pin<&mut SwervePathBuilderImpl>, - from_idx: usize, - to_idx: usize, - heading: f64, - ); fn sgmt_point_at( self: Pin<&mut SwervePathBuilderImpl>, from_idx: usize, @@ -187,11 +162,14 @@ mod ffi { diagnostics: bool, uuid: i64, ) -> Result; + fn add_progress_callback( self: Pin<&mut SwervePathBuilderImpl>, callback: fn(HolonomicTrajectory, i64), ); + fn cancel_all(self: Pin<&mut SwervePathBuilderImpl>); + fn new_swerve_path_builder_impl() -> UniquePtr; } } @@ -271,15 +249,6 @@ impl SwervePathBuilder { ); } - pub fn wpt_linear_velocity_polar(&mut self, idx: usize, magnitude: f64, angle: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_linear_velocity_polar( - self.path.pin_mut(), - idx, - magnitude, - angle, - ); - } - pub fn wpt_angular_velocity(&mut self, idx: usize, angular_velocity: f64) { crate::ffi::SwervePathBuilderImpl::wpt_angular_velocity( self.path.pin_mut(), @@ -296,18 +265,6 @@ impl SwervePathBuilder { ); } - pub fn wpt_x(&mut self, idx: usize, x: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_x(self.path.pin_mut(), idx, 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_heading(&mut self, idx: usize, heading: f64) { - crate::ffi::SwervePathBuilderImpl::wpt_heading(self.path.pin_mut(), idx, heading); - } - pub fn wpt_point_at( &mut self, idx: usize, @@ -347,22 +304,6 @@ impl SwervePathBuilder { ); } - pub fn sgmt_linear_velocity_polar( - &mut self, - from_idx: usize, - to_idx: usize, - magnitude: f64, - angle: f64, - ) { - crate::ffi::SwervePathBuilderImpl::sgmt_linear_velocity_polar( - self.path.pin_mut(), - from_idx, - to_idx, - magnitude, - angle, - ); - } - pub fn sgmt_angular_velocity(&mut self, from_idx: usize, to_idx: usize, angular_velocity: f64) { crate::ffi::SwervePathBuilderImpl::sgmt_angular_velocity( self.path.pin_mut(), @@ -386,23 +327,6 @@ impl SwervePathBuilder { ); } - 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_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_heading(&mut self, from_idx: usize, to_idx: usize, heading: f64) { - crate::ffi::SwervePathBuilderImpl::sgmt_heading( - self.path.pin_mut(), - from_idx, - to_idx, - heading, - ); - } - pub fn sgmt_point_at( &mut self, from_idx: usize, diff --git a/src/optimization/HolonomicTrajoptUtil.hpp b/src/optimization/HolonomicTrajoptUtil.hpp deleted file mode 100644 index 1b689aad..00000000 --- a/src/optimization/HolonomicTrajoptUtil.hpp +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include - -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/constraint/HeadingConstraint.hpp" -#include "trajopt/constraint/LinePointConstraint.hpp" -#include "trajopt/constraint/PointAtConstraint.hpp" -#include "trajopt/constraint/PointLineConstraint.hpp" -#include "trajopt/constraint/PointPointConstraint.hpp" -#include "trajopt/constraint/TranslationConstraint.hpp" -#include "trajopt/constraint/holonomic/HolonomicConstraint.hpp" - -namespace trajopt { - -inline void ApplyHolonomicConstraint( - sleipnir::OptimizationProblem& problem, const Pose2v& pose, - const Translation2v v, const sleipnir::Variable& omega, - [[maybe_unused]] const Translation2v& a, - [[maybe_unused]] const sleipnir::Variable& alpha, - const HolonomicConstraint& constraint) { - if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - ApplyConstraint(problem, pose, std::get(constraint)); - } else if (std::holds_alternative(constraint)) { - const auto& angularVelocityConstraint = - std::get(constraint); - ApplyIntervalSet1dConstraint( - problem, omega, angularVelocityConstraint.angularVelocityBound); - } else if (std::holds_alternative(constraint)) { - const auto& velocityHolonomicConstraint = - std::get(constraint); - ApplySet2dConstraint(problem, v, velocityHolonomicConstraint.velocityBound); - } else if (std::holds_alternative(constraint)) { - auto pointAtConstraint = std::get(constraint); - const auto& [fieldPoint, headingTolerance] = pointAtConstraint; - - // dx,dy = desired heading - // ux,uy = unit vector of desired heading - // hx,hy = heading - // dot = dot product of ux,uy and hx,hy - // - // constrain dot to cos(1.0), which is colinear - // and cos(thetaTolerance) - auto dx = fieldPoint.X() - pose.X(); - auto dy = fieldPoint.Y() - pose.Y(); - auto dot = pose.Rotation().Cos() * dx + pose.Rotation().Sin() * dy; - problem.SubjectTo(dot >= - std::cos(headingTolerance) * sleipnir::hypot(dx, dy)); - } -} - -} // namespace trajopt diff --git a/src/optimization/SwerveTrajoptUtil.hpp b/src/optimization/SwerveTrajoptUtil.hpp deleted file mode 100644 index 2aa04ad7..00000000 --- a/src/optimization/SwerveTrajoptUtil.hpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include - -#include -#include - -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/drivetrain/SwerveDrivetrain.hpp" -#include "trajopt/geometry/Translation2.hpp" -#include "trajopt/solution/SwerveSolution.hpp" - -namespace trajopt { - -inline std::pair SolveNetForce( - const std::vector& Fx, - const std::vector& Fy) { - return {std::accumulate(Fx.begin(), Fx.end(), sleipnir::Variable{0.0}), - std::accumulate(Fy.begin(), Fy.end(), sleipnir::Variable{0.0})}; -} - -inline sleipnir::Variable SolveNetTorque( - const Rotation2v& theta, const std::vector& Fx, - const std::vector& Fy, - const std::vector& swerveModules) { - sleipnir::Variable tau_net = 0; - - for (size_t moduleIdx = 0; moduleIdx < swerveModules.size(); ++moduleIdx) { - auto& swerveModule = swerveModules.at(moduleIdx); - - const auto& [x_m, y_m] = swerveModule.translation.RotateBy(theta); - Translation2v r{x_m, y_m}; - - Translation2v F{Fx.at(moduleIdx), Fy.at(moduleIdx)}; - - tau_net += r.Cross(F); - } - - return tau_net; -} - -inline void ApplyKinematicsConstraints( - sleipnir::OptimizationProblem& problem, - const std::vector& x, - const std::vector& y, - const std::vector& thetacos, - const std::vector& thetasin, - const std::vector& vx, - const std::vector& vy, - const std::vector& omega, - const std::vector& ax, - const std::vector& ay, - const std::vector& alpha, - 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); - - problem.SubjectTo(x_n_1 + vx_n * dt_sgmt == x_n); - problem.SubjectTo(y_n_1 + vy_n * dt_sgmt == y_n); - - auto theta_diff = theta_n - theta_n_1; - - // Constrain angle equality on manifold: theta_diff = omega_n * dt_sgmt. - // - // Let a = . NOLINT - // Let b = . NOLINT - // - // If the angles are equal, the angle between the unit vectors should be - // zero. - // - // a x b = ||a|| ||b|| sin(angleBetween) NOLINT - // = 1 * 1 * 0 - // = 0 - // - // NOTE: angleBetween = π rad would be another solution - Translation2v a{theta_diff.Cos(), theta_diff.Sin()}; - Translation2v b{sleipnir::cos(omega_n * dt_sgmt), - sleipnir::sin(omega_n * dt_sgmt)}; - problem.SubjectTo(a.Cross(b) == 0); - problem.SubjectTo( - Translation2v{theta_n_1.Cos(), theta_n_1.Sin()}.SquaredNorm() == 1); - - problem.SubjectTo(vx_n_1 + ax_n * dt_sgmt == vx_n); - 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) == - 1); - } -} - -/** - * Applies the drivetrain-specific constraints to the optimizer. These - * constraints prevent motors from spinning too fast or with too much power. - * For swerve, this applies constraints that connect the speed and direction - * of each swerve module wheel to the overall kinematics and dynamics of the - * system. There are two parts: the velocity of each wheel is connected to the - * velocity of the robot, and the force generated by each wheel is connected - * to the acceleration of the robot. For both of these, limits are placed on - * the speed and torque of each wheel. This allows the optimizer to generate - * an efficient, smooth path that the robot can follow. - * - * @param problem the current optimization problem to which to apply constraints - * @param theta (controlIntervalTotal + 1) x 1 column vector of the robot's - * heading for each sample point - * @param vx (controlIntervalTotal + 1) x 1 column vector of the x-coordinate - * of the robot's velocity for each sample point - * @param vy (controlIntervalTotal + 1) x 1 column vector of the y-coordinate - * of the robot's velocity for each sample point - * @param omega (controlIntervalTotal + 1) x 1 column vector of the robot's - * angular velocity for each sample point - * @param ax controlIntervalTotal x 1 column vector of the x-coordinate of the - * robot's acceleration for each sample point - * @param ay controlIntervalTotal x 1 column vector of the y-coordinate of the - * robot's acceleration for each sample point - * @param alpha controlIntervalTotal x 1 column vector of the robot's angular - * velocity for each sample point - * @param swerveDrivetrain the swerve drivetrain - */ -inline void ApplyDynamicsConstraints( - sleipnir::OptimizationProblem& problem, const sleipnir::Variable& ax, - const sleipnir::Variable& ay, const sleipnir::Variable& alpha, - const sleipnir::Variable& Fx_net, const sleipnir::Variable& Fy_net, - const sleipnir::Variable& tau_net, double mass, double moi) { - problem.SubjectTo(Fx_net == mass * ax); - problem.SubjectTo(Fy_net == mass * ay); - problem.SubjectTo(tau_net == moi * alpha); -} - -inline void ApplyPowerConstraints(sleipnir::OptimizationProblem& problem, - const Rotation2v& theta, - const Translation2v& v, - const sleipnir::Variable& omega, - const std::vector& Fx, - const std::vector& Fy, - const SwerveDrivetrain& swerveDrivetrain) { - const auto [vx_prime, vy_prime] = v.RotateBy(-theta); - - size_t moduleCount = swerveDrivetrain.modules.size(); - - std::vector vx_m; - std::vector vy_m; - 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; - 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); - 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); - problem.SubjectTo(_vx_m * _vx_m + _vy_m * _vy_m <= - maxWheelVelocity * maxWheelVelocity); - - problem.SubjectTo(Fx_m * Fx_m + Fy_m * Fy_m <= maxForce * maxForce); - } -} - -inline SwerveSolution ConstructSwerveSolution( - std::vector& x, std::vector& y, - std::vector& thetacos, - std::vector& thetasin, - std::vector& vx, std::vector& vy, - std::vector& omega, std::vector& ax, - std::vector& ay, std::vector& alpha, - std::vector>& Fx, - 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); - double dt_val = dt_sgmt.Value(); - for (size_t i = 0; i < N_sgmt; ++i) { - dtPerSamp.push_back(dt_val); - } - } - return SwerveSolution{ - {{dtPerSamp, RowSolutionValue(x), RowSolutionValue(y), - RowSolutionValue(thetacos), RowSolutionValue(thetasin)}, - RowSolutionValue(vx), - RowSolutionValue(vy), - RowSolutionValue(omega), - RowSolutionValue(ax), - RowSolutionValue(ay), - RowSolutionValue(alpha)}, - MatrixSolutionValue(Fx), - MatrixSolutionValue(Fy)}; -} - -} // namespace trajopt diff --git a/src/optimization/TrajoptUtil.hpp b/src/optimization/TrajoptUtil.hpp deleted file mode 100644 index 2f48ec14..00000000 --- a/src/optimization/TrajoptUtil.hpp +++ /dev/null @@ -1,440 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -#include "trajopt/constraint/Constraint.hpp" -#include "trajopt/constraint/LinePointConstraint.hpp" -#include "trajopt/constraint/PointLineConstraint.hpp" -#include "trajopt/constraint/PointPointConstraint.hpp" -#include "trajopt/constraint/TranslationConstraint.hpp" -#include "trajopt/geometry/Pose2.hpp" -#include "trajopt/set/ConeSet2d.hpp" -#include "trajopt/set/EllipticalSet2d.hpp" -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/set/LinearSet2d.hpp" -#include "trajopt/set/RectangularSet2d.hpp" -#include "trajopt/set/Set2d.hpp" -#include "trajopt/solution/Solution.hpp" - -namespace trajopt { - -inline sleipnir::Variable max(const sleipnir::Variable& a, - const sleipnir::Variable& b) { - return +0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + a; -} - -inline sleipnir::Variable min(const sleipnir::Variable& a, - const sleipnir::Variable& b) { - return -0.5 * (1 + sleipnir::sign(b - a)) * (b - a) + b; -} - -/** - * @brief Get the index of an item in a decision variable array, given the - * waypoint and sample indices and whether this array includes an entry - * 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 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; - } - for (size_t _wptIdx = 1; _wptIdx < wptIdx; ++_wptIdx) { - idx += N.at(_wptIdx - 1); - } - idx += sampIdx; - return idx; -} - -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); - auto T_sgmt = dt_sgmt * static_cast(N_sgmt); - T_tot += T_sgmt; - - problem.SubjectTo(dt_sgmt >= 0); - dt_sgmt.SetValue(5.0 / N_sgmt); - } - problem.Minimize(std::move(T_tot)); -} - -inline void ApplyIntervalSet1dConstraint(sleipnir::OptimizationProblem& problem, - const sleipnir::Variable& scalar, - const IntervalSet1d& set1d) { - if (set1d.IsExact()) { - problem.SubjectTo(scalar == set1d.lower); - } else { - if (set1d.IsLowerBounded()) { - problem.SubjectTo(scalar >= set1d.lower); - } - if (set1d.IsUpperBounded()) { - problem.SubjectTo(scalar <= set1d.upper); - } - } -} - -inline void ApplySet2dConstraint(sleipnir::OptimizationProblem& problem, - const Translation2v& vector, - const Set2d& set2d) { - if (std::holds_alternative(set2d)) { - auto& rectangularSet2d = std::get(set2d); - - ApplyIntervalSet1dConstraint(problem, vector.X(), rectangularSet2d.xBound); - ApplyIntervalSet1dConstraint(problem, vector.Y(), rectangularSet2d.yBound); - } else if (std::holds_alternative(set2d)) { - auto& linearSet2d = std::get(set2d); - Rotation2d theta{std::sin(linearSet2d.theta), std::cos(linearSet2d.theta)}; - - problem.SubjectTo(vector.X() * theta.Sin() == vector.Y() * theta.Cos()); - } else if (std::holds_alternative(set2d)) { - auto& ellipticalSet2d = std::get(set2d); - - auto scaledVectorXSquared = - (vector.X() * vector.X()) / - (ellipticalSet2d.xRadius * ellipticalSet2d.xRadius); - auto scaledVectorYSquared = - (vector.Y() * vector.Y()) / - (ellipticalSet2d.yRadius * ellipticalSet2d.yRadius); - auto lhs = scaledVectorXSquared + scaledVectorYSquared; - using enum EllipticalSet2d::Direction; - switch (ellipticalSet2d.direction) { - case kInside: - problem.SubjectTo(lhs <= 1.0); - break; - case kCentered: - problem.SubjectTo(lhs == 1.0); - break; - case kOutside: - problem.SubjectTo(lhs >= 1.0); - break; - } - } else if (std::holds_alternative(set2d)) { - auto& coneSet2d = std::get(set2d); - - problem.SubjectTo(vector.X() * sleipnir::sin(coneSet2d.thetaBound.upper) >= - vector.Y() * sleipnir::cos(coneSet2d.thetaBound.upper)); - problem.SubjectTo(vector.X() * sleipnir::sin(coneSet2d.thetaBound.lower) <= - vector.Y() * sleipnir::cos(coneSet2d.thetaBound.lower)); - } else if (std::holds_alternative(set2d)) { - auto& manifoldSet2d = std::get(set2d); - Translation2d mid{std::cos(manifoldSet2d.middle), - std::sin(manifoldSet2d.middle)}; - - auto dot = mid.Dot(vector); - // middle ⋅ vector ≥ std::cos(tolerance) ||vector|| - if (manifoldSet2d.tolerance == 0) { - problem.SubjectTo(dot == sleipnir::hypot(vector.X(), vector.Y())); - } else if (manifoldSet2d.tolerance < std::numbers::pi) { - problem.SubjectTo(dot >= std::cos(manifoldSet2d.tolerance) * - sleipnir::hypot(vector.X(), vector.Y())); - } - // wider tolerances permit the whole circle, so do nothing - } -} - -inline std::vector RowSolutionValue( - std::vector& rowVector) { - std::vector valueRowVector; - valueRowVector.reserve(rowVector.size()); - for (auto& expression : rowVector) { - valueRowVector.push_back(expression.Value()); - } - return valueRowVector; -} - -inline std::vector> MatrixSolutionValue( - std::vector>& matrix) { - std::vector> valueMatrix; - valueMatrix.reserve(matrix.size()); - for (auto& row : matrix) { - valueMatrix.push_back(RowSolutionValue(row)); - } - return valueMatrix; -} - -template -inline auto Lerp(T a, U b, V t) { - return a + t * (b - a); -} - -// https://www.desmos.com/calculator/cqmc1tjtsv -template -decltype(auto) linePointDist(const Translation2& lineStart, - const Translation2& lineEnd, - const Translation2& point) { - using R = decltype(std::declval() + std::declval()); - - Translation2 l{lineEnd.X() - lineStart.X(), lineEnd.Y() - lineStart.Y()}; - Translation2 v{point.X() - lineStart.X(), point.Y() - lineStart.Y()}; - - auto t = v.Dot(l) / l.SquaredNorm(); - auto tBounded = max(min(t, 1), 0); // NOLINT - Translation2 i{Lerp(lineStart.X(), lineEnd.X(), tBounded), - Lerp(lineStart.Y(), lineEnd.Y(), tBounded)}; - return (i - point).SquaredNorm(); -} - -inline void ApplyConstraint(sleipnir::OptimizationProblem& problem, - const Pose2v& pose, const Constraint& constraint) { - if (std::holds_alternative(constraint)) { - auto& translationConstraint = std::get(constraint); - - ApplySet2dConstraint(problem, {pose.X(), pose.Y()}, - translationConstraint.translationBound); - } else if (std::holds_alternative(constraint)) { - auto& headingConstraint = std::get(constraint); - - ApplySet2dConstraint(problem, - {pose.Rotation().Cos(), pose.Rotation().Sin()}, - headingConstraint.headingBound); - } else if (std::holds_alternative(constraint)) { - auto linePointConstraint = std::get(constraint); - const auto& [robotLineStart, robotLineEnd, fieldPoint, distance] = - linePointConstraint; - - auto lineStart = - pose.Translation() + robotLineStart.RotateBy(pose.Rotation()); - auto lineEnd = pose.Translation() + robotLineEnd.RotateBy(pose.Rotation()); - auto dist = - linePointDist(lineStart, lineEnd, linePointConstraint.fieldPoint); - auto distSquared = dist * dist; - auto& distInterval = linePointConstraint.distance; - auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), - std::pow(distInterval.upper, 2)); - ApplyIntervalSet1dConstraint(problem, distSquared, distIntervalSquared); - } else if (std::holds_alternative(constraint)) { - auto pointLineConstraint = std::get(constraint); - const auto& [robotPoint, fieldLineStart, fieldLineEnd, distance] = - pointLineConstraint; - - auto point = pose.Translation() + robotPoint.RotateBy(pose.Rotation()); - auto dist = linePointDist(fieldLineStart, fieldLineEnd, point); - auto distSquared = dist * dist; - auto& distInterval = pointLineConstraint.distance; - auto distIntervalSquared = IntervalSet1d(std::pow(distInterval.lower, 2), - std::pow(distInterval.upper, 2)); - ApplyIntervalSet1dConstraint(problem, distSquared, distIntervalSquared); - } else if (std::holds_alternative(constraint)) { - auto pointPointConstraint = std::get(constraint); - const auto& [robotPoint, fieldPoint, distance] = pointPointConstraint; - - auto bumperCorner = - pose.Translation() + robotPoint.RotateBy(pose.Rotation()); - auto dx = fieldPoint.X() - bumperCorner.X(); - auto dy = fieldPoint.Y() - bumperCorner.Y(); - auto pointDistSquared = dx * dx + dy * dy; - IntervalSet1d distSquared = pointPointConstraint.distance; - distSquared.lower *= distSquared.lower; - distSquared.upper *= distSquared.upper; - ApplyIntervalSet1dConstraint(problem, pointDistSquared, distSquared); - } -} - -inline std::vector Linspace(double startValue, double endValue, - size_t numSamples) { - std::vector result; - double delta = (endValue - startValue) / numSamples; - for (size_t index = 1; index <= numSamples; index++) { - result.push_back(startValue + index * delta); - } - return result; -} - -inline std::vector AngleLinspace(double startValue, double endValue, - size_t numSamples) { - auto diff = endValue - startValue; - // angleModulus - const double modulus = 2 * std::numbers::pi; - const double minimumInput = -std::numbers::pi; - const double maximumInput = std::numbers::pi; - // Wrap input if it's above the maximum input - const double numMax = std::trunc((diff - minimumInput) / modulus); - diff -= numMax * modulus; - - // Wrap input if it's below the minimum input - const double numMin = std::trunc((diff - maximumInput) / modulus); - diff -= numMin * modulus; - - return Linspace(startValue, startValue + diff, numSamples); -} - -template -inline void append_vector(std::vector& base, - const std::vector& newItems) { - base.insert(base.end(), newItems.begin(), newItems.end()); -} - -inline Solution GenerateLinearInitialGuess( - const std::vector>& initialGuessPoints, - const std::vector controlIntervalCounts) { - size_t wptCnt = controlIntervalCounts.size() + 1; - size_t sampTot = GetIdx(controlIntervalCounts, wptCnt, 0); - - Solution initialGuess; - - initialGuess.x.reserve(sampTot); - initialGuess.y.reserve(sampTot); - initialGuess.thetacos.reserve(sampTot); - initialGuess.thetasin.reserve(sampTot); - initialGuess.dt.reserve(sampTot); - - initialGuess.x.push_back(initialGuessPoints.front().front().X()); - initialGuess.y.push_back(initialGuessPoints.front().front().Y()); - initialGuess.thetacos.push_back( - initialGuessPoints.front().front().Rotation().Cos()); - initialGuess.thetasin.push_back( - initialGuessPoints.front().front().Rotation().Sin()); - - for (size_t i = 0; i < sampTot; i++) { - 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(); - 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)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx - 1).back().Y(), - initialGuessPoints.at(wptIdx).front().Y(), N_guessSgmt)); - auto wptThetas = AngleLinspace( - initialGuessPoints.at(wptIdx - 1).back().Rotation().Radians(), - initialGuessPoints.at(wptIdx).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 - append_vector( - initialGuess.x, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).X(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).X(), - N_guessSgmt)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointIdx - 1).Y(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).Y(), - N_guessSgmt)); - auto guessThetas = AngleLinspace( - initialGuessPoints.at(wptIdx) - .at(guessPointIdx - 1) - .Rotation() - .Radians(), - initialGuessPoints.at(wptIdx).at(guessPointIdx).Rotation().Radians(), - N_guessSgmt); - for (auto theta : guessThetas) { - initialGuess.thetacos.push_back(std::cos(theta)); - initialGuess.thetasin.push_back(std::sin(theta)); - } - } - if (guessPointCount > 1) { // if two or more guess points - 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)); - append_vector( - initialGuess.y, - Linspace(initialGuessPoints.at(wptIdx).at(guessPointCount - 2).Y(), - initialGuessPoints.at(wptIdx).back().Y(), N_lastGuessSgmt)); - auto lastThetas = AngleLinspace( - initialGuessPoints.at(wptIdx) - .at(guessPointCount - 2) - .Rotation() - .Radians(), - initialGuessPoints.at(wptIdx).back().Rotation().Radians(), - N_lastGuessSgmt); - for (auto theta : lastThetas) { - initialGuess.thetacos.push_back(std::cos(theta)); - initialGuess.thetasin.push_back(std::sin(theta)); - } - } - } - - return initialGuess; -} - -inline void ApplyInitialGuess(const Solution& solution, - std::vector& x, - std::vector& y, - std::vector& thetacos, - std::vector& thetasin, - std::vector& vx, - std::vector& vy, - std::vector& omega, - std::vector& ax, - std::vector& ay, - std::vector& alpha) { - size_t sampleTotal = x.size(); - for (size_t sampleIndex = 0; sampleIndex < sampleTotal; sampleIndex++) { - x[sampleIndex].SetValue(solution.x[sampleIndex]); - y[sampleIndex].SetValue(solution.y[sampleIndex]); - thetacos[sampleIndex].SetValue(solution.thetacos[sampleIndex]); - thetasin[sampleIndex].SetValue(solution.thetasin[sampleIndex]); - } - - vx[0].SetValue(0.0); - vy[0].SetValue(0.0); - omega[0].SetValue(0.0); - ax[0].SetValue(0.0); - ay[0].SetValue(0.0); - alpha[0].SetValue(0.0); - - for (size_t sampleIndex = 1; sampleIndex < sampleTotal; sampleIndex++) { - vx[sampleIndex].SetValue( - (solution.x[sampleIndex] - solution.x[sampleIndex - 1]) / - solution.dt[sampleIndex]); - vy[sampleIndex].SetValue( - (solution.y[sampleIndex] - solution.y[sampleIndex - 1]) / - solution.dt[sampleIndex]); - - double thetacos = solution.thetacos[sampleIndex]; - double thetasin = solution.thetasin[sampleIndex]; - double last_thetacos = solution.thetacos[sampleIndex - 1]; - double last_thetasin = solution.thetasin[sampleIndex - 1]; - - omega[sampleIndex].SetValue( - Rotation2d{thetacos, thetasin} - .RotateBy(-Rotation2d{last_thetacos, last_thetasin}) - .Radians() / - solution.dt[sampleIndex]); - - ax[sampleIndex].SetValue( - (vx[sampleIndex].Value() - vx[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - ay[sampleIndex].SetValue( - (vy[sampleIndex].Value() - vy[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - alpha[sampleIndex].SetValue( - (omega[sampleIndex].Value() - omega[sampleIndex - 1].Value()) / - solution.dt[sampleIndex]); - } -} - -} // namespace trajopt diff --git a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp b/src/optimization/algorithms/SwerveDiscreteOptimal.hpp deleted file mode 100644 index 1bfad996..00000000 --- a/src/optimization/algorithms/SwerveDiscreteOptimal.hpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "optimization/Cancellation.hpp" -#include "optimization/HolonomicTrajoptUtil.hpp" -#include "optimization/SwerveTrajoptUtil.hpp" -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/drivetrain/SwerveDrivetrain.hpp" -#include "trajopt/expected" -#include "trajopt/path/Path.hpp" -#include "trajopt/solution/SwerveSolution.hpp" - -namespace trajopt { - -class SwerveDiscreteOptimal { - public: - /** - * Construct a new swerve trajectory optimization problem. - */ - SwerveDiscreteOptimal(const SwervePath& path, const std::vector& N, - const Solution& initialGuess, int64_t handle = 0) - : path(path), N(N) { - callbacks.emplace_back([this, handle = handle] { - constexpr int fps = 60; - constexpr std::chrono::duration timePerFrame{1.0 / fps}; - - // FPS limit on sending updates - static auto lastFrameTime = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); - if (now - lastFrameTime < timePerFrame) { - return; - } - - lastFrameTime = now; - - auto soln = - ConstructSwerveSolution(x, y, thetacos, thetasin, vx, vy, omega, ax, - ay, alpha, Fx, Fy, dt, this->N); - for (auto& callback : this->path.callbacks) { - callback(soln, handle); - } - }); - size_t wptCnt = 1 + N.size(); - size_t sgmtCnt = N.size(); - size_t sampTot = GetIdx(N, wptCnt, 0); - size_t moduleCnt = path.drivetrain.modules.size(); - - x.reserve(sampTot); - y.reserve(sampTot); - thetacos.reserve(sampTot); - thetasin.reserve(sampTot); - vx.reserve(sampTot); - vy.reserve(sampTot); - omega.reserve(sampTot); - ax.reserve(sampTot); - ay.reserve(sampTot); - alpha.reserve(sampTot); - - Fx.reserve(sampTot); - Fy.reserve(sampTot); - for (size_t sampIdx = 0; sampIdx < sampTot; ++sampIdx) { - auto& _Fx = Fx.emplace_back(); - auto& _Fy = Fy.emplace_back(); - _Fx.reserve(moduleCnt); - _Fy.reserve(moduleCnt); - } - - dt.reserve(sgmtCnt); - - for (size_t idx = 0; idx < sampTot; ++idx) { - x.emplace_back(problem.DecisionVariable()); - y.emplace_back(problem.DecisionVariable()); - thetacos.emplace_back(problem.DecisionVariable()); - thetasin.emplace_back(problem.DecisionVariable()); - vx.emplace_back(problem.DecisionVariable()); - vy.emplace_back(problem.DecisionVariable()); - omega.emplace_back(problem.DecisionVariable()); - ax.emplace_back(problem.DecisionVariable()); - 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()); - } - } - - double minWidth = INFINITY; - for (size_t i = 1; i < path.drivetrain.modules.size(); i++) { - if (std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X()) != 0) { - minWidth = std::min( - minWidth, - std::abs(path.drivetrain.modules.at(i - 1).translation.X() - - path.drivetrain.modules.at(i).translation.X())); - } - if (std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y()) != 0) { - minWidth = std::min( - minWidth, - std::abs(path.drivetrain.modules.at(i - 1).translation.Y() - - path.drivetrain.modules.at(i).translation.Y())); - } - } - - for (size_t sgmtIdx = 0; sgmtIdx < sgmtCnt; ++sgmtIdx) { - dt.emplace_back(problem.DecisionVariable()); - for (auto module : path.drivetrain.modules) { - problem.SubjectTo(dt.at(sgmtIdx) * module.wheelRadius * - module.wheelMaxAngularVelocity <= - minWidth); - } - } - - ApplyDiscreteTimeObjective(problem, dt, N); - 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)); - 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), - 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); - } - - 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); - } - } - // 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 (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) { - 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); - } - } - } - - ApplyInitialGuess(initialGuess, x, y, thetacos, thetasin, vx, vy, omega, ax, - ay, alpha); - } - - /** - * Generates an optimal trajectory. - * - * This function may take a long time to complete. - * - * @param diagnostics Enables diagnostic prints. - * @return Returns a holonomic trajectory on success, or a string containing a - * failure reason. - */ - expected Generate(bool diagnostics = false) { - GetCancellationFlag() = 0; - problem.Callback([=, this](const sleipnir::SolverIterationInfo&) -> bool { - for (auto& callback : callbacks) { - callback(); - } - return trajopt::GetCancellationFlag(); - }); - - // tolerance of 1e-4 is 0.1 mm - auto status = - problem.Solve({.tolerance = 1e-4, .diagnostics = diagnostics}); - - if (static_cast(status.exitCondition) < 0 || - status.exitCondition == - sleipnir::SolverExitCondition::kCallbackRequestedStop) { - return unexpected{std::string{sleipnir::ToMessage(status.exitCondition)}}; - } else { - return ConstructSwerveSolution(x, y, thetacos, thetasin, vx, vy, omega, - ax, ay, alpha, Fx, Fy, dt, N); - } - } - - private: - /** - * The swerve drivetrain. - */ - const SwervePath& path; - - /// State Variables - std::vector x; - std::vector y; - std::vector thetacos; - std::vector thetasin; - std::vector vx; - std::vector vy; - std::vector omega; - std::vector ax; - std::vector ay; - std::vector alpha; - - /// Input Variables - std::vector> Fx; - std::vector> Fy; - - /// Time Variables - std::vector dt; - - /// Discretization Constants - const std::vector& N; - - sleipnir::OptimizationProblem problem; - std::vector> callbacks; -}; - -} // namespace trajopt diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp index a60356bf..8987080f 100644 --- a/src/path/SwervePathBuilder.cpp +++ b/src/path/SwervePathBuilder.cpp @@ -2,28 +2,25 @@ #include "trajopt/path/SwervePathBuilder.hpp" -#include -#include #include -#include "optimization/Cancellation.hpp" -#include "optimization/TrajoptUtil.hpp" -#include "trajopt/constraint/AngularVelocityConstraint.hpp" -#include "trajopt/constraint/Constraint.hpp" -#include "trajopt/constraint/HeadingConstraint.hpp" #include "trajopt/constraint/LinePointConstraint.hpp" #include "trajopt/constraint/PointLineConstraint.hpp" -#include "trajopt/constraint/TranslationConstraint.hpp" -#include "trajopt/constraint/holonomic/HolonomicVelocityConstraint.hpp" +#include "trajopt/constraint/PointPointConstraint.hpp" +#include "trajopt/constraint/PoseEqualityConstraint.hpp" +#include "trajopt/constraint/TranslationEqualityConstraint.hpp" #include "trajopt/obstacle/Obstacle.hpp" -#include "trajopt/set/EllipticalSet2d.hpp" -#include "trajopt/set/IntervalSet1d.hpp" -#include "trajopt/set/LinearSet2d.hpp" -#include "trajopt/set/RectangularSet2d.hpp" +#include "trajopt/solution/SwerveSolution.hpp" +#include "trajopt/util/Cancellation.hpp" +#include "trajopt/util/GenerateLinearInitialGuess.hpp" namespace trajopt { -const SwervePath& SwervePathBuilder::GetPath() const { +void SwervePathBuilder::CancelAll() { + trajopt::GetCancellationFlag() = 1; +} + +SwervePath& SwervePathBuilder::GetPath() { return path; } @@ -33,162 +30,156 @@ void SwervePathBuilder::SetDrivetrain(SwerveDrivetrain drivetrain) { void SwervePathBuilder::PoseWpt(size_t index, double x, double y, double heading) { - NewWpts(index); - path.waypoints.at(index).waypointConstraints.emplace_back( - TranslationConstraint{RectangularSet2d{x, y}}); - path.waypoints.at(index).waypointConstraints.emplace_back( - HeadingConstraint{heading}); + WptConstraint(index, PoseEqualityConstraint{x, y, heading}); WptInitialGuessPoint(index, {x, y, {heading}}); } void SwervePathBuilder::TranslationWpt(size_t index, double x, double y, double headingGuess) { - NewWpts(index); - path.waypoints.at(index).waypointConstraints.emplace_back( - TranslationConstraint{RectangularSet2d{x, y}}); + WptConstraint(index, TranslationEqualityConstraint{x, 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::WptVelocityMagnitude(size_t idx, double v) { - if (std::abs(v) < 1e-4) { - WptZeroVelocity(idx); - } else { - WptConstraint(idx, - HolonomicVelocityConstraint{EllipticalSet2d::CircularSet2d(v), - CoordinateSystem::kField}); - } -} - -void SwervePathBuilder::WptZeroVelocity(size_t idx) { - WptConstraint(idx, 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::WptAngularVelocity(size_t idx, - double angular_velocity) { - WptConstraint(idx, AngularVelocityConstraint{angular_velocity}); -} - -void SwervePathBuilder::WptAngularVelocityMaxMagnitude( - size_t idx, double angular_velocity) { - WptConstraint(idx, AngularVelocityConstraint{ - IntervalSet1d{-angular_velocity, angular_velocity}}); -} - -void SwervePathBuilder::WptZeroAngularVelocity(size_t idx) { - WptConstraint(idx, AngularVelocityConstraint{0.0}); -} - -void SwervePathBuilder::SgmtVelocityDirection(size_t fromIdx, size_t toIdx, - double angle, bool includeWpts) { - SgmtConstraint( - fromIdx, toIdx, - HolonomicVelocityConstraint{LinearSet2d{angle}, CoordinateSystem::kField}, - includeWpts); -} - -void SwervePathBuilder::SgmtVelocityMagnitude(size_t fromIdx, size_t toIdx, - 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, - HolonomicVelocityConstraint{set, CoordinateSystem::kField}, - includeWpts); -} - -void SwervePathBuilder::SgmtAngularVelocity(size_t fromIdx, size_t toIdx, - double angular_velocity, - bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, AngularVelocityConstraint{angular_velocity}, - includeWpts); -} - -void SwervePathBuilder::SgmtAngularVelocityMaxMagnitude(size_t fromIdx, - size_t toIdx, - double angular_velocity, - bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, - AngularVelocityConstraint{ - IntervalSet1d{-angular_velocity, angular_velocity}}, - includeWpts); -} - -void SwervePathBuilder::SgmtZeroAngularVelocity(size_t fromIdx, size_t toIdx, - bool includeWpts) { - SgmtConstraint(fromIdx, toIdx, AngularVelocityConstraint{0.0}, includeWpts); +void SwervePathBuilder::AddBumpers(Bumpers&& newBumpers) { + bumpers.emplace_back(std::move(newBumpers)); } -void SwervePathBuilder::WptConstraint(size_t idx, - const HolonomicConstraint& constraint) { - NewWpts(idx); - path.waypoints.at(idx).waypointConstraints.push_back(constraint); -} +void SwervePathBuilder::WptObstacle(size_t index, const Obstacle& obstacle) { + for (auto& _bumpers : bumpers) { + auto minDistance = _bumpers.safetyDistance + obstacle.safetyDistance; + + size_t bumperCornerCount = _bumpers.points.size(); + size_t obstacleCornerCount = obstacle.points.size(); + if (bumperCornerCount == 1 && obstacleCornerCount == 1) { + // if the bumpers and obstacle are only one point + WptConstraint(index, + PointPointConstraint{_bumpers.points.at(0), + obstacle.points.at(0), minDistance}); + return; + } -void SwervePathBuilder::SgmtConstraint(size_t fromIdx, size_t toIdx, - const HolonomicConstraint& constraint, - bool includeWpts) { - assert(fromIdx < toIdx); + // robot bumper edge to obstacle point constraints + for (auto& obstaclePoint : obstacle.points) { + // First apply constraint for all but last edge + for (size_t bumperCornerIndex = 0; + bumperCornerIndex < bumperCornerCount - 1; bumperCornerIndex++) { + WptConstraint(index, LinePointConstraint{ + _bumpers.points.at(bumperCornerIndex), + _bumpers.points.at(bumperCornerIndex + 1), + obstaclePoint, minDistance}); + } + // apply to last edge: the edge connecting the last point to the first + // must have at least three points to need this + if (bumperCornerCount >= 3) { + WptConstraint(index, + LinePointConstraint{ + _bumpers.points.at(bumperCornerCount - 1), + _bumpers.points.at(0), obstaclePoint, minDistance}); + } + } - NewWpts(toIdx); - if (includeWpts) { - path.waypoints.at(fromIdx).waypointConstraints.push_back(constraint); - } - for (size_t idx = fromIdx + 1; idx <= toIdx; idx++) { - if (includeWpts) { - path.waypoints.at(idx).waypointConstraints.push_back(constraint); + // obstacle edge to bumper corner constraints + for (auto& bumperCorner : _bumpers.points) { + if (obstacleCornerCount > 1) { + for (size_t obstacleCornerIndex = 0; + obstacleCornerIndex < obstacleCornerCount - 1; + obstacleCornerIndex++) { + WptConstraint( + index, + PointLineConstraint{ + bumperCorner, obstacle.points.at(obstacleCornerIndex), + obstacle.points.at(obstacleCornerIndex + 1), minDistance}); + } + if (obstacleCornerCount >= 3) { + WptConstraint(index, PointLineConstraint{ + bumperCorner, + obstacle.points.at(bumperCornerCount - 1), + obstacle.points.at(0), minDistance}); + } + } else { + WptConstraint(index, + PointPointConstraint{bumperCorner, obstacle.points.at(0), + minDistance}); + } } - path.waypoints.at(idx).segmentConstraints.push_back(constraint); } } -void SwervePathBuilder::AddBumpers(Bumpers&& newBumpers) { - bumpers.emplace_back(std::move(newBumpers)); -} - -void SwervePathBuilder::WptObstacle(size_t idx, const Obstacle& obstacle) { +void SwervePathBuilder::SgmtObstacle(size_t fromIndex, size_t toIndex, + const Obstacle& obstacle) { for (auto& _bumpers : bumpers) { - auto constraints = GetConstraintsForObstacle(_bumpers, obstacle); - for (auto& constraint : constraints) { - WptConstraint(idx, constraint); + auto minDistance = _bumpers.safetyDistance + obstacle.safetyDistance; + + size_t bumperCornerCount = _bumpers.points.size(); + size_t obstacleCornerCount = obstacle.points.size(); + if (bumperCornerCount == 1 && obstacleCornerCount == 1) { + // if the bumpers and obstacle are only one point + SgmtConstraint(fromIndex, toIndex, + PointPointConstraint{_bumpers.points.at(0), + obstacle.points.at(0), minDistance}); + return; } - } -} -void SwervePathBuilder::SgmtObstacle(size_t fromIdx, size_t toIdx, - const Obstacle& obstacle, - bool includeWpts) { - for (auto& _bumpers : bumpers) { - auto constraints = GetConstraintsForObstacle(_bumpers, obstacle); - for (auto& constraint : constraints) { - SgmtConstraint(fromIdx, toIdx, constraint, includeWpts); + // robot bumper edge to obstacle point constraints + for (auto& obstaclePoint : obstacle.points) { + // First apply constraint for all but last edge + for (size_t bumperCornerIndex = 0; + bumperCornerIndex < bumperCornerCount - 1; bumperCornerIndex++) { + SgmtConstraint( + fromIndex, toIndex, + LinePointConstraint{_bumpers.points.at(bumperCornerIndex), + _bumpers.points.at(bumperCornerIndex + 1), + obstaclePoint, minDistance}); + } + // apply to last edge: the edge connecting the last point to the first + // must have at least three points to need this + if (bumperCornerCount >= 3) { + SgmtConstraint(fromIndex, toIndex, + LinePointConstraint{ + _bumpers.points.at(bumperCornerCount - 1), + _bumpers.points.at(0), obstaclePoint, minDistance}); + } + } + + // obstacle edge to bumper corner constraints + for (auto& bumperCorner : _bumpers.points) { + if (obstacleCornerCount > 1) { + for (size_t obstacleCornerIndex = 0; + obstacleCornerIndex < obstacleCornerCount - 1; + obstacleCornerIndex++) { + SgmtConstraint( + fromIndex, toIndex, + PointLineConstraint{ + bumperCorner, obstacle.points.at(obstacleCornerIndex), + obstacle.points.at(obstacleCornerIndex + 1), minDistance}); + } + if (obstacleCornerCount >= 3) { + SgmtConstraint( + fromIndex, toIndex, + PointLineConstraint{bumperCorner, + obstacle.points.at(bumperCornerCount - 1), + obstacle.points.at(0), minDistance}); + } + } else { + SgmtConstraint(fromIndex, toIndex, + PointPointConstraint{bumperCorner, obstacle.points.at(0), + minDistance}); + } } } } @@ -201,16 +192,22 @@ const std::vector& SwervePathBuilder::GetControlIntervalCounts() const { return controlIntervalCounts; } -Solution SwervePathBuilder::CalculateInitialGuess() const { - return GenerateLinearInitialGuess(initialGuessPoints, controlIntervalCounts); +SwerveSolution SwervePathBuilder::CalculateInitialGuess() const { + return GenerateLinearInitialGuess(initialGuessPoints, + controlIntervalCounts); +} + +void SwervePathBuilder::AddIntermediateCallback( + const std::function callback) { + path.callbacks.push_back(callback); } 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) { - path.waypoints.emplace_back(SwerveWaypoint{}); + 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(); initialGuessPoints.emplace_back(std::vector{{0.0, 0.0, {0.0}}}); if (i != 0) { controlIntervalCounts.push_back(40); @@ -219,68 +216,4 @@ void SwervePathBuilder::NewWpts(size_t finalIndex) { } } -std::vector SwervePathBuilder::GetConstraintsForObstacle( - const Bumpers& bumpers, const Obstacle& obstacle) { - auto distConst = IntervalSet1d::GreaterThan(bumpers.safetyDistance + - obstacle.safetyDistance); - - size_t bumperCornerCount = bumpers.points.size(); - size_t obstacleCornerCount = obstacle.points.size(); - if (bumperCornerCount == 1 && obstacleCornerCount == 1) { - // if the bumpers and obstacle are only one point - return {PointPointConstraint{bumpers.points.at(0), obstacle.points.at(0), - distConst}}; - } - - std::vector constraints; - - // robot bumper edge to obstacle point constraints - for (auto& obstaclePoint : obstacle.points) { - // First apply constraint for all but last edge - for (size_t bumperCornerIndex = 0; - bumperCornerIndex < bumperCornerCount - 1; bumperCornerIndex++) { - constraints.emplace_back(LinePointConstraint{ - bumpers.points.at(bumperCornerIndex), - bumpers.points.at(bumperCornerIndex + 1), obstaclePoint, distConst}); - } - // apply to last edge: the edge connecting the last point to the first - // must have at least three points to need this - if (bumperCornerCount >= 3) { - constraints.emplace_back( - LinePointConstraint{bumpers.points.at(bumperCornerCount - 1), - bumpers.points.at(0), obstaclePoint, distConst}); - } - } - - // obstacle edge to bumper corner constraints - for (auto& bumperCorner : bumpers.points) { - if (obstacleCornerCount > 1) { - for (size_t obstacleCornerIndex = 0; - obstacleCornerIndex < obstacleCornerCount - 1; - obstacleCornerIndex++) { - constraints.emplace_back(PointLineConstraint{ - bumperCorner, obstacle.points.at(obstacleCornerIndex), - obstacle.points.at(obstacleCornerIndex + 1), distConst}); - } - if (obstacleCornerCount >= 3) { - constraints.emplace_back(PointLineConstraint{ - bumperCorner, obstacle.points.at(bumperCornerCount - 1), - obstacle.points.at(0), distConst}); - } - } else { - constraints.emplace_back( - PointPointConstraint{bumperCorner, obstacle.points.at(0), distConst}); - } - } - return constraints; -} - -void SwervePathBuilder::AddIntermediateCallback( - const std::function callback) { - path.callbacks.push_back(callback); -} - -void SwervePathBuilder::CancelAll() { - trajopt::GetCancellationFlag() = 1; -} } // namespace trajopt diff --git a/src/trajoptlibrust.cpp b/src/trajoptlibrust.cpp index 12b75de7..03da9ef0 100644 --- a/src/trajoptlibrust.cpp +++ b/src/trajoptlibrust.cpp @@ -9,7 +9,12 @@ #include #include -#include "trajopt/OptimalTrajectoryGenerator.hpp" +#include "trajopt/SwerveTrajectoryGenerator.hpp" +#include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp" +#include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp" +#include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp" +#include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp" +#include "trajopt/constraint/PointAtConstraint.hpp" #include "trajopt/drivetrain/SwerveDrivetrain.hpp" #include "trajopt/geometry/Translation2.hpp" #include "trajopt/trajectory/HolonomicTrajectory.hpp" @@ -90,153 +95,110 @@ 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.WptConstraint(index, trajopt::LinearVelocityDirectionConstraint{angle}); } void SwervePathBuilderImpl::wpt_linear_velocity_max_magnitude( - size_t idx, double magnitude) { - path.WptVelocityMagnitude(idx, magnitude); + size_t index, double magnitude) { + path.WptConstraint(index, + trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); } -void SwervePathBuilderImpl::wpt_linear_velocity_polar(size_t idx, - double magnitude, - double angle) { - path.WptVelocityPolar(idx, 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.WptConstraint( + index, trajopt::AngularVelocityEqualityConstraint{angular_velocity}); } void SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude( - size_t idx, double angular_velocity) { - path.WptAngularVelocityMaxMagnitude(idx, angular_velocity); -} - -void SwervePathBuilderImpl::wpt_x(size_t idx, double x) { - path.WptConstraint(idx, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); -} - -void SwervePathBuilderImpl::wpt_y(size_t idx, double y) { - path.WptConstraint(idx, - 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}); + size_t index, double angular_velocity) { + path.WptConstraint( + index, trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity}); } -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.SgmtConstraint(from_index, to_index, + trajopt::LinearVelocityDirectionConstraint{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.SgmtConstraint(from_index, to_index, + trajopt::LinearVelocityMaxMagnitudeConstraint{magnitude}); } -void SwervePathBuilderImpl::sgmt_linear_velocity_polar(size_t from_idx, - size_t to_idx, - double magnitude, - double angle) { - path.SgmtConstraint( - from_idx, to_idx, - 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); -} - -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); -} - -void SwervePathBuilderImpl::sgmt_x(size_t from_idx, size_t to_idx, double x) { path.SgmtConstraint( - from_idx, to_idx, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = x, .yBound = trajopt::IntervalSet1d::R1()}}); + from_index, to_index, + trajopt::AngularVelocityEqualityConstraint{angular_velocity}); } -void SwervePathBuilderImpl::sgmt_y(size_t from_idx, size_t to_idx, double y) { +void SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude( + size_t from_index, size_t to_index, double angular_velocity) { path.SgmtConstraint( - from_idx, to_idx, - trajopt::TranslationConstraint{trajopt::RectangularSet2d{ - .xBound = trajopt::IntervalSet1d::R1(), .yBound = y}}); -} - -void SwervePathBuilderImpl::sgmt_heading(size_t from_idx, size_t to_idx, - double heading) { - path.SgmtConstraint(from_idx, to_idx, trajopt::HeadingConstraint{heading}); + from_index, to_index, + trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity}); } -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 +210,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( @@ -305,9 +267,8 @@ void SwervePathBuilderImpl::add_progress_callback( HolonomicTrajectory SwervePathBuilderImpl::generate(bool diagnostics, int64_t handle) const { - if (auto sol = trajopt::OptimalTrajectoryGenerator::Generate( - path, diagnostics, handle); - sol.has_value()) { + trajopt::SwerveTrajectoryGenerator generator{path, handle}; + if (auto sol = generator.Generate(diagnostics); sol.has_value()) { return _convert_holonomic_trajectory( trajopt::HolonomicTrajectory{sol.value()}); } else { diff --git a/src/trajoptlibrust.hpp b/src/trajoptlibrust.hpp index 73005112..05637546 100644 --- a/src/trajoptlibrust.hpp +++ b/src/trajoptlibrust.hpp @@ -17,59 +17,54 @@ struct SwerveDrivetrain; class SwervePathBuilderImpl { public: + SwervePathBuilderImpl() = default; + void set_drivetrain(const SwerveDrivetrain& drivetrain); 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_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, 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, - 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_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 // throwing exception, once cxx supports it HolonomicTrajectory generate(bool diagnostics = false, int64_t handle = 0) const; + void add_progress_callback( rust::Fn callback); - void cancel_all(); - SwervePathBuilderImpl() = default; + void cancel_all(); private: trajopt::SwervePathBuilder path; diff --git a/src/optimization/Cancellation.cpp b/src/util/Cancellation.cpp similarity index 75% rename from src/optimization/Cancellation.cpp rename to src/util/Cancellation.cpp index 7a9b08b7..fe37f174 100644 --- a/src/optimization/Cancellation.cpp +++ b/src/util/Cancellation.cpp @@ -1,8 +1,6 @@ // Copyright (c) TrajoptLib contributors -#include "optimization/Cancellation.hpp" - -#include +#include "trajopt/util/Cancellation.hpp" namespace trajopt { diff --git a/test/src/ObstacleTest.cpp b/test/src/ObstacleTest.cpp index 599c4099..24e8e18d 100644 --- a/test/src/ObstacleTest.cpp +++ b/test/src/ObstacleTest.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { @@ -34,5 +34,6 @@ TEST_CASE("Obstacle - Linear initial guess", "[Obstacle]") { 0, 1, trajopt::Obstacle{.safetyDistance = 1.0, .points = {{1.0, 1.0}}}); path.ControlIntervalCounts({10}); - CHECK_NOTHROW(trajopt::OptimalTrajectoryGenerator::Generate(path)); + trajopt::SwerveTrajectoryGenerator generator{path}; + CHECK_NOTHROW(generator.Generate()); } diff --git a/test/src/SwerveTrajoptUtilTest.cpp b/test/src/SwerveTrajoptUtilTest.cpp deleted file mode 100644 index 924b56e0..00000000 --- a/test/src/SwerveTrajoptUtilTest.cpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include - -#include -#include -#include -#include - -#include "optimization/SwerveTrajoptUtil.hpp" - -TEST_CASE("SwerveTrajoptUtil - SolveNetForce()", "[SwerveTrajoptUtil]") { - std::vector Fx{4.0, 1.0, 3.0, 5.0}; - std::vector Fy{0.0, 2.0, -5.0, 2.0}; - - auto [Fx_net, Fy_net] = trajopt::SolveNetForce(Fx, Fy); - - CHECK(Fx_net.Value() == Catch::Approx(13.0).margin(1e-3)); - CHECK(Fy_net.Value() == Catch::Approx(-1.0).margin(1e-3)); -} - -TEST_CASE("SwerveTrajoptUtil - SolveNetTorque()", "[SwerveTrajoptUtil]") { - constexpr double theta = 1.0; - - std::vector Fx{-3.0, -5.0, -4.0, 2.0}; - std::vector Fy{4.0, -5.0, -2.0, -4.0}; - std::vector swerveModules = { - {trajopt::Translation2d{1.0, 1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{1.0, -1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{-1.0, 1.0}, 0.0, 0.0, 0.0}, - {trajopt::Translation2d{-1.0, -1.0}, 0.0, 0.0, 0.0}, - }; - - sleipnir::Variable tau_net = trajopt::SolveNetTorque( - {std::cos(theta), std::sin(theta)}, Fx, Fy, swerveModules); - - CHECK(tau_net.Value() == Catch::Approx(0.6553658).margin(1e-3)); -} diff --git a/test/src/TrajoptUtilTest.cpp b/test/src/TrajoptUtilTest.cpp deleted file mode 100644 index a6cab965..00000000 --- a/test/src/TrajoptUtilTest.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) TrajoptLib contributors - -#include - -#include -#include - -#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); - CHECK(result0 == 0); - CHECK(result1 == 2); - CHECK(result2 == 5); - CHECK(result3 == 6); -} - -TEST_CASE("TrajoptUtil - Linspace()", "[TrajoptUtil]") { - auto result = trajopt::Linspace(0.0, 2.0, 2); - std::vector correct{1.0, 2.0}; - CHECK(result == correct); -} - -TEST_CASE("TrajoptUtil - Linear initial guess", "[TrajoptUtil]") { - std::vector> initialGuessPoints{ - {{1, 0, 0}}, {{2, 0, 0}, {3, 0, 0}}, {{6, 0, 0}}}; - std::vector controlIntervalCounts{2, 3}; - std::vector expectedX{1, 2, 3, 4, 5, 6}; - auto result = trajopt::GenerateLinearInitialGuess(initialGuessPoints, - controlIntervalCounts); - CHECK(expectedX == result.x); -} diff --git a/test/src/geometry/Translation2dTest.cpp b/test/src/geometry/Translation2dTest.cpp index 57cd3480..d46804fe 100644 --- a/test/src/geometry/Translation2dTest.cpp +++ b/test/src/geometry/Translation2dTest.cpp @@ -45,6 +45,22 @@ TEST_CASE("Translation2d - UnaryMinus", "[Translation2d]") { CHECK(inverted.Y() == -7.0); } +TEST_CASE("Translation2d - Multiplication", "[Translation2d]") { + const trajopt::Translation2d original{3.0, 5.0}; + const auto mult = original * 3; + + CHECK(mult.X() == 9.0); + CHECK(mult.Y() == 15.0); +} + +TEST_CASE("Translation2d - Division", "[Translation2d]") { + const trajopt::Translation2d original{3.0, 5.0}; + const auto div = original / 2; + + CHECK(div.X() == 1.5); + CHECK(div.Y() == 2.5); +} + TEST_CASE("Translation2d - RotateBy", "[Translation2d]") { const trajopt::Translation2d another{3.0, 0.0}; const auto rotated =