diff --git a/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp new file mode 100644 index 0000000000..7482e22662 --- /dev/null +++ b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/HolonomicDriveController.h" + +#include + +using namespace frc; + +HolonomicDriveController::HolonomicDriveController( + const frc2::PIDController& xController, + const frc2::PIDController& yController, + const ProfiledPIDController& thetaController) + : m_xController(xController), + m_yController(yController), + m_thetaController(thetaController) {} + +bool HolonomicDriveController::AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_poseError.Rotation(); + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); +} + +void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) { + m_poseTolerance = tolerance; +} + +ChassisSpeeds HolonomicDriveController::Calculate( + const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) { + // Calculate feedforward velocities (field-relative) + auto xFF = linearVelocityRef * poseRef.Rotation().Cos(); + auto yFF = linearVelocityRef * poseRef.Rotation().Sin(); + auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate( + currentPose.Rotation().Radians(), angleRef.Radians())); + + m_poseError = poseRef.RelativeTo(currentPose); + + if (!m_enabled) { + return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF, + currentPose.Rotation()); + } + + // Calculate feedback velocities (based on position error). + auto xFeedback = units::meters_per_second_t(m_xController.Calculate( + currentPose.X().to(), poseRef.X().to())); + auto yFeedback = units::meters_per_second_t(m_yController.Calculate( + currentPose.Y().to(), poseRef.Y().to())); + + // Return next output. + return ChassisSpeeds::FromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation()); +} + +ChassisSpeeds HolonomicDriveController::Calculate( + const Pose2d& currentPose, const Trajectory::State& desiredState, + const Rotation2d& angleRef) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + angleRef); +} + +void HolonomicDriveController::SetEnabled(bool enabled) { m_enabled = enabled; } diff --git a/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h new file mode 100644 index 0000000000..916e1a9c9a --- /dev/null +++ b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -0,0 +1,112 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "frc/controller/PIDController.h" +#include "frc/controller/ProfiledPIDController.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/ChassisSpeeds.h" +#include "frc/trajectory/Trajectory.h" + +namespace frc { +/** + * This holonomic drive controller can be used to follow trajectories using a + * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory + * following is a much simpler problem to solve compared to skid-steer style + * drivetrains because it is possible to individually control forward, sideways, + * and angular velocity. + * + * The holonomic drive controller takes in one PID controller for each + * direction, forward and sideways, and one profiled PID controller for the + * angular direction. Because the heading dynamics are decoupled from + * translations, users can specify a custom heading that the drivetrain should + * point toward. This heading reference is profiled for smoothness. + */ +class HolonomicDriveController { + public: + /** + * Constructs a holonomic drive controller. + * + * @param xController A PID Controller to respond to error in the + * field-relative x direction. + * @param yController A PID Controller to respond to error in the + * field-relative y direction. + * @param thetaController A profiled PID controller to respond to error in + * angle. + */ + HolonomicDriveController( + const frc2::PIDController& xController, + const frc2::PIDController& yController, + const ProfiledPIDController& thetaController); + + /** + * Returns true if the pose error is within tolerance of the reference. + */ + bool AtReference() const; + + /** + * Sets the pose error which is considered tolerable for use with + * AtReference(). + * + * @param poseTolerance Pose error which is tolerable. + */ + void SetTolerance(const Pose2d& tolerance); + + /** + * Returns the next output of the holonomic drive controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRef The desired linear velocity. + * @param angleRef The desired ending angle. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + const Rotation2d& angleRef); + + /** + * Returns the next output of the holonomic drive controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param desiredState The desired pose, linear velocity, and angular velocity + * from a trajectory. + * @param angleRef The desired ending angle. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, + const Trajectory::State& desiredState, + const Rotation2d& angleRef); + + /** + * Enables and disables the controller for troubleshooting purposes. When + * Calculate() is called on a disabled controller, only feedforward values + * are returned. + * + * @param enabled If the controller is enabled or not. + */ + void SetEnabled(bool enabled); + + private: + Pose2d m_poseError; + Pose2d m_poseTolerance; + bool m_enabled = true; + + frc2::PIDController m_xController; + frc2::PIDController m_yController; + ProfiledPIDController m_thetaController; +}; +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java new file mode 100644 index 0000000000..4a98232484 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java @@ -0,0 +1,134 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; + +/** + * This holonomic drive controller can be used to follow trajectories using a + * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory following + * is a much simpler problem to solve compared to skid-steer style drivetrains because + * it is possible to individually control forward, sideways, and angular velocity. + * + *

The holonomic drive controller takes in one PID controller for each direction, forward + * and sideways, and one profiled PID controller for the angular direction. Because the + * heading dynamics are decoupled from translations, users can specify a custom heading + * that the drivetrain should point toward. This heading reference is profiled for smoothness. + */ +@SuppressWarnings("MemberName") +public class HolonomicDriveController { + private Pose2d m_poseError = new Pose2d(); + private Pose2d m_poseTolerance = new Pose2d(); + private boolean m_enabled = true; + + private final PIDController m_xController; + private final PIDController m_yController; + private final ProfiledPIDController m_thetaController; + + /** + * Constructs a holonomic drive controller. + * + * @param xController A PID Controller to respond to error in the field-relative x direction. + * @param yController A PID Controller to respond to error in the field-relative y direction. + * @param thetaController A profiled PID controller to respond to error in angle. + */ + @SuppressWarnings("ParameterName") + public HolonomicDriveController(PIDController xController, + PIDController yController, + ProfiledPIDController thetaController) { + m_xController = xController; + m_yController = yController; + m_thetaController = thetaController; + } + + /** + * Returns true if the pose error is within tolerance of the reference. + * + * @return True if the pose error is within tolerance of the reference. + */ + public boolean atReference() { + final var eTranslate = m_poseError.getTranslation(); + final var eRotate = m_poseError.getRotation(); + final var tolTranslate = m_poseTolerance.getTranslation(); + final var tolRotate = m_poseTolerance.getRotation(); + return Math.abs(eTranslate.getX()) < tolTranslate.getX() + && Math.abs(eTranslate.getY()) < tolTranslate.getY() + && Math.abs(eRotate.getRadians()) < tolRotate.getRadians(); + } + + /** + * Sets the pose error which is considered tolerance for use with atReference(). + * + * @param tolerance The pose error which is tolerable. + */ + public void setTolerance(Pose2d tolerance) { + m_poseTolerance = tolerance; + } + + /** + * Returns the next output of the holonomic drive controller. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRefMeters The linear velocity reference. + * @param angleRef The angular reference. + * @return The next output of the holonomic drive controller. + */ + @SuppressWarnings("LocalVariableName") + public ChassisSpeeds calculate(Pose2d currentPose, + Pose2d poseRef, + double linearVelocityRefMeters, + Rotation2d angleRef) { + // Calculate feedforward velocities (field-relative). + double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos(); + double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); + double thetaFF = m_thetaController.calculate(currentPose.getRotation().getRadians(), + angleRef.getRadians()); + + m_poseError = poseRef.relativeTo(currentPose); + + if (!m_enabled) { + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation()); + } + + // Calculate feedback velocities (based on position error). + double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX()); + double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY()); + + // Return next output. + return ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback, + yFF + yFeedback, thetaFF, currentPose.getRotation()); + } + + /** + * Returns the next output of the holonomic drive controller. + * + * @param currentPose The current pose. + * @param desiredState The desired trajectory state. + * @param angleRef The desired end-angle. + * @return The next output of the holonomic drive controller. + */ + public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState, + Rotation2d angleRef) { + return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, + angleRef); + } + + /** + * Enables and disables the controller for troubleshooting problems. When calculate() + * is called on a disabled controller, only feedforward values are returned. + * + * @param enabled If the controller is enabled or not. + */ + public void setEnabled(boolean enabled) { + m_enabled = enabled; + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java new file mode 100644 index 0000000000..2bf7900451 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import java.util.ArrayList; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Twist2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpiutil.math.MathUtil; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class HolonomicDriveControllerTest { + private static final double kTolerance = 1 / 12.0; + private static final double kAngularTolerance = Math.toRadians(2); + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + void testReachesReference() { + HolonomicDriveController controller = new HolonomicDriveController( + new PIDController(1.0, 0.0, 0.0), + new PIDController(1.0, 0.0, 0.0), + new ProfiledPIDController(1.0, 0.0, 0.0, + new TrapezoidProfile.Constraints(6.28, 3.14)) + ); + Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0)); + + List waypoints = new ArrayList<>(); + waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0))); + waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8))); + + TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0); + Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); + + final double kDt = 0.02; + final double kTotalTime = trajectory.getTotalTimeSeconds(); + + for (int i = 0; i < (kTotalTime / kDt); i++) { + Trajectory.State state = trajectory.sample(kDt * i); + ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d()); + + robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt, + output.vyMetersPerSecond * kDt, output.omegaRadiansPerSecond * kDt)); + } + + final List states = trajectory.getStates(); + final Pose2d endPose = states.get(states.size() - 1).poseMeters; + + // Java lambdas require local variables referenced from a lambda expression + // must be final or effectively final. + final Pose2d finalRobotPose = robotPose; + + assertAll( + () -> assertEquals(endPose.getX(), finalRobotPose.getX(), + kTolerance), + () -> assertEquals(endPose.getY(), finalRobotPose.getY(), + kTolerance), + () -> assertEquals(0.0, + MathUtil.normalizeAngle(finalRobotPose.getRotation().getRadians()), + kAngularTolerance) + ); + } +}