Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Prevent NaN trajectory time calculations #994

Merged
merged 4 commits into from
Jan 10, 2025
Merged

Prevent NaN trajectory time calculations #994

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
515ad6b
[gui] prevent NaN trajectory time calculations
mjansen4857 Jan 10, 2025
947405c
[java] prevent NaN trajectory time calculations
mjansen4857 Jan 10, 2025
85c0ac1
[cpp] prevent NaN trajectory time calculations
mjansen4857 Jan 10, 2025
f3a9f04
[python] prevent NaN trajectory time calculations
mjansen4857 Jan 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions lib/trajectory/trajectory.dart
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -450,8 +450,13 @@
pow(states[i - 1].fieldSpeeds.vy, 2));
num v = sqrt(
pow(states[i].fieldSpeeds.vx, 2) + pow(states[i].fieldSpeeds.vy, 2));
num dt = (2 * states[i].deltaPos) / (v + v0);
states[i].timeSeconds = states[i - 1].timeSeconds + dt;
num sumV = v + v0;
if (sumV.abs() < 1e-6) {
states[i].timeSeconds = states[i - 1].timeSeconds;

Check warning on line 455 in lib/trajectory/trajectory.dart

View check run for this annotation

Codecov / codecov/patch

lib/trajectory/trajectory.dart#L455 

Added line #L455 was not covered by tests
} else {
num dt = (2 * states[i].deltaPos) / sumV;
states[i].timeSeconds = states[i - 1].timeSeconds + dt;
}
}

DateTime now = DateTime.now();
Expand Down
2 changes: 1 addition & 1 deletion pathplannerlib-python/pathplannerlib/path.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

targetIncrement: Final[float] = 0.05
targetSpacing: Final[float] = 0.2
autoControlDistanceFactor: Final[float] = 1.0 / 3.0
autoControlDistanceFactor: Final[float] = 0.4


@dataclass(frozen=True)
Expand Down
78 changes: 43 additions & 35 deletions pathplannerlib-python/pathplannerlib/trajectory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,41 +239,49 @@ def __init__(self, path: Union[PathPlannerPath, None], starting_speeds: Union[Ch

v0 = prevState.linearVelocity
v = state.linearVelocity
dt = (2 * state.deltaPos) / (v + v0)
state.timeSeconds = prevState.timeSeconds + dt

prevRobotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(prevState.fieldSpeeds,
prevState.pose.rotation())
robotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.rotation())
chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt
chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt
chassisForceX = chassisAccelX * config.massKG
chassisForceY = chassisAccelY * config.massKG

angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega) / dt
angTorque = angularAccel * config.MOI
chassisForces = ChassisSpeeds(chassisForceX, chassisForceY, angTorque)

wheelForces = config.chassisForcesToWheelForceVectors(chassisForces)
accelFF = []
linearForceFF = []
torqueCurrentFF = []
forceXFF = []
forceYFF = []
for m in range(config.numModules):
wheelForceDist = wheelForces[m].norm()
appliedForce = 0.0 if wheelForceDist <= 1e-6 else wheelForceDist * (
wheelForces[m].angle() - state.moduleStates[m].angle).cos()
wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters
torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt

accelFF.append((state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt)
linearForceFF.append(appliedForce)
torqueCurrentFF.append(torqueCurrent)
forceXFF.append(wheelForces[m].x)
forceYFF.append(wheelForces[m].y)
prevState.feedforwards = DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF,
forceYFF)
sumV = v + v0
if abs(sumV) < 1e-6:
state.timeSeconds = prevState.timeSeconds
if i != 1:
prevState.feedforwards = self._states[i - 2].feedforwards
else:
prevState.feedforwards = DriveFeedforwards.zeros(config.numModules)
else:
dt = (2 * state.deltaPos) / sumV
state.timeSeconds = prevState.timeSeconds + dt

prevRobotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(prevState.fieldSpeeds,
prevState.pose.rotation())
robotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.rotation())
chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt
chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt
chassisForceX = chassisAccelX * config.massKG
chassisForceY = chassisAccelY * config.massKG

angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega) / dt
angTorque = angularAccel * config.MOI
chassisForces = ChassisSpeeds(chassisForceX, chassisForceY, angTorque)

wheelForces = config.chassisForcesToWheelForceVectors(chassisForces)
accelFF = []
linearForceFF = []
torqueCurrentFF = []
forceXFF = []
forceYFF = []
for m in range(config.numModules):
wheelForceDist = wheelForces[m].norm()
appliedForce = 0.0 if wheelForceDist <= 1e-6 else wheelForceDist * (
wheelForces[m].angle() - state.moduleStates[m].angle).cos()
wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters
torqueCurrent = wheelTorque / config.moduleConfig.driveMotor.Kt

accelFF.append((state.moduleStates[m].speed - prevState.moduleStates[m].speed) / dt)
linearForceFF.append(appliedForce)
torqueCurrentFF.append(torqueCurrent)
forceXFF.append(wheelForces[m].x)
forceYFF.append(wheelForces[m].y)
prevState.feedforwards = DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF,
forceYFF)

# Un-added events have their timestamp set to a waypoint relative position
# When adding the event to this trajectory, set its timestamp properly
Expand Down
2 changes: 1 addition & 1 deletion pathplannerlib/src/main/java/com/pathplanner/lib/path/Waypoint.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
* @param nextControl Next control point
*/
public record Waypoint(Translation2d prevControl, Translation2d anchor, Translation2d nextControl) {
private static final double AUTO_CONTROL_DISTANCE_FACTOR = 1.0 / 3.0;
private static final double AUTO_CONTROL_DISTANCE_FACTOR = 0.4;

/**
* Flip this waypoint to the other side of the field, maintaining a blue alliance origin
Expand Down
102 changes: 56 additions & 46 deletions pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,53 +132,63 @@ public PathPlannerTrajectory(

double v0 = prevState.linearVelocity;
double v = state.linearVelocity;
double dt = (2 * state.deltaPos) / (v + v0);
state.timeSeconds = prevState.timeSeconds + dt;

ChassisSpeeds prevRobotSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
prevState.fieldSpeeds, prevState.pose.getRotation());
ChassisSpeeds robotSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation());
double chassisAccelX =
(robotSpeeds.vxMetersPerSecond - prevRobotSpeeds.vxMetersPerSecond) / dt;
double chassisAccelY =
(robotSpeeds.vyMetersPerSecond - prevRobotSpeeds.vyMetersPerSecond) / dt;
double chassisForceX = chassisAccelX * config.massKG;
double chassisForceY = chassisAccelY * config.massKG;

double angularAccel =
(robotSpeeds.omegaRadiansPerSecond - prevRobotSpeeds.omegaRadiansPerSecond) / dt;
double angTorque = angularAccel * config.MOI;
ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque);

Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces);
double[] accelFF = new double[config.numModules];
double[] linearForceFF = new double[config.numModules];
double[] torqueCurrentFF = new double[config.numModules];
double[] forceXFF = new double[config.numModules];
double[] forceYFF = new double[config.numModules];
for (int m = 0; m < config.numModules; m++) {
double wheelForceDist = wheelForces[m].getNorm();
double appliedForce =
wheelForceDist > 1e-6
? wheelForceDist
* wheelForces[m].getAngle().minus(state.moduleStates[m].angle).getCos()
: 0.0;
double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters;
double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque);

accelFF[m] =
(state.moduleStates[m].speedMetersPerSecond
- prevState.moduleStates[m].speedMetersPerSecond)
/ dt;
linearForceFF[m] = appliedForce;
torqueCurrentFF[m] = torqueCurrent;
forceXFF[m] = wheelForces[m].getX();
forceYFF[m] = wheelForces[m].getY();
double sumV = v + v0;
if (Math.abs(sumV) < 1e-6) {
state.timeSeconds = prevState.timeSeconds;
if (i != 1) {
prevState.feedforwards = states.get(i - 2).feedforwards;
} else {
prevState.feedforwards = DriveFeedforwards.zeros(config.numModules);
}
} else {
double dt = (2 * state.deltaPos) / sumV;
state.timeSeconds = prevState.timeSeconds + dt;

ChassisSpeeds prevRobotSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
prevState.fieldSpeeds, prevState.pose.getRotation());
ChassisSpeeds robotSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(state.fieldSpeeds, state.pose.getRotation());
double chassisAccelX =
(robotSpeeds.vxMetersPerSecond - prevRobotSpeeds.vxMetersPerSecond) / dt;
double chassisAccelY =
(robotSpeeds.vyMetersPerSecond - prevRobotSpeeds.vyMetersPerSecond) / dt;
double chassisForceX = chassisAccelX * config.massKG;
double chassisForceY = chassisAccelY * config.massKG;

double angularAccel =
(robotSpeeds.omegaRadiansPerSecond - prevRobotSpeeds.omegaRadiansPerSecond) / dt;
double angTorque = angularAccel * config.MOI;
ChassisSpeeds chassisForces = new ChassisSpeeds(chassisForceX, chassisForceY, angTorque);

Translation2d[] wheelForces = config.chassisForcesToWheelForceVectors(chassisForces);
double[] accelFF = new double[config.numModules];
double[] linearForceFF = new double[config.numModules];
double[] torqueCurrentFF = new double[config.numModules];
double[] forceXFF = new double[config.numModules];
double[] forceYFF = new double[config.numModules];
for (int m = 0; m < config.numModules; m++) {
double wheelForceDist = wheelForces[m].getNorm();
double appliedForce =
wheelForceDist > 1e-6
? wheelForceDist
* wheelForces[m].getAngle().minus(state.moduleStates[m].angle).getCos()
: 0.0;
double wheelTorque = appliedForce * config.moduleConfig.wheelRadiusMeters;
double torqueCurrent = config.moduleConfig.driveMotor.getCurrent(wheelTorque);

accelFF[m] =
(state.moduleStates[m].speedMetersPerSecond
- prevState.moduleStates[m].speedMetersPerSecond)
/ dt;
linearForceFF[m] = appliedForce;
torqueCurrentFF[m] = torqueCurrent;
forceXFF[m] = wheelForces[m].getX();
forceYFF[m] = wheelForces[m].getY();
}
prevState.feedforwards =
new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF);
}
prevState.feedforwards =
new DriveFeedforwards(accelFF, linearForceFF, torqueCurrentFF, forceXFF, forceYFF);

// Un-added events have their timestamp set to a waypoint relative position
// When adding the event to this trajectory, set its timestamp properly
Expand Down
116 changes: 65 additions & 51 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,59 +109,73 @@ PathPlannerTrajectory::PathPlannerTrajectory(

units::meters_per_second_t v0 = prevState.linearVelocity;
units::meters_per_second_t v = state.linearVelocity;
units::second_t dt = (2 * state.deltaPos) / (v + v0);
state.time = prevState.time + dt;

frc::ChassisSpeeds prevRobotSpeeds =
frc::ChassisSpeeds::FromFieldRelativeSpeeds(
prevState.fieldSpeeds, prevState.pose.Rotation());
frc::ChassisSpeeds robotSpeeds =
frc::ChassisSpeeds::FromFieldRelativeSpeeds(
state.fieldSpeeds, state.pose.Rotation());

auto chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt;
auto chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt;
auto chassisForceX = chassisAccelX * config.mass;
auto chassisForceY = chassisAccelY * config.mass;

auto angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega)
/ dt;
auto angTorque = angularAccel * config.MOI;
frc::ChassisSpeeds chassisForces { units::meters_per_second_t {
chassisForceX() }, units::meters_per_second_t {
chassisForceY() },
units::radians_per_second_t { angTorque() }, };

auto wheelForces = config.chassisForcesToWheelForceVectors(
chassisForces);
std::vector < units::meters_per_second_squared_t > accelFF;
std::vector < units::newton_t > linearForceFF;
std::vector < units::ampere_t > torqueCurrentFF;
std::vector < units::newton_t > forceXFF;
std::vector < units::newton_t > forceYFF;
for (size_t m = 0; m < config.numModules; m++) {
units::meter_t wheelForceDist = wheelForces[m].Norm();
units::newton_t appliedForce { 0.0 };
if (wheelForceDist() > 1e-6) {
appliedForce = units::newton_t { wheelForceDist()
* (wheelForces[m].Angle()
- state.moduleStates[m].angle).Cos() };
units::meters_per_second_t sumV = v + v0;
if (units::math::abs(sumV) < 1e-6_mps) {
state.time = prevState.time;
if (i != 1) {
prevState.feedforwards = m_states[i - 2].feedforwards;
} else {
prevState.feedforwards = DriveFeedforwards::zeros(
config.numModules);
}
} else {
units::second_t dt = (2 * state.deltaPos) / sumV;
state.time = prevState.time + dt;

frc::ChassisSpeeds prevRobotSpeeds =
frc::ChassisSpeeds::FromFieldRelativeSpeeds(
prevState.fieldSpeeds,
prevState.pose.Rotation());
frc::ChassisSpeeds robotSpeeds =
frc::ChassisSpeeds::FromFieldRelativeSpeeds(
state.fieldSpeeds, state.pose.Rotation());

auto chassisAccelX = (robotSpeeds.vx - prevRobotSpeeds.vx) / dt;
auto chassisAccelY = (robotSpeeds.vy - prevRobotSpeeds.vy) / dt;
auto chassisForceX = chassisAccelX * config.mass;
auto chassisForceY = chassisAccelY * config.mass;

auto angularAccel = (robotSpeeds.omega - prevRobotSpeeds.omega)
/ dt;
auto angTorque = angularAccel * config.MOI;
frc::ChassisSpeeds chassisForces { units::meters_per_second_t {
chassisForceX() }, units::meters_per_second_t {
chassisForceY() }, units::radians_per_second_t {
angTorque() }, };

auto wheelForces = config.chassisForcesToWheelForceVectors(
chassisForces);
std::vector < units::meters_per_second_squared_t > accelFF;
std::vector < units::newton_t > linearForceFF;
std::vector < units::ampere_t > torqueCurrentFF;
std::vector < units::newton_t > forceXFF;
std::vector < units::newton_t > forceYFF;
for (size_t m = 0; m < config.numModules; m++) {
units::meter_t wheelForceDist = wheelForces[m].Norm();
units::newton_t appliedForce { 0.0 };
if (wheelForceDist() > 1e-6) {
appliedForce = units::newton_t { wheelForceDist()
* (wheelForces[m].Angle()
- state.moduleStates[m].angle).Cos() };
}
units::newton_meter_t wheelTorque = appliedForce
* config.moduleConfig.wheelRadius;
units::ampere_t torqueCurrent =
config.moduleConfig.driveMotor.Current(wheelTorque);

accelFF.emplace_back(
(state.moduleStates[m].speed
- prevState.moduleStates[m].speed) / dt);
linearForceFF.emplace_back(appliedForce);
torqueCurrentFF.emplace_back(torqueCurrent);
forceXFF.emplace_back(
units::newton_t { wheelForces[m].X()() });
forceYFF.emplace_back(
units::newton_t { wheelForces[m].Y()() });
}
units::newton_meter_t wheelTorque = appliedForce
* config.moduleConfig.wheelRadius;
units::ampere_t torqueCurrent =
config.moduleConfig.driveMotor.Current(wheelTorque);

accelFF.emplace_back(
(state.moduleStates[m].speed
- prevState.moduleStates[m].speed) / dt);
linearForceFF.emplace_back(appliedForce);
torqueCurrentFF.emplace_back(torqueCurrent);
forceXFF.emplace_back(units::newton_t { wheelForces[m].X()() });
forceYFF.emplace_back(units::newton_t { wheelForces[m].Y()() });
prevState.feedforwards = DriveFeedforwards { accelFF,
linearForceFF, torqueCurrentFF, forceXFF, forceYFF };
}
prevState.feedforwards = DriveFeedforwards { accelFF, linearForceFF,
torqueCurrentFF, forceXFF, forceYFF };

// Un-added events have their timestamp set to a waypoint relative position
// When adding the event to this trajectory, set its timestamp properly
Expand Down
2 changes: 1 addition & 1 deletion pathplannerlib/src/main/native/include/pathplanner/lib/path/Waypoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@

namespace pathplanner {

#define AUTO_CONTROL_DISTANCE_FACTOR 1.0 / 3.0
#define AUTO_CONTROL_DISTANCE_FACTOR 0.4

class Waypoint {
public:
Expand Down
Loading