Skip to content

Commit

Permalink
Generate joining path for PathfindThenFollowPath commands (#956)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjansen4857
mjansen4857 authored Dec 30, 2024
1 parent a57174e commit 4066ffe
Show file tree
Hide file tree
Showing 7 changed files with 255 additions and 67 deletions.
38 changes: 21 additions & 17 deletions lib/trajectory/trajectory.dart
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,15 +264,17 @@ class PathPlannerTrajectory {
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (int m = 0; m < numModules; m++) {
Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
states[i - 1].moduleStates[m].angle;
if (prevRotDelta.degrees.abs() >= 60) {
continue;
if (maxDT > 0) {
// Recalculate all module velocities with the allowed DT
for (int m = 0; m < numModules; m++) {
Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
states[i - 1].moduleStates[m].angle;
if (prevRotDelta.degrees.abs() >= 60) {
continue;
}
states[i].moduleStates[m].speedMetersPerSecond =
states[i + 1].moduleStates[m].deltaPos / maxDT;
}
states[i].moduleStates[m].speedMetersPerSecond =
states[i + 1].moduleStates[m].deltaPos / maxDT;
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down Expand Up @@ -401,16 +403,18 @@ class PathPlannerTrajectory {
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (int m = 0; m < numModules; m++) {
Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
states[i - 1].moduleStates[m].angle;
if (prevRotDelta.degrees.abs() >= 60) {
continue;
if (maxDT > 0) {
// Recalculate all module velocities with the allowed DT
for (int m = 0; m < numModules; m++) {
Rotation2d prevRotDelta = states[i].moduleStates[m].angle -
states[i - 1].moduleStates[m].angle;
if (prevRotDelta.degrees.abs() >= 60) {
continue;
}

states[i].moduleStates[m].speedMetersPerSecond =
states[i + 1].moduleStates[m].deltaPos / maxDT;
}

states[i].moduleStates[m].speedMetersPerSecond =
states[i + 1].moduleStates[m].deltaPos / maxDT;
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down
53 changes: 49 additions & 4 deletions pathplannerlib-python/pathplannerlib/commands.py
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,17 @@
from math import hypot

from .controller import *
from .path import PathPlannerPath, GoalEndState, PathConstraints
from .path import PathPlannerPath, GoalEndState, PathConstraints, IdealStartingState
from .trajectory import PathPlannerTrajectory
from .telemetry import PPLibTelemetry
from .logging import PathPlannerLogging
from .util import floatLerp, FlippingUtil, DriveFeedforwards
from wpimath.geometry import Pose2d
from wpimath.geometry import Pose2d, Rotation2d
from wpimath.kinematics import ChassisSpeeds
from wpilib import Timer
from commands2 import Command, Subsystem, SequentialCommandGroup
from typing import Callable, List
from commands2 import Command, Subsystem, SequentialCommandGroup, DeferredCommand
import commands2.cmd as cmd
from typing import Callable
from .config import RobotConfig
from .pathfinding import Pathfinding
from .events import EventScheduler
Expand Down Expand Up @@ -408,6 +411,47 @@ def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathCons
"""
super().__init__()

def buildJoinCommand() -> Command:
if goal_path.numPoints() < 2:
return cmd.none()

startPose = pose_supplier()
startSpeeds = speeds_supplier()
startFieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(startSpeeds, startPose.rotation())

startHeading = Rotation2d(startFieldSpeeds.vx, startFieldSpeeds.vy)

endWaypoint = Pose2d(goal_path.getPoint(0).position, goal_path.getInitialHeading())
shouldFlip = should_flip_path() and not goal_path.preventFlipping
if shouldFlip:
endWaypoint = FlippingUtil.flipFieldPose(endWaypoint)

endState = GoalEndState(pathfinding_constraints.maxVelocityMps, startPose.rotation())
if goal_path.getIdealStartingState() is not None:
endRot = goal_path.getIdealStartingState().rotation
if shouldFlip:
endRot = FlippingUtil.flipFieldRotation(endRot)
endState = GoalEndState(goal_path.getIdealStartingState().velocity, endRot)

joinPath = PathPlannerPath(
PathPlannerPath.waypointsFromPoses([Pose2d(startPose.translation(), startHeading), endWaypoint]),
pathfinding_constraints,
IdealStartingState(hypot(startSpeeds.vx, startSpeeds.vy), startPose.rotation()),
endState
)
joinPath.preventFlipping = True

return FollowPathCommand(
joinPath,
pose_supplier,
speeds_supplier,
output,
controller,
robot_config,
should_flip_path,
*requirements
)

self.addCommands(
PathfindingCommand(
pathfinding_constraints,
Expand All @@ -420,6 +464,7 @@ def __init__(self, goal_path: PathPlannerPath, pathfinding_constraints: PathCons
*requirements,
target_path=goal_path
),
DeferredCommand(buildJoinCommand, requirements),
FollowPathCommand(
goal_path,
pose_supplier,
Expand Down
26 changes: 14 additions & 12 deletions pathplannerlib-python/pathplannerlib/trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -588,13 +588,14 @@ def _forwardAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
if maxDT == 0.0:
maxDT = realMaxDT

# Recalculate all module velocities with the allowed DT
for m in range(config.numModules):
prevRotDelta = state.moduleStates[m].angle - prevState.moduleStates[m].angle
if abs(prevRotDelta.degrees()) >= 60:
continue
if maxDT > 0:
# Recalculate all module velocities with the allowed DT
for m in range(config.numModules):
prevRotDelta = state.moduleStates[m].angle - prevState.moduleStates[m].angle
if abs(prevRotDelta.degrees()) >= 60:
continue

state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos / maxDT
state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos / maxDT

# Use the calculated module velocities to calculate the robot speeds
desiredSpeeds = config.toChassisSpeeds(state.moduleStates)
Expand Down Expand Up @@ -684,13 +685,14 @@ def _reverseAccelPass(states: List[PathPlannerTrajectoryState], config: RobotCon
if maxDT == 0.0:
maxDT = realMaxDT

# Recalculate all module velocities with the allowed DT
for m in range(config.numModules):
prevRotDelta = state.moduleStates[m].angle - states[i - 1].moduleStates[m].angle
if abs(prevRotDelta.degrees()) >= 60:
continue
if maxDT > 0:
# Recalculate all module velocities with the allowed DT
for m in range(config.numModules):
prevRotDelta = state.moduleStates[m].angle - states[i - 1].moduleStates[m].angle
if abs(prevRotDelta.degrees()) >= 60:
continue

state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos / maxDT
state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos / maxDT

# Use the calculated module velocities to calculate the robot speeds
desiredSpeeds = config.toChassisSpeeds(state.moduleStates)
Expand Down
64 changes: 64 additions & 0 deletions pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPath.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,19 @@

import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PathFollowingController;
import com.pathplanner.lib.path.GoalEndState;
import com.pathplanner.lib.path.IdealStartingState;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.DriveFeedforwards;
import com.pathplanner.lib.util.FlippingUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.Subsystem;
import java.util.Set;
import java.util.function.BiConsumer;
import java.util.function.BooleanSupplier;
import java.util.function.Supplier;
Expand Down Expand Up @@ -54,6 +60,64 @@ public PathfindThenFollowPath(
robotConfig,
shouldFlipPath,
requirements),
// Use a deferred command to generate an on-the-fly path to join
// the end of the pathfinding command to the start of the path
Commands.defer(
() -> {
if (goalPath.numPoints() < 2) {
return Commands.none();
}

Pose2d startPose = poseSupplier.get();
ChassisSpeeds startSpeeds = currentRobotRelativeSpeeds.get();
ChassisSpeeds startFieldSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(startSpeeds, startPose.getRotation());
Rotation2d startHeading =
new Rotation2d(
startFieldSpeeds.vxMetersPerSecond, startFieldSpeeds.vyMetersPerSecond);

Pose2d endWaypoint =
new Pose2d(goalPath.getPoint(0).position, goalPath.getInitialHeading());
boolean shouldFlip = shouldFlipPath.getAsBoolean() && !goalPath.preventFlipping;
if (shouldFlip) {
endWaypoint = FlippingUtil.flipFieldPose(endWaypoint);
}

GoalEndState endState;
if (goalPath.getIdealStartingState() != null) {
Rotation2d endRot = goalPath.getIdealStartingState().rotation();
if (shouldFlip) {
endRot = FlippingUtil.flipFieldRotation(endRot);
}
endState = new GoalEndState(goalPath.getIdealStartingState().velocityMPS(), endRot);
} else {
endState =
new GoalEndState(
pathfindingConstraints.maxVelocityMPS(), startPose.getRotation());
}

PathPlannerPath joinPath =
new PathPlannerPath(
PathPlannerPath.waypointsFromPoses(
new Pose2d(startPose.getTranslation(), startHeading), endWaypoint),
pathfindingConstraints,
new IdealStartingState(
Math.hypot(startSpeeds.vxMetersPerSecond, startSpeeds.vyMetersPerSecond),
startPose.getRotation()),
endState);
joinPath.preventFlipping = true;

return new FollowPathCommand(
joinPath,
poseSupplier,
currentRobotRelativeSpeeds,
output,
controller,
robotConfig,
shouldFlipPath,
requirements);
},
Set.of(requirements)),
new FollowPathCommand(
goalPath,
poseSupplier,
Expand Down
36 changes: 20 additions & 16 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 @@ -422,15 +422,17 @@ private static void forwardAccelPass(
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (int m = 0; m < config.numModules; m++) {
Rotation2d prevRotDelta =
state.moduleStates[m].angle.minus(prevState.moduleStates[m].angle);
if (Math.abs(prevRotDelta.getDegrees()) >= 60) {
continue;
}
if (maxDT > 0) {
// Recalculate all module velocities with the allowed DT
for (int m = 0; m < config.numModules; m++) {
Rotation2d prevRotDelta =
state.moduleStates[m].angle.minus(prevState.moduleStates[m].angle);
if (Math.abs(prevRotDelta.getDegrees()) >= 60) {
continue;
}

state.moduleStates[m].speedMetersPerSecond = nextState.moduleStates[m].deltaPos / maxDT;
state.moduleStates[m].speedMetersPerSecond = nextState.moduleStates[m].deltaPos / maxDT;
}
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down Expand Up @@ -551,15 +553,17 @@ private static void reverseAccelPass(
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (int m = 0; m < config.numModules; m++) {
Rotation2d prevRotDelta =
state.moduleStates[m].angle.minus(states.get(i - 1).moduleStates[m].angle);
if (Math.abs(prevRotDelta.getDegrees()) >= 60) {
continue;
}
if (maxDT > 0) {
// Recalculate all module velocities with the allowed DT
for (int m = 0; m < config.numModules; m++) {
Rotation2d prevRotDelta =
state.moduleStates[m].angle.minus(states.get(i - 1).moduleStates[m].angle);
if (Math.abs(prevRotDelta.getDegrees()) >= 60) {
continue;
}

state.moduleStates[m].speedMetersPerSecond = nextState.moduleStates[m].deltaPos / maxDT;
state.moduleStates[m].speedMetersPerSecond = nextState.moduleStates[m].deltaPos / maxDT;
}
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down
40 changes: 22 additions & 18 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 @@ -459,16 +459,18 @@ void PathPlannerTrajectory::forwardAccelPass(
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (size_t m = 0; m < config.numModules; m++) {
frc::Rotation2d prevRotDelta = state.moduleStates[m].angle
- prevState.moduleStates[m].angle;
if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) {
continue;
}
if (maxDT > 0_s) {
// Recalculate all module velocities with the allowed DT
for (size_t m = 0; m < config.numModules; m++) {
frc::Rotation2d prevRotDelta = state.moduleStates[m].angle
- prevState.moduleStates[m].angle;
if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) {
continue;
}

state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos
/ maxDT;
state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos
/ maxDT;
}
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down Expand Up @@ -604,16 +606,18 @@ void PathPlannerTrajectory::reverseAccelPass(
maxDT = realMaxDT;
}

// Recalculate all module velocities with the allowed DT
for (size_t m = 0; m < config.numModules; m++) {
frc::Rotation2d prevRotDelta = state.moduleStates[m].angle
- states[i - 1].moduleStates[m].angle;
if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) {
continue;
}
if (maxDT > 0_s) {
// Recalculate all module velocities with the allowed DT
for (size_t m = 0; m < config.numModules; m++) {
frc::Rotation2d prevRotDelta = state.moduleStates[m].angle
- states[i - 1].moduleStates[m].angle;
if (units::math::abs(prevRotDelta.Degrees()) >= 60_deg) {
continue;
}

state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos
/ maxDT;
state.moduleStates[m].speed = nextState.moduleStates[m].deltaPos
/ maxDT;
}
}

// Use the calculated module velocities to calculate the robot speeds
Expand Down
Loading

0 comments on commit 4066ffe

Please sign in to comment.