Skip to content

Commit

Permalink
Add ability to mirror paths & autos (#987)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mjansen4857
mjansen4857 authored Jan 8, 2025
1 parent a11e5e4 commit 068a610
Show file tree
Hide file tree
Showing 5 changed files with 231 additions and 37 deletions.
51 changes: 30 additions & 21 deletions pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,23 +31,25 @@ public static Command wrappedEventCommand(Command eventCommand) {
*
* @param commandJson the JSON object to build the command from
* @param loadChoreoPaths Load path commands using choreo trajectories
* @param mirror Should the paths be mirrored
* @return a command built from the JSON object
* @throws IOException if attempting to load a path file that does not exist or cannot be read
* @throws ParseException If attempting to load a path with JSON that cannot be parsed
*/
public static Command commandFromJson(JSONObject commandJson, boolean loadChoreoPaths)
public static Command commandFromJson(
JSONObject commandJson, boolean loadChoreoPaths, boolean mirror)
throws IOException, ParseException {
String type = (String) commandJson.get("type");
JSONObject data = (JSONObject) commandJson.get("data");

return switch (type) {
case "wait" -> waitCommandFromData(data);
case "named" -> namedCommandFromData(data);
case "path" -> pathCommandFromData(data, loadChoreoPaths);
case "sequential" -> sequentialGroupFromData(data, loadChoreoPaths);
case "parallel" -> parallelGroupFromData(data, loadChoreoPaths);
case "race" -> raceGroupFromData(data, loadChoreoPaths);
case "deadline" -> deadlineGroupFromData(data, loadChoreoPaths);
case "path" -> pathCommandFromData(data, loadChoreoPaths, mirror);
case "sequential" -> sequentialGroupFromData(data, loadChoreoPaths, mirror);
case "parallel" -> parallelGroupFromData(data, loadChoreoPaths, mirror);
case "race" -> raceGroupFromData(data, loadChoreoPaths, mirror);
case "deadline" -> deadlineGroupFromData(data, loadChoreoPaths, mirror);
default -> Commands.none();
};
}
Expand All @@ -69,53 +71,60 @@ private static Command namedCommandFromData(JSONObject dataJson) {
return NamedCommands.getCommand(name);
}

private static Command pathCommandFromData(JSONObject dataJson, boolean choreoPath)
throws IOException, ParseException {
private static Command pathCommandFromData(
JSONObject dataJson, boolean choreoPath, boolean mirror) throws IOException, ParseException {
String pathName = (String) dataJson.get("pathName");

if (choreoPath) {
return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(pathName));
} else {
return AutoBuilder.followPath(PathPlannerPath.fromPathFile(pathName));
PathPlannerPath path =
choreoPath
? PathPlannerPath.fromChoreoTrajectory(pathName)
: PathPlannerPath.fromPathFile(pathName);
if (mirror) {
path = path.mirrorPath();
}
return AutoBuilder.followPath(path);
}

private static Command sequentialGroupFromData(JSONObject dataJson, boolean loadChoreoPaths)
private static Command sequentialGroupFromData(
JSONObject dataJson, boolean loadChoreoPaths, boolean mirror)
throws IOException, ParseException {
SequentialCommandGroup group = new SequentialCommandGroup();
for (var cmdJson : (JSONArray) dataJson.get("commands")) {
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths));
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths, mirror));
}
return group;
}

private static Command parallelGroupFromData(JSONObject dataJson, boolean loadChoreoPaths)
private static Command parallelGroupFromData(
JSONObject dataJson, boolean loadChoreoPaths, boolean mirror)
throws IOException, ParseException {
ParallelCommandGroup group = new ParallelCommandGroup();
for (var cmdJson : (JSONArray) dataJson.get("commands")) {
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths));
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths, mirror));
}
return group;
}

private static Command raceGroupFromData(JSONObject dataJson, boolean loadChoreoPaths)
private static Command raceGroupFromData(
JSONObject dataJson, boolean loadChoreoPaths, boolean mirror)
throws IOException, ParseException {
ParallelRaceGroup group = new ParallelRaceGroup();
for (var cmdJson : (JSONArray) dataJson.get("commands")) {
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths));
group.addCommands(commandFromJson((JSONObject) cmdJson, loadChoreoPaths, mirror));
}
return group;
}

private static Command deadlineGroupFromData(JSONObject dataJson, boolean loadChoreoPaths)
private static Command deadlineGroupFromData(
JSONObject dataJson, boolean loadChoreoPaths, boolean mirror)
throws IOException, ParseException {
JSONArray cmds = (JSONArray) dataJson.get("commands");

if (!cmds.isEmpty()) {
Command deadline = commandFromJson((JSONObject) cmds.get(0), loadChoreoPaths);
Command deadline = commandFromJson((JSONObject) cmds.get(0), loadChoreoPaths, mirror);
ParallelDeadlineGroup group = new ParallelDeadlineGroup(deadline);
for (int i = 1; i < cmds.size(); i++) {
group.addCommands(commandFromJson((JSONObject) cmds.get(i), loadChoreoPaths));
group.addCommands(commandFromJson((JSONObject) cmds.get(i), loadChoreoPaths, mirror));
}
return group;
} else {
Expand Down
32 changes: 24 additions & 8 deletions pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathPlannerAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,20 @@ public class PathPlannerAuto extends Command {
* autonomous routine
*/
public PathPlannerAuto(String autoName) {
this(autoName, false);
}

/**
* Constructs a new PathPlannerAuto command.
*
* @param autoName the name of the autonomous routine to load and run
* @param mirror Mirror all paths to the other side of the current alliance. For example, if a
* path is on the right of the blue alliance side of the field, it will be mirrored to the
* left of the blue alliance side of the field.
* @throws AutoBuilderException if AutoBuilder is not configured before attempting to load the
* autonomous routine
*/
public PathPlannerAuto(String autoName, boolean mirror) {
if (!AutoBuilder.isConfigured()) {
throw new AutoBuilderException(
"AutoBuilder was not configured before attempting to load a PathPlannerAuto from file");
Expand All @@ -86,7 +100,7 @@ public PathPlannerAuto(String autoName) {
throw new FileVersionException(version, "2025.X", autoName + ".auto");
}

initFromJson(json);
initFromJson(json, mirror);
} catch (FileNotFoundException e) {
DriverStation.reportError(e.getMessage(), e.getStackTrace());
autoCommand = Commands.none();
Expand Down Expand Up @@ -620,27 +634,29 @@ public static List<PathPlannerPath> getPathGroupFromAutoFile(String autoName)
*/
public void hotReload(JSONObject autoJson) {
try {
initFromJson(autoJson);
initFromJson(autoJson, false);
} catch (Exception e) {
DriverStation.reportError("Failed to load path during hot reload", e.getStackTrace());
}
}

private void initFromJson(JSONObject autoJson)
private void initFromJson(JSONObject autoJson, boolean mirror)
throws IOException, ParseException, FileVersionException {
boolean choreoAuto = autoJson.get("choreoAuto") != null && (boolean) autoJson.get("choreoAuto");
JSONObject commandJson = (JSONObject) autoJson.get("command");
Command cmd = CommandUtil.commandFromJson(commandJson, choreoAuto);
Command cmd = CommandUtil.commandFromJson(commandJson, choreoAuto, mirror);
boolean resetOdom = autoJson.get("resetOdom") != null && (boolean) autoJson.get("resetOdom");
List<PathPlannerPath> pathsInAuto = pathsFromCommandJson(commandJson, choreoAuto);
if (!pathsInAuto.isEmpty()) {
PathPlannerPath path0 = pathsInAuto.get(0);
if (mirror) {
path0 = path0.mirrorPath();
}
if (AutoBuilder.isHolonomic()) {
this.startingPose =
new Pose2d(
pathsInAuto.get(0).getPoint(0).position,
pathsInAuto.get(0).getIdealStartingState().rotation());
new Pose2d(path0.getPoint(0).position, path0.getIdealStartingState().rotation());
} else {
this.startingPose = pathsInAuto.get(0).getStartingDifferentialPose();
this.startingPose = path0.getStartingDifferentialPose();
}
} else {
this.startingPose = null;
Expand Down
2 changes: 1 addition & 1 deletion pathplannerlib/src/main/java/com/pathplanner/lib/path/EventMarker.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ static EventMarker fromJson(JSONObject markerJson) {
Command cmd = null;
if (markerJson.get("command") != null) {
try {
cmd = CommandUtil.commandFromJson((JSONObject) markerJson.get("command"), false);
cmd = CommandUtil.commandFromJson((JSONObject) markerJson.get("command"), false, false);
} catch (Exception ignored) {
// Path files won't be loaded from event markers
}
Expand Down
179 changes: 174 additions & 5 deletions pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,7 @@
import com.pathplanner.lib.events.ScheduleCommandEvent;
import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
import com.pathplanner.lib.util.DriveFeedforwards;
import com.pathplanner.lib.util.FileVersionException;
import com.pathplanner.lib.util.GeometryUtil;
import com.pathplanner.lib.util.PPLibTelemetry;
import com.pathplanner.lib.util.*;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
Expand Down Expand Up @@ -432,7 +429,7 @@ private static void loadChoreoTrajectoryIntoCache(String trajectoryName)

if (markerJson.get("event") != null) {
Command eventCommand =
CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true);
CommandUtil.commandFromJson((JSONObject) markerJson.get("event"), true, false);
fullEvents.add(new ScheduleCommandEvent(fromTimestamp, eventCommand));
}
}
Expand Down Expand Up @@ -1191,6 +1188,178 @@ public PathPlannerPath flipPath() {
return path;
}

private static Translation2d mirrorTranslation(Translation2d translation) {
return new Translation2d(translation.getX(), FlippingUtil.fieldSizeY - translation.getY());
}

/**
* Mirror a path to the other side of the current alliance. For example, if this path is on the
* right of the blue alliance side of the field, it will be mirrored to the left of the blue
* alliance side of the field.
*
* @return The mirrored path
*/
public PathPlannerPath mirrorPath() {
PathPlannerPath path = new PathPlannerPath();

Optional<PathPlannerTrajectory> mirroredTraj = Optional.empty();
if (idealTrajectory.isPresent()) {
PathPlannerTrajectory traj = idealTrajectory.get();
// Flip the ideal trajectory
mirroredTraj =
Optional.of(
new PathPlannerTrajectory(
traj.getStates().stream()
.map(
s -> {
var state = new PathPlannerTrajectoryState();

state.timeSeconds = s.timeSeconds;
state.linearVelocity = s.linearVelocity;
state.pose =
new Pose2d(
mirrorTranslation(s.pose.getTranslation()),
s.pose.getRotation().unaryMinus());
state.fieldSpeeds =
new ChassisSpeeds(
s.fieldSpeeds.vxMetersPerSecond,
-s.fieldSpeeds.vyMetersPerSecond,
-s.fieldSpeeds.omegaRadiansPerSecond);
DriveFeedforwards ff = s.feedforwards;
if (ff.accelerationsMPSSq().length == 4) {
state.feedforwards =
new DriveFeedforwards(
new double[] {
ff.accelerationsMPSSq()[1],
ff.accelerationsMPSSq()[0],
ff.accelerationsMPSSq()[3],
ff.accelerationsMPSSq()[2]
},
new double[] {
ff.linearForcesNewtons()[1],
ff.linearForcesNewtons()[0],
ff.linearForcesNewtons()[3],
ff.linearForcesNewtons()[2]
},
new double[] {
ff.torqueCurrentsAmps()[1],
ff.torqueCurrentsAmps()[0],
ff.torqueCurrentsAmps()[3],
ff.torqueCurrentsAmps()[2]
},
new double[] {
ff.robotRelativeForcesXNewtons()[1],
ff.robotRelativeForcesXNewtons()[0],
ff.robotRelativeForcesXNewtons()[3],
ff.robotRelativeForcesXNewtons()[2]
},
new double[] {
ff.robotRelativeForcesYNewtons()[1],
ff.robotRelativeForcesYNewtons()[0],
ff.robotRelativeForcesYNewtons()[3],
ff.robotRelativeForcesYNewtons()[2]
});
} else if (ff.accelerationsMPSSq().length == 2) {
state.feedforwards =
new DriveFeedforwards(
new double[] {
ff.accelerationsMPSSq()[1], ff.accelerationsMPSSq()[0]
},
new double[] {
ff.linearForcesNewtons()[1], ff.linearForcesNewtons()[0]
},
new double[] {
ff.torqueCurrentsAmps()[1], ff.torqueCurrentsAmps()[0]
},
new double[] {
ff.robotRelativeForcesXNewtons()[1],
ff.robotRelativeForcesXNewtons()[0]
},
new double[] {
ff.robotRelativeForcesYNewtons()[1],
ff.robotRelativeForcesYNewtons()[0]
});
} else {
state.feedforwards = ff;
}
state.heading = s.heading.unaryMinus();

return state;
})
.toList(),
traj.getEvents()));
}

path.waypoints =
waypoints.stream()
.map(
w -> {
Translation2d prevControl = null;
Translation2d anchor = mirrorTranslation(w.anchor());
Translation2d nextControl = null;

if (w.prevControl() != null) {
prevControl = mirrorTranslation(w.prevControl());
}
if (w.nextControl() != null) {
nextControl = mirrorTranslation(w.nextControl());
}
return new Waypoint(prevControl, anchor, nextControl);
})
.toList();
path.rotationTargets =
rotationTargets.stream()
.map(t -> new RotationTarget(t.position(), t.rotation().unaryMinus()))
.toList();
path.pointTowardsZones =
pointTowardsZones.stream()
.map(
z ->
new PointTowardsZone(
z.name(),
mirrorTranslation(z.targetPosition()),
z.rotationOffset(),
z.minPosition(),
z.maxPosition()))
.toList();
path.constraintZones = constraintZones;
path.eventMarkers = eventMarkers;
path.globalConstraints = globalConstraints;
if (idealStartingState != null) {
path.idealStartingState =
new IdealStartingState(
idealStartingState.velocityMPS(), idealStartingState.rotation().unaryMinus());
} else {
path.idealStartingState = null;
}
path.goalEndState =
new GoalEndState(goalEndState.velocityMPS(), goalEndState.rotation().unaryMinus());
path.allPoints =
allPoints.stream()
.map(
p -> {
PathPoint point = new PathPoint(mirrorTranslation(p.position));
point.distanceAlongPath = p.distanceAlongPath;
point.maxV = p.maxV;
if (p.rotationTarget != null) {
point.rotationTarget =
new RotationTarget(
p.rotationTarget.position(), p.rotationTarget.rotation().unaryMinus());
}
point.constraints = p.constraints;
point.waypointRelativePos = p.waypointRelativePos;
return point;
})
.toList();
path.reversed = reversed;
path.isChoreoPath = isChoreoPath;
path.idealTrajectory = mirroredTraj;
path.preventFlipping = preventFlipping;
path.name = name;

return path;
}

/**
* Get a list of poses representing every point in this path. This can be used to display a path
* on a field 2d widget, for example.
Expand Down
4 changes: 2 additions & 2 deletions pathplannerlib/src/main/java/com/pathplanner/lib/util/FlippingUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,8 @@ public static Translation2d flipFieldPosition(Translation2d pos) {
*/
public static Rotation2d flipFieldRotation(Rotation2d rotation) {
return switch (symmetryType) {
case kMirrored -> new Rotation2d(Math.PI).minus(rotation);
case kRotational -> rotation.minus(new Rotation2d(Math.PI));
case kMirrored -> Rotation2d.kPi.minus(rotation);
case kRotational -> rotation.minus(Rotation2d.kPi);
};
}

Expand Down

0 comments on commit 068a610

Please sign in to comment.