From 10fa954201c15ddb187273a51dd3299d2764b934 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 24 Jun 2024 16:08:52 -0700 Subject: [PATCH] Add acceleration to constrainable variables --- .../constraint/AngularVelocityEqualityConstraint.hpp | 7 ++++++- .../AngularVelocityMaxMagnitudeConstraint.hpp | 7 ++++++- include/trajopt/constraint/Constraint.hpp | 10 ++++++++++ include/trajopt/constraint/LinePointConstraint.hpp | 7 ++++++- .../constraint/LinearVelocityDirectionConstraint.hpp | 7 ++++++- .../LinearVelocityMaxMagnitudeConstraint.hpp | 7 ++++++- include/trajopt/constraint/PointAtConstraint.hpp | 7 ++++++- include/trajopt/constraint/PointLineConstraint.hpp | 7 ++++++- include/trajopt/constraint/PointPointConstraint.hpp | 7 ++++++- include/trajopt/constraint/PoseEqualityConstraint.hpp | 7 ++++++- .../constraint/TranslationEqualityConstraint.hpp | 7 ++++++- src/SwerveTrajectoryGenerator.cpp | 10 ++++++++-- 12 files changed, 78 insertions(+), 12 deletions(-) diff --git a/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp b/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp index 3c9fb21a..c23c3742 100644 --- a/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp +++ b/include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp @@ -2,6 +2,7 @@ #pragma once +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -30,11 +31,15 @@ class TRAJOPT_DLLEXPORT AngularVelocityEqualityConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, [[maybe_unused]] const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - const sleipnir::Variable& angularVelocity) { + const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { problem.SubjectTo(angularVelocity == m_angularVelocity); } diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp index 41a3b869..6b76d927 100644 --- a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp +++ b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp @@ -4,6 +4,7 @@ #include +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -35,11 +36,15 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, [[maybe_unused]] const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - const sleipnir::Variable& angularVelocity) { + const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { if (m_maxMagnitude == 0.0) { problem.SubjectTo(angularVelocity == 0.0); } else { diff --git a/include/trajopt/constraint/Constraint.hpp b/include/trajopt/constraint/Constraint.hpp index eeb07fdd..5180efef 100644 --- a/include/trajopt/constraint/Constraint.hpp +++ b/include/trajopt/constraint/Constraint.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp" @@ -28,8 +29,17 @@ namespace trajopt { template concept ConstraintType = requires(T t) { { + // Parameters: + // + // sleipnir::OptimizationProblem& problem + // const Pose2v& pose + // const Translation2v& linearVelocity + // const sleipnir::Variable& angularVelocity + // const Translation2v& linearAcceleration + // const sleipnir::Variable& angularAcceleration t.Apply(std::declval(), std::declval(), std::declval(), + std::declval(), std::declval(), std::declval()) }; // NOLINT(readability/braces) }; diff --git a/include/trajopt/constraint/LinePointConstraint.hpp b/include/trajopt/constraint/LinePointConstraint.hpp index 744d89e3..ded526c6 100644 --- a/include/trajopt/constraint/LinePointConstraint.hpp +++ b/include/trajopt/constraint/LinePointConstraint.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include "trajopt/constraint/detail/LinePointDistance.hpp" @@ -48,10 +49,14 @@ class TRAJOPT_DLLEXPORT LinePointConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { auto lineStart = pose.Translation() + m_robotLineStart.RotateBy(pose.Rotation()); auto lineEnd = diff --git a/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp b/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp index 9188c933..9a206a58 100644 --- a/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp +++ b/include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp @@ -2,6 +2,7 @@ #pragma once +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -29,11 +30,15 @@ class TRAJOPT_DLLEXPORT LinearVelocityDirectionConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply([[maybe_unused]] sleipnir::OptimizationProblem& problem, [[maybe_unused]] const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { // and must be parallel // // (v ā‹… u)/ā€–vā€– = 1 diff --git a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp index 8ce14bc5..9909d662 100644 --- a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp +++ b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp @@ -4,6 +4,7 @@ #include +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -35,11 +36,15 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, [[maybe_unused]] const Pose2v& pose, const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { if (m_maxMagnitude == 0.0) { problem.SubjectTo(linearVelocity.X() == 0.0); problem.SubjectTo(linearVelocity.Y() == 0.0); diff --git a/include/trajopt/constraint/PointAtConstraint.hpp b/include/trajopt/constraint/PointAtConstraint.hpp index c2b2801a..cab80770 100644 --- a/include/trajopt/constraint/PointAtConstraint.hpp +++ b/include/trajopt/constraint/PointAtConstraint.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -40,10 +41,14 @@ class TRAJOPT_DLLEXPORT PointAtConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { // dx,dy = desired heading // ux,uy = unit vector of desired heading // hx,hy = heading diff --git a/include/trajopt/constraint/PointLineConstraint.hpp b/include/trajopt/constraint/PointLineConstraint.hpp index 4cd5c7d3..13f362b9 100644 --- a/include/trajopt/constraint/PointLineConstraint.hpp +++ b/include/trajopt/constraint/PointLineConstraint.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include "trajopt/constraint/detail/LinePointDistance.hpp" @@ -48,10 +49,14 @@ class TRAJOPT_DLLEXPORT PointLineConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { auto point = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); auto dist = detail::LinePointDistance(m_fieldLineStart, m_fieldLineEnd, point); diff --git a/include/trajopt/constraint/PointPointConstraint.hpp b/include/trajopt/constraint/PointPointConstraint.hpp index 81506356..dbfecb03 100644 --- a/include/trajopt/constraint/PointPointConstraint.hpp +++ b/include/trajopt/constraint/PointPointConstraint.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -44,10 +45,14 @@ class TRAJOPT_DLLEXPORT PointPointConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { auto bumperCorner = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation()); auto dx = m_fieldPoint.X() - bumperCorner.X(); diff --git a/include/trajopt/constraint/PoseEqualityConstraint.hpp b/include/trajopt/constraint/PoseEqualityConstraint.hpp index d1670a71..583fb0a2 100644 --- a/include/trajopt/constraint/PoseEqualityConstraint.hpp +++ b/include/trajopt/constraint/PoseEqualityConstraint.hpp @@ -2,6 +2,7 @@ #pragma once +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -32,10 +33,14 @@ class TRAJOPT_DLLEXPORT PoseEqualityConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { problem.SubjectTo(pose == m_pose); } diff --git a/include/trajopt/constraint/TranslationEqualityConstraint.hpp b/include/trajopt/constraint/TranslationEqualityConstraint.hpp index 567872b7..281a0ed4 100644 --- a/include/trajopt/constraint/TranslationEqualityConstraint.hpp +++ b/include/trajopt/constraint/TranslationEqualityConstraint.hpp @@ -2,6 +2,7 @@ #pragma once +#include #include #include "trajopt/geometry/Pose2.hpp" @@ -30,10 +31,14 @@ class TRAJOPT_DLLEXPORT TranslationEqualityConstraint { * @param pose The robot's pose. * @param linearVelocity The robot's linear velocity. * @param angularVelocity The robot's angular velocity. + * @param linearAcceleration The robot's linear acceleration. + * @param angularAcceleration The robot's angular acceleration. */ void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose, [[maybe_unused]] const Translation2v& linearVelocity, - [[maybe_unused]] const sleipnir::Variable& angularVelocity) { + [[maybe_unused]] const sleipnir::Variable& angularVelocity, + [[maybe_unused]] const Translation2v& linearAcceleration, + [[maybe_unused]] const sleipnir::Variable& angularAcceleration) { problem.SubjectTo(pose.Translation() == m_translation); } diff --git a/src/SwerveTrajectoryGenerator.cpp b/src/SwerveTrajectoryGenerator.cpp index 236848f7..173e7529 100644 --- a/src/SwerveTrajectoryGenerator.cpp +++ b/src/SwerveTrajectoryGenerator.cpp @@ -232,10 +232,13 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( x.at(index), y.at(index), {thetacos.at(index), thetasin.at(index)}}; Translation2v linearVelocity{vx.at(index), vy.at(index)}; auto angularVelocity = omega.at(index); + Translation2v linearAcceleration{ax.at(index), ay.at(index)}; + auto angularAcceleration = alpha.at(index); std::visit( [&](auto&& arg) { - arg.Apply(problem, pose, linearVelocity, angularVelocity); + arg.Apply(problem, pose, linearVelocity, angularVelocity, + linearAcceleration, angularAcceleration); }, constraint); } @@ -252,10 +255,13 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator( x.at(index), y.at(index), {thetacos.at(index), thetasin.at(index)}}; Translation2v linearVelocity{vx.at(index), vy.at(index)}; auto angularVelocity = omega.at(index); + Translation2v linearAcceleration{ax.at(index), ay.at(index)}; + auto angularAcceleration = alpha.at(index); std::visit( [&](auto&& arg) { - arg.Apply(problem, pose, linearVelocity, angularVelocity); + arg.Apply(problem, pose, linearVelocity, angularVelocity, + linearAcceleration, angularAcceleration); }, constraint); }