Skip to content

Commit

Permalink
Add More Empty Translation Checks (#911)
Browse files Browse the repository at this point in the history
  • Loading branch information
@PickleFace5
PickleFace5 authored Nov 25, 2024
1 parent 48a2704 commit 61cde90
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 15 deletions.
23 changes: 18 additions & 5 deletions pathplannerlib-python/pathplannerlib/trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,7 +435,11 @@ def _generateStates(states: List[PathPlannerTrajectoryState], path: PathPlannerP

# Calculate robot heading
if i != path.numPoints() - 1:
state.heading = (path.getPoint(i + 1).position - state.pose.translation()).angle()
headingTranslation = path.getPoint(i + 1).position - state.pose.translation()
if headingTranslation.norm() <= 1e-6:
state.heading = Rotation2d()
else:
state.heading = headingTranslation.angle()
else:
state.heading = states[i - 1].heading

Expand All @@ -462,8 +466,11 @@ def _generateStates(states: List[PathPlannerTrajectoryState], path: PathPlannerP
for i in range(len(states)):
for m in range(config.numModules):
if i != len(states) - 1:
states[i].moduleStates[m].fieldAngle = (
states[i + 1].moduleStates[m].fieldPos - states[i].moduleStates[m].fieldPos).angle()
fieldTranslation = states[i + 1].moduleStates[m].fieldPos - states[i].moduleStates[m].fieldPos
if fieldTranslation.norm() <= 1e-6:
states[i].moduleStates[m].fieldAngle = Rotation2d()
else:
states[i].moduleStates[m].fieldAngle = fieldTranslation.angle()
states[i].moduleStates[m].angle = states[i].moduleStates[m].fieldAngle - states[i].pose.rotation()
else:
states[i].moduleStates[m].fieldAngle = states[i - 1].moduleStates[m].fieldAngle
Expand Down Expand Up @@ -497,7 +504,10 @@ def _forwardAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon

# Calculate the torque this module will apply to the robot
angleToModule = (state.moduleStates[m].fieldPos - state.pose.translation()).angle()
theta = forceVec.angle() - angleToModule
if forceVec.norm() <= 1e-6:
theta = Rotation2d() - angleToModule
else:
theta = forceVec.angle() - angleToModule
totalTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.sin()

# Use the robot accelerations to calculate how each module should accelerate
Expand Down Expand Up @@ -596,7 +606,10 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon

# Calculate the torque this module will apply to the robot
angleToModule = (state.moduleStates[m].fieldPos - state.pose.translation()).angle()
theta = forceVec.angle() - angleToModule
if forceVec.norm() <= 1e-6:
theta = Rotation2d() - angleToModule
else:
theta = forceVec.angle() - angleToModule
totalTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.sin()

# Use the robot accelerations to calculate how each module should accelerate
Expand Down
32 changes: 26 additions & 6 deletions pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,13 @@ private static void generateStates(

// Calculate robot heading
if (i != path.numPoints() - 1) {
state.heading = path.getPoint(i + 1).position.minus(state.pose.getTranslation()).getAngle();
Translation2d headingTranslation =
path.getPoint(i + 1).position.minus(state.pose.getTranslation());
if (headingTranslation.getNorm() <= 1e-6) {
state.heading = Rotation2d.kZero;
} else {
state.heading = headingTranslation.getAngle();
}
} else {
state.heading = states.get(i - 1).heading;
}
Expand Down Expand Up @@ -284,13 +290,17 @@ private static void generateStates(
for (int i = 0; i < states.size(); i++) {
for (int m = 0; m < config.numModules; m++) {
if (i != states.size() - 1) {
states.get(i).moduleStates[m].fieldAngle =
Translation2d fieldTranslation =
states
.get(i + 1)
.moduleStates[m]
.fieldPos
.minus(states.get(i).moduleStates[m].fieldPos)
.getAngle();
.minus(states.get(i).moduleStates[m].fieldPos);
if (fieldTranslation.getNorm() <= 1e-6) {
states.get(i).moduleStates[m].fieldAngle = Rotation2d.kZero;
} else {
states.get(i).moduleStates[m].fieldAngle = fieldTranslation.getAngle();
}
states.get(i).moduleStates[m].angle =
states.get(i).moduleStates[m].fieldAngle.minus(states.get(i).pose.getRotation());
} else {
Expand Down Expand Up @@ -335,7 +345,12 @@ private static void forwardAccelPass(
// Calculate the torque this module will apply to the robot
Rotation2d angleToModule =
state.moduleStates[m].fieldPos.minus(state.pose.getTranslation()).getAngle();
Rotation2d theta = forceVec.getAngle().minus(angleToModule);
Rotation2d theta;
if (forceVec.getNorm() <= 1e-6) {
theta = Rotation2d.kZero.minus(angleToModule);
} else {
theta = forceVec.getAngle().minus(angleToModule);
}
totalTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin();
}

Expand Down Expand Up @@ -472,7 +487,12 @@ private static void reverseAccelPass(
// Calculate the torque this module will apply to the robot
Rotation2d angleToModule =
state.moduleStates[m].fieldPos.minus(state.pose.getTranslation()).getAngle();
Rotation2d theta = forceVec.getAngle().minus(angleToModule);
Rotation2d theta;
if (forceVec.getNorm() <= 1e-6) {
theta = Rotation2d.kZero.minus(angleToModule);
} else {
theta = forceVec.getAngle().minus(angleToModule);
}
totalTorque += forceAtCarpet * config.modulePivotDistance[m] * theta.getSin();
}

Expand Down
32 changes: 28 additions & 4 deletions pathplannerlib/src/main/native/cpp/pathplanner/lib/trajectory/PathPlannerTrajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -262,8 +262,13 @@ void PathPlannerTrajectory::generateStates(

// Calculate robot heading
if (i != path->numPoints() - 1) {
state.heading = (path->getPoint(i + 1).position
- state.pose.Translation()).Angle();
frc::Translation2d headingTranslation =
path->getPoint(i + 1).position - state.pose.Translation();
if (headingTranslation.Norm()() <= 1e-6) {
state.heading = frc::Rotation2d();
} else {
state.heading = headingTranslation.Angle();
}
} else {
state.heading = states[i - 1].heading;
}
Expand Down Expand Up @@ -305,6 +310,15 @@ void PathPlannerTrajectory::generateStates(
states[i].moduleStates[m].fieldAngle =
(states[i + 1].moduleStates[m].fieldPos
- states[i].moduleStates[m].fieldPos).Angle();
frc::Translation2d fieldTranslation =
states[i + 1].moduleStates[m].fieldPos
- states[i].moduleStates[m].fieldPos;
if (fieldTranslation.Norm()() <= 1e-6) {
states[i].moduleStates[m].fieldAngle = frc::Rotation2d();
} else {
states[i].moduleStates[m].fieldAngle =
fieldTranslation.Angle();
}
states[i].moduleStates[m].angle =
states[i].moduleStates[m].fieldAngle
- states[i].pose.Rotation();
Expand Down Expand Up @@ -357,7 +371,12 @@ void PathPlannerTrajectory::forwardAccelPass(
// Calculate the torque this module will apply to the robot
frc::Rotation2d angleToModule = (state.moduleStates[m].fieldPos
- state.pose.Translation()).Angle();
frc::Rotation2d theta = forceVec.Angle() - angleToModule;
frc::Rotation2d theta;
if (forceVec.Norm()() <= 1e-6) {
theta = frc::Rotation2d() - angleToModule;
} else {
theta = forceVec.Angle() - angleToModule;
}
totalTorque += forceAtCarpet * config.modulePivotDistance[m]
* theta.Sin();
}
Expand Down Expand Up @@ -510,7 +529,12 @@ void PathPlannerTrajectory::reverseAccelPass(
// Calculate the torque this module will apply to the robot
frc::Rotation2d angleToModule = (state.moduleStates[m].fieldPos
- state.pose.Translation()).Angle();
frc::Rotation2d theta = forceVec.Angle() - angleToModule;
frc::Rotation2d theta;
if (forceVec.Norm()() <= 1e-6) {
theta = frc::Rotation2d() - angleToModule;
} else {
theta = forceVec.Angle() - angleToModule;
}
totalTorque += forceAtCarpet * config.modulePivotDistance[m]
* theta.Sin();
}
Expand Down

0 comments on commit 61cde90

Please sign in to comment.