Skip to content

Commit

Permalink
[choreolib] Make trajectory sample classes constexpr (#932)
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul authored Nov 18, 2024
1 parent 6217721 commit 87c00c2
Show file tree
Hide file tree
Showing 7 changed files with 111 additions and 144 deletions.
48 changes: 5 additions & 43 deletions choreolib/src/main/native/cpp/choreo/trajectory/DifferentialSample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,11 @@

#include "choreo/trajectory/DifferentialSample.h"

#include <wpi/MathExtras.h>
#include <wpi/json.h>

#include "choreo/Choreo.h"
#include "choreo/trajectory/TrajectorySample.h"

namespace choreo {

units::second_t DifferentialSample::GetTimestamp() const {
return timestamp;
}

frc::Pose2d DifferentialSample::GetPose() const {
return frc::Pose2d{x, y, frc::Rotation2d{heading}};
}

frc::ChassisSpeeds DifferentialSample::GetChassisSpeeds() const {
frc::ChassisSpeeds choreo::DifferentialSample::GetChassisSpeeds() const {
return frc::ChassisSpeeds{
(vl + vr) / 2.0, 0_mps,
(vr - vl) /
Expand All @@ -27,33 +15,8 @@ frc::ChassisSpeeds DifferentialSample::GetChassisSpeeds() const {
1_rad};
}

DifferentialSample DifferentialSample::OffsetBy(
units::second_t timeStampOffset) const {
return DifferentialSample{
timestamp + timeStampOffset, x, y, heading, vl, vr, al, ar, fl, fr};
}

DifferentialSample DifferentialSample::Interpolate(
const DifferentialSample& endValue, units::second_t t) const {
units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp);
frc::Pose2d interpolatedPose =
frc::Interpolate(GetPose(), endValue.GetPose(), scale.value());

return DifferentialSample{
wpi::Lerp(timestamp, endValue.timestamp, scale),
interpolatedPose.X(),
interpolatedPose.Y(),
interpolatedPose.Rotation().Radians(),
wpi::Lerp(vl, endValue.vl, scale),
wpi::Lerp(vr, endValue.vr, scale),
wpi::Lerp(al, endValue.al, scale),
wpi::Lerp(ar, endValue.ar, scale),
wpi::Lerp(fl, endValue.fl, scale),
wpi::Lerp(fr, endValue.fr, scale),
};
}

void to_json(wpi::json& json, const DifferentialSample& trajectorySample) {
void choreo::to_json(wpi::json& json,
const DifferentialSample& trajectorySample) {
json = wpi::json{{"t", trajectorySample.timestamp.value()},
{"x", trajectorySample.x.value()},
{"y", trajectorySample.y.value()},
Expand All @@ -66,7 +29,8 @@ void to_json(wpi::json& json, const DifferentialSample& trajectorySample) {
{"fr", trajectorySample.fr.value()}};
}

void from_json(const wpi::json& json, DifferentialSample& trajectorySample) {
void choreo::from_json(const wpi::json& json,
DifferentialSample& trajectorySample) {
trajectorySample.timestamp = units::second_t{json.at("t").get<double>()};
trajectorySample.x = units::meter_t{json.at("x").get<double>()};
trajectorySample.y = units::meter_t{json.at("y").get<double>()};
Expand All @@ -80,5 +44,3 @@ void from_json(const wpi::json& json, DifferentialSample& trajectorySample) {
trajectorySample.fl = units::newton_t{json.at("fl").get<double>()};
trajectorySample.fr = units::newton_t{json.at("fr").get<double>()};
}

} // namespace choreo
2 changes: 0 additions & 2 deletions choreolib/src/main/native/cpp/choreo/trajectory/EventMarker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@

#include <wpi/json.h>

using namespace choreo;

void choreo::to_json(wpi::json& json, const EventMarker& event) {
json = wpi::json{{"data", wpi::json{{"t", event.timestamp.value()}}},
{"event", wpi::json{{"name", event.event}}}};
Expand Down
2 changes: 0 additions & 2 deletions choreolib/src/main/native/cpp/choreo/trajectory/ProjectFile.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@

#include <wpi/json.h>

using namespace choreo;

void choreo::to_json(wpi::json& json, const Expression& exp) {
json = wpi::json{{"exp", exp.expression}, {"val", exp.val}};
}
Expand Down
61 changes: 0 additions & 61 deletions choreolib/src/main/native/cpp/choreo/trajectory/SwerveSample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,69 +4,8 @@

#include <algorithm>

#include <wpi/MathExtras.h>
#include <wpi/json.h>

#include "choreo/trajectory/TrajectorySample.h"

using namespace choreo;

units::second_t SwerveSample::GetTimestamp() const {
return timestamp;
}

frc::Pose2d SwerveSample::GetPose() const {
return frc::Pose2d{x, y, frc::Rotation2d{heading}};
}

frc::ChassisSpeeds SwerveSample::GetChassisSpeeds() const {
return frc::ChassisSpeeds{vx, vy, omega};
}

SwerveSample SwerveSample::OffsetBy(units::second_t timeStampOffset) const {
return SwerveSample{timestamp + timeStampOffset,
x,
y,
heading,
vx,
vy,
omega,
ax,
ay,
alpha,
moduleForcesX,
moduleForcesY};
}

SwerveSample SwerveSample::Interpolate(const SwerveSample& endValue,
units::second_t t) const {
units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp);
frc::Pose2d interpolatedPose =
frc::Interpolate(GetPose(), endValue.GetPose(), scale.value());

std::array<units::newton_t, 4> interpolatedForcesX;
std::array<units::newton_t, 4> interpolatedForcesY;
for (int i = 0; i < 4; i++) {
interpolatedForcesX[i] =
wpi::Lerp(moduleForcesX[i], endValue.moduleForcesX[i], scale.value());
interpolatedForcesY[i] =
wpi::Lerp(moduleForcesY[i], endValue.moduleForcesY[i], scale.value());
}

return SwerveSample{wpi::Lerp(timestamp, endValue.timestamp, scale),
interpolatedPose.X(),
interpolatedPose.Y(),
interpolatedPose.Rotation().Radians(),
wpi::Lerp(vx, endValue.vx, scale),
wpi::Lerp(vy, endValue.vy, scale),
wpi::Lerp(omega, endValue.omega, scale),
wpi::Lerp(ax, endValue.ax, scale),
wpi::Lerp(ay, endValue.ay, scale),
wpi::Lerp(alpha, endValue.alpha, scale),
interpolatedForcesX,
interpolatedForcesY};
}

void choreo::to_json(wpi::json& json, const SwerveSample& trajectorySample) {
std::array<double, 4> fx;
std::transform(trajectorySample.moduleForcesX.begin(),
Expand Down
2 changes: 0 additions & 2 deletions choreolib/src/main/native/cpp/choreo/trajectory/Trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@

#include <wpi/json.h>

using namespace choreo;

void choreo::to_json(wpi::json& json,
const Trajectory<SwerveSample>& trajectory) {
json = wpi::json{{"name", trajectory.name},
Expand Down
57 changes: 41 additions & 16 deletions choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#pragma once

#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <units/acceleration.h>
#include <units/angle.h>
Expand All @@ -10,8 +11,10 @@
#include <units/force.h>
#include <units/length.h>
#include <units/velocity.h>
#include <wpi/MathExtras.h>
#include <wpi/json_fwd.h>

#include "choreo/trajectory/TrajectorySample.h"
#include "choreo/util/AllianceFlipperUtil.h"

namespace choreo {
Expand All @@ -24,7 +27,7 @@ class DifferentialSample {
/**
* Constructs a DifferentialSample that is defaulted.
*/
DifferentialSample() = default;
constexpr DifferentialSample() = default;

/**
* Constructs a DifferentialSample with the specified parameters.
Expand All @@ -41,13 +44,13 @@ class DifferentialSample {
* @param fl The force of the left wheels
* @param fr The force of the right wheels
*/
DifferentialSample(units::second_t timestamp, units::meter_t x,
units::meter_t y, units::radian_t heading,
units::meters_per_second_t vl,
units::meters_per_second_t vr,
units::meters_per_second_squared_t al,
units::meters_per_second_squared_t ar, units::newton_t fl,
units::newton_t fr)
constexpr DifferentialSample(units::second_t timestamp, units::meter_t x,
units::meter_t y, units::radian_t heading,
units::meters_per_second_t vl,
units::meters_per_second_t vr,
units::meters_per_second_squared_t al,
units::meters_per_second_squared_t ar,
units::newton_t fl, units::newton_t fr)
: timestamp{timestamp},
x{x},
y{y},
Expand All @@ -64,14 +67,16 @@ class DifferentialSample {
*
* @return The timestamp.
*/
units::second_t GetTimestamp() const;
units::second_t GetTimestamp() const { return timestamp; }

/**
* Gets the Pose2d of the DifferentialSample.
*
* @return The pose.
*/
frc::Pose2d GetPose() const;
constexpr frc::Pose2d GetPose() const {
return frc::Pose2d{x, y, frc::Rotation2d{heading}};
}

/**
* Gets the field-relative chassis speeds of the DifferentialSample.
Expand All @@ -86,7 +91,10 @@ class DifferentialSample {
* @param timeStampOffset time to move sample by
* @return DifferentialSample that is moved forward by the offset
*/
DifferentialSample OffsetBy(units::second_t timeStampOffset) const;
constexpr DifferentialSample OffsetBy(units::second_t timeStampOffset) const {
return DifferentialSample{
timestamp + timeStampOffset, x, y, heading, vl, vr, al, ar, fl, fr};
}

/**
* Interpolates between endValue and this by t
Expand All @@ -95,8 +103,25 @@ class DifferentialSample {
* @param t time to move sample by
* @return the interpolated sample
*/
DifferentialSample Interpolate(const DifferentialSample& endValue,
units::second_t t) const;
constexpr DifferentialSample Interpolate(const DifferentialSample& endValue,
units::second_t t) const {
units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp);
frc::Pose2d interpolatedPose =
frc::Interpolate(GetPose(), endValue.GetPose(), scale.value());

return DifferentialSample{
wpi::Lerp(timestamp, endValue.timestamp, scale),
interpolatedPose.X(),
interpolatedPose.Y(),
interpolatedPose.Rotation().Radians(),
wpi::Lerp(vl, endValue.vl, scale),
wpi::Lerp(vr, endValue.vr, scale),
wpi::Lerp(al, endValue.al, scale),
wpi::Lerp(ar, endValue.ar, scale),
wpi::Lerp(fl, endValue.fl, scale),
wpi::Lerp(fr, endValue.fr, scale),
};
}

/**
* Returns the current sample flipped based on the field year.
Expand All @@ -105,8 +130,8 @@ class DifferentialSample {
* @return DifferentialSample that is flipped based on the field layout.
*/
template <int Year>
DifferentialSample Flipped() const {
static constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
constexpr DifferentialSample Flipped() const {
constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
if constexpr (flipper.isMirrored) {
return DifferentialSample(timestamp, flipper.FlipX(x), y,
flipper.FlipHeading(heading), vl, vr, al, ar,
Expand All @@ -124,7 +149,7 @@ class DifferentialSample {
* @param other The other DifferentialSample.
* @return True for equality.
*/
bool operator==(const DifferentialSample& other) const {
constexpr bool operator==(const DifferentialSample& other) const {
constexpr double epsilon = 1e-6;

auto compare_units = [epsilon](const auto& a, const auto& b) {
Expand Down
Loading

0 comments on commit 87c00c2

Please sign in to comment.