Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add HolonomicDriveController class and tests

Browse Source
This commit is contained in:
Prateek Machiraju
2020-07-12 21:03:25 -04:00
committed by Peter Johnson
parent af588adce5
commit 5ca2702083
4 changed files with 394 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp Normal file
View File

@@ -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 <units/angular_velocity.h>
using namespace frc;
HolonomicDriveController::HolonomicDriveController(
const frc2::PIDController& xController,
const frc2::PIDController& yController,
const ProfiledPIDController<units::radian>& 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<double>(), poseRef.X().to<double>()));
auto yFeedback = units::meters_per_second_t(m_yController.Calculate(
currentPose.Y().to<double>(), poseRef.Y().to<double>()));
// 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; }

112
wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h Normal file
View File

@@ -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 <units/angle.h>
#include <units/velocity.h>
#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<units::radian>& 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<units::radian> m_thetaController;
};
} // namespace frc

134
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java Normal file
View File

@@ -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.
*
* <p>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;
}
}

78
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java Normal file
View File

@@ -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<Pose2d> 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<Trajectory.State> 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)
);
}
}