From efe8c522f1739353bcc5d0fb9bac886c42030169 Mon Sep 17 00:00:00 2001 From: Alberto Jahuey Moncada Date: Fri, 26 Jan 2024 15:02:00 -0600 Subject: [PATCH 1/2] Fix idempotency and flipping issue with PathfindingCommand (#578) --- .../pathplannerlib/commands.py | 6 +++++- .../lib/commands/PathfindingCommand.java | 10 ++++++++-- .../lib/commands/PathfindingCommand.cpp | 19 +++++++++++-------- .../lib/commands/PathfindingCommand.h | 1 + 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py index f7ecc02f..7fc27965 100644 --- a/pathplannerlib-python/pathplannerlib/commands.py +++ b/pathplannerlib-python/pathplannerlib/commands.py @@ -275,6 +275,7 @@ class PathfindingCommand(Command): _timer: Timer = Timer() _targetPath: Union[PathPlannerPath, None] _targetPose: Pose2d + _originalTargetPose: Pose2d _goalEndState: GoalEndState _constraints: PathConstraints _poseSupplier: Callable[[], Pose2d] @@ -332,10 +333,12 @@ def __init__(self, constraints: PathConstraints, pose_supplier: Callable[[], Pos break self._targetPath = target_path self._targetPose = Pose2d(target_path.getPoint(0).position, targetRotation) + self._originalTargetPose = Pose2d(target_path.getPoint(0).position, targetRotation) self._goalEndState = GoalEndState(goalEndVel, targetRotation, True) else: self._targetPath = None self._targetPose = target_pose + self._originalTargetPose = target_pose self._goalEndState = GoalEndState(goal_end_vel, target_pose.rotation(), True) PathfindingCommand._instances += 1 @@ -352,7 +355,8 @@ def initialize(self): if self._targetPath is not None: self._targetPose = Pose2d(self._targetPath.getPoint(0).position, self._goalEndState.rotation) if self._shouldFlipPath(): - self._targetPose = flipFieldPose(self._targetPose) + self._targetPose = flipFieldPose(self._originalTargetPose) + self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation(), True) if currentPose.translation().distance(self._targetPose.translation()) < 0.5: self._output(ChassisSpeeds()) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 35e25114..a5bfeaef 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -27,7 +27,8 @@ public class PathfindingCommand extends Command { private final Timer timer = new Timer(); private final PathPlannerPath targetPath; private Pose2d targetPose; - private final GoalEndState goalEndState; + private Pose2d originalTargetPose; + private GoalEndState goalEndState; private final PathConstraints constraints; private final Supplier poseSupplier; private final Supplier speedsSupplier; @@ -94,6 +95,8 @@ public PathfindingCommand( this.targetPath = targetPath; this.targetPose = new Pose2d(this.targetPath.getPoint(0).position, targetRotation); + this.originalTargetPose = + new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); this.goalEndState = new GoalEndState(goalEndVel, targetRotation, true); this.constraints = constraints; this.controller = controller; @@ -141,6 +144,8 @@ public PathfindingCommand( this.targetPath = null; this.targetPose = targetPose; + this.originalTargetPose = + new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); this.goalEndState = new GoalEndState(goalEndVel, targetPose.getRotation(), true); this.constraints = constraints; this.controller = controller; @@ -167,7 +172,8 @@ public void initialize() { if (targetPath != null) { targetPose = new Pose2d(this.targetPath.getPoint(0).position, goalEndState.getRotation()); if (shouldFlipPath.getAsBoolean()) { - targetPose = GeometryUtil.flipFieldPose(targetPose); + targetPose = GeometryUtil.flipFieldPose(this.originalTargetPose); + goalEndState = new GoalEndState(goalEndState.getVelocity(), targetPose.getRotation()); } } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index 3ca94e1b..38b68b71 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -46,6 +46,7 @@ PathfindingCommand::PathfindingCommand( m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position, targetRotation); + m_originalTargetPose = m_targetPose; m_goalEndState = GoalEndState(goalEndVel, targetRotation, true); m_instances++; @@ -61,13 +62,13 @@ PathfindingCommand::PathfindingCommand(frc::Pose2d targetPose, std::unique_ptr controller, units::meter_t rotationDelayDistance, ReplanningConfig replanningConfig, frc2::Requirements requirements) : m_targetPath(), m_targetPose( - targetPose), m_goalEndState(goalEndVel, targetPose.Rotation(), true), m_constraints( - constraints), m_poseSupplier(poseSupplier), m_speedsSupplier( - speedsSupplier), m_output(output), m_controller(std::move(controller)), m_rotationDelayDistance( - rotationDelayDistance), m_replanningConfig(replanningConfig), m_shouldFlipPath( - []() { - return false; - }) { + targetPose), m_originalTargetPose(targetPose), m_goalEndState( + goalEndVel, targetPose.Rotation(), true), m_constraints(constraints), m_poseSupplier( + poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller( + std::move(controller)), m_rotationDelayDistance(rotationDelayDistance), m_replanningConfig( + replanningConfig), m_shouldFlipPath([]() { + return false; +}) { AddRequirements(requirements); Pathfinding::ensureInitialized(); @@ -89,7 +90,9 @@ void PathfindingCommand::Initialize() { m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position, m_goalEndState.getRotation()); if (m_shouldFlipPath()) { - m_targetPose = GeometryUtil::flipFieldPose(m_targetPose); + m_targetPose = GeometryUtil::flipFieldPose(m_originalTargetPose); + m_goalEndState = GoalEndState(m_goalEndState.getVelocity(), + m_targetPose.Rotation(), true); } } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h index aa50ac1b..aa2e2c08 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathfindingCommand.h @@ -86,6 +86,7 @@ class PathfindingCommand: public frc2::CommandHelper m_targetPath; frc::Pose2d m_targetPose; + frc::Pose2d m_originalTargetPose; GoalEndState m_goalEndState; PathConstraints m_constraints; std::function m_poseSupplier; From 6789c3e4aa54b74b9e331308214d479c6fcf1266 Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Fri, 26 Jan 2024 16:07:21 -0500 Subject: [PATCH 2/2] make sure original target pose gets updated on hot reload --- pathplannerlib-python/pathplannerlib/commands.py | 2 +- .../java/com/pathplanner/lib/commands/PathfindingCommand.java | 3 ++- .../cpp/pathplanner/lib/commands/PathfindingCommand.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/commands.py b/pathplannerlib-python/pathplannerlib/commands.py index 7fc27965..56b677a8 100644 --- a/pathplannerlib-python/pathplannerlib/commands.py +++ b/pathplannerlib-python/pathplannerlib/commands.py @@ -353,7 +353,7 @@ def initialize(self): self._controller.reset(currentPose, self._speedsSupplier()) if self._targetPath is not None: - self._targetPose = Pose2d(self._targetPath.getPoint(0).position, self._goalEndState.rotation) + self._originalTargetPose = Pose2d(self._targetPath.getPoint(0).position, self._originalTargetPose.rotation()) if self._shouldFlipPath(): self._targetPose = flipFieldPose(self._originalTargetPose) self._goalEndState = GoalEndState(self._goalEndState.velocity, self._targetPose.rotation(), True) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index a5bfeaef..7167101d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -170,7 +170,8 @@ public void initialize() { controller.reset(currentPose, speedsSupplier.get()); if (targetPath != null) { - targetPose = new Pose2d(this.targetPath.getPoint(0).position, goalEndState.getRotation()); + originalTargetPose = + new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation()); if (shouldFlipPath.getAsBoolean()) { targetPose = GeometryUtil.flipFieldPose(this.originalTargetPose); goalEndState = new GoalEndState(goalEndState.getVelocity(), targetPose.getRotation()); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index 38b68b71..c05ba4bc 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp @@ -87,8 +87,8 @@ void PathfindingCommand::Initialize() { m_controller->reset(currentPose, m_speedsSupplier()); if (m_targetPath) { - m_targetPose = frc::Pose2d(m_targetPath->getPoint(0).position, - m_goalEndState.getRotation()); + m_originalTargetPose = frc::Pose2d(m_targetPath->getPoint(0).position, + m_originalTargetPose.Rotation()); if (m_shouldFlipPath()) { m_targetPose = GeometryUtil::flipFieldPose(m_originalTargetPose); m_goalEndState = GoalEndState(m_goalEndState.getVelocity(),