From 616405f7aebe6ee937bac2cf275aea54508afafa Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Thu, 12 Nov 2020 01:37:14 -0500 Subject: [PATCH] [wpilib] Fix DiffDriveSim pose reset and example (#2837) Calling the resetPosition method on an odometry instance expects encoder positions to be reset to zero. --- .../main/native/cpp/simulation/DifferentialDrivetrainSim.cpp | 2 ++ .../first/wpilibj/simulation/DifferentialDrivetrainSim.java | 2 ++ .../statespacedifferentialdrivesimulation/RobotContainer.java | 3 +++ .../subsystems/DriveSubsystem.java | 1 + 4 files changed, 8 insertions(+) diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 73a0a0892a..51ed94a66c 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -98,6 +98,8 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) { m_x(State::kX) = pose.X().to(); m_x(State::kY) = pose.Y().to(); m_x(State::kHeading) = pose.Rotation().Radians().to(); + m_x(State::kLeftPosition) = 0; + m_x(State::kRightPosition) = 0; } Eigen::Matrix DifferentialDrivetrainSim::Dynamics( diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 79e67bc072..2e608bc36c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -192,6 +192,8 @@ public class DifferentialDrivetrainSim { m_x.set(State.kX.value, 0, pose.getX()); m_x.set(State.kY.value, 0, pose.getY()); m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians()); + m_x.set(State.kLeftPosition.value, 0, 0); + m_x.set(State.kRightPosition.value, 0, 0); } @SuppressWarnings({"DuplicatedCode", "LocalVariableName"}) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java index f63fcbdad6..0790a40d14 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java @@ -130,6 +130,9 @@ public class RobotContainer { m_robotDrive ); + // Reset odometry to starting pose of trajectory. + m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()); + // Run path following command, then stop at the end. return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java index 90620533fb..aaa0d8c29c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -163,6 +163,7 @@ public class DriveSubsystem extends SubsystemBase { */ public void resetOdometry(Pose2d pose) { resetEncoders(); + m_drivetrainSimulator.setPose(pose); m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); }