Skip to content

Commit

Permalink
Merge pull request #540 from truher/main
Browse files Browse the repository at this point in the history 
remove second-order swerve control (drive accel and steer velocity)
  • Loading branch information
@truher
truher authored Nov 6, 2024
2 parents 27d0db9 + d2d122f commit 2de7aaa
Show file tree
Hide file tree
Showing 10 changed files with 14 additions and 526 deletions.
4 changes: 0 additions & 4 deletions lib/src/main/java/org/team100/lib/experiments/Experiment.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,6 @@ public enum Experiment {
* Use softer vision update gains
*/
AvoidVisionJitter,
/**
*
*/
UseSecondDerivativeSwerve,
/**
* Filter rotational output to remove oscillation
*/
Expand Down
11 changes: 2 additions & 9 deletions lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveLocal.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,17 +226,10 @@ public void resetSetpoint(SwerveSetpoint setpoint) {

public void setChassisSpeedsNormally(ChassisSpeeds speeds, double gyroRateRad_S) {
// Informs SwerveDriveKinematics of the module states.
SwerveModuleStates states;
if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) {
states = m_swerveKinodynamics.toSwerveModuleStates(speeds, m_prevSetpoint.getChassisSpeeds(),
m_prevSetpoint.getModuleStates(), gyroRateRad_S);
} else {
states = m_swerveKinodynamics.toSwerveModuleStates(speeds, gyroRateRad_S);
// System.out.println(speeds);
m_log_chassis_speed.log(() -> speeds);
}
SwerveModuleStates states = m_swerveKinodynamics.toSwerveModuleStates(speeds, gyroRateRad_S);
setModuleStates(states);
m_prevSetpoint = new SwerveSetpoint(speeds, states);
m_log_chassis_speed.log(() -> speeds);
}

/** Updates visualization. */
Expand Down
132 changes: 1 addition & 131 deletions ...rc/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveDriveKinematics100.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,6 @@
/**
* Helper class that converts between chassis state and module state.
*
* Note: sometimes one wheel will slip more than the others; we should ignore it
* in the forward calculation. see https://github.com/Team100/all24/issues/348
*
* Note: forward kinematics is never more accurate than the gyro and we
* absolutely cannot operate without a functional gyro, so we should use the
* gyro instead. see https://github.com/Team100/all24/issues/350
Expand All @@ -25,7 +22,6 @@ public class SwerveDriveKinematics100 {
private static final double kEpsilon = 1e-6;
private final int m_numModules;
private final Translation2d[] m_moduleLocations;
private final SimpleMatrix[] m_mat = new SimpleMatrix[4];

/**
* this (2n x 3) matrix looks something like
Expand Down Expand Up @@ -107,15 +103,9 @@ public SwerveDriveKinematics100(Translation2d... moduleTranslationsM) {
checkModuleCount(moduleTranslationsM);
m_numModules = moduleTranslationsM.length;
m_moduleLocations = Arrays.copyOf(moduleTranslationsM, m_numModules);
for (int i = 0; i < m_moduleLocations.length; i++) {
m_mat[i] = new SimpleMatrix(2, 3);
m_mat[i].setRow(0, 0, 1, 0, -m_moduleLocations[i].getY());
m_mat[i].setRow(1, 0, 0, 1, m_moduleLocations[i].getX());
}
m_inverseKinematics = inverseMatrix(m_moduleLocations);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
// try to avoid startup transient
// m_moduleHeadings = zeros(m_numModules);
// nulls avoid startup transient
m_moduleHeadings = nulls();
}

Expand Down Expand Up @@ -145,46 +135,6 @@ public SwerveModuleStates toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
return states;
}

public SwerveModuleStates toSwerveModuleStates(
ChassisSpeeds chassisSpeeds,
ChassisSpeeds chassisSpeedsAcceleration) {
if (fullStop(chassisSpeeds)) {
return constantModuleHeadings(); // avoid steering when stopped
}
// [vx; vy; omega] (3 x 1)
SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds);
SimpleMatrix chassisSpeedsAccelerationVector = chassisSpeeds2Vector(chassisSpeedsAcceleration);
// [v cos; v sin; ...] (2n x 1)
SwerveModuleStates prevStates = new SwerveModuleStates(
new SwerveModuleState100(0, Optional.of(new Rotation2d())),
new SwerveModuleState100(0, Optional.of(new Rotation2d())),
new SwerveModuleState100(0, Optional.of(new Rotation2d())),
new SwerveModuleState100(0, Optional.of(new Rotation2d())));
SwerveModuleStates states = accelerationFromVector(chassisSpeedsVector, chassisSpeedsAccelerationVector,
prevStates);
updateHeadings(states);
return states;
}

public SwerveModuleStates toSwerveModuleStates(
ChassisSpeeds chassisSpeeds,
ChassisSpeeds chassisSpeedsAcceleration,
SwerveModuleStates prevStates) {
if (fullStop(chassisSpeeds)) {
return constantModuleHeadings(); // avoid steering when stopped
}
// [vx; vy; omega] (3 x 1)
SimpleMatrix chassisSpeedsVector = chassisSpeeds2Vector(chassisSpeeds);
SimpleMatrix chassisSpeedsAccelerationVector = chassisSpeeds2Vector(chassisSpeedsAcceleration);
// [v cos; v sin; ...] (2n x 1)
SwerveModuleStates states = accelerationFromVector(
chassisSpeedsVector,
chassisSpeedsAccelerationVector,
prevStates);
updateHeadings(states);
return states;
}

/**
* INVERSE: twist -> module position deltas
*
Expand Down Expand Up @@ -439,83 +389,10 @@ private SwerveModuleState100 stateFromVector(double x, double y) {
}
}

/**
* // TODO: test this and keep it or toss it
*
* https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf
*/
public SwerveModuleStates accelerationFromVector(
SimpleMatrix chassisSpeedsMatrix,
SimpleMatrix chassisSpeedsAccelerationMatrix,
SwerveModuleStates prevStates) {
return new SwerveModuleStates(
oneAccel(0, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix),
oneAccel(1, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix),
oneAccel(2, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix),
oneAccel(3, chassisSpeedsMatrix, chassisSpeedsAccelerationMatrix));
}

private SwerveModuleState100 oneAccel(
int moduleLocation,
SimpleMatrix chassisSpeedsMatrix,
SimpleMatrix chassisSpeedsAccelerationMatrix) {

SimpleMatrix dmodulexy = m_mat[moduleLocation].mult(chassisSpeedsMatrix);
double vx = dmodulexy.get(0, 0);
double vy = dmodulexy.get(1, 0);
double speed = Math.hypot(vx, vy);
Rotation2d angle;
if (speed <= 1e-6) {
// avoid the garbage rotation, extrapolate the angle using omega
// double dtheta = prevStates[i].omega * dt;
// angle = new Rotation2d(MathUtil.angleModulus(
// angle2.get().getRadians() + dtheta));
// actually we really have no idea what the current state should be
// TODO: confirm this is OK
return new SwerveModuleState100(0, Optional.empty());
} else {
angle = new Rotation2d(vx, vy);
}
SimpleMatrix multiplier = new SimpleMatrix(2, 2);
multiplier.setRow(0, 0, angle.getCos(), angle.getSin());
multiplier.setRow(1, 0, -1.0 * angle.getSin(), angle.getCos());
SimpleMatrix moduleAccelerationXY = getModuleAccelerationXY(
moduleLocation,
chassisSpeedsAccelerationMatrix);
SimpleMatrix moduleAccelMat = multiplier.mult(moduleAccelerationXY);
if (speed != 0) {
moduleAccelMat.set(1, 0, (moduleAccelMat.get(1, 0) / speed));
} else {
// TODO: what is this 100000?
moduleAccelMat.set(1, 0, moduleAccelMat.get(1, 0) * 100000);
}
double accelMetersPerSecond_2 = moduleAccelMat.get(0, 0);
double omega = moduleAccelMat.get(1, 0);
return new SwerveModuleState100(
speed,
Optional.of(angle),
accelMetersPerSecond_2,
omega);
}

public Translation2d[] getModuleLocations() {
return m_moduleLocations;
}

/**
* Outputs a 2x1 matrix of acceleration of the module in x and y
*/
public SimpleMatrix getModuleAccelerationXY(
int moduleLocation,
SimpleMatrix chassisSpeedsAccelerationMatrix) {
SimpleMatrix acceleration2vector = new SimpleMatrix(3, 1);
acceleration2vector.setColumn(0, 0,
chassisSpeedsAccelerationMatrix.get(0, 0),
chassisSpeedsAccelerationMatrix.get(1, 0),
chassisSpeedsAccelerationMatrix.get(2, 0));
return m_mat[moduleLocation].mult(acceleration2vector);
}

/**
* The resulting distance is always positive.
*
Expand Down Expand Up @@ -570,11 +447,4 @@ private void checkModuleCount(Translation2d... moduleTranslationsM) {
throw new IllegalArgumentException("Swerve requires at least two modules");
}
}

private void checkLength(Object[] objs) {
int ct = objs.length;
if (ct != m_numModules) {
throw new IllegalArgumentException("Wrong module count: " + ct);
}
}
}
50 changes: 0 additions & 50 deletions lib/src/main/java/org/team100/lib/motion/drivetrain/kinodynamics/SwerveKinodynamics.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -318,56 +318,6 @@ SwerveModuleStates toSwerveModuleStates(ChassisSpeeds in, double gyroRateRad_S,
return m_kinematics.toSwerveModuleStates(descretized);
}

/**
* Inverse kinematics, chassis speeds => module states.
*
* The resulting state speeds are always positive.
*
* This version does **DISCRETIZATION** to correct for swerve veering.
*
* It also does extra veering correction proportional to rotation rate and
* translational acceleration.
*
* @param in chassis speeds to transform
* @param gyroRateRad_S current gyro rate, or the trajectory gyro rate
* @param accelM_S magnitude of acceleration
*/
public SwerveModuleStates toSwerveModuleStates(
ChassisSpeeds in,
ChassisSpeeds prevIn,
SwerveModuleStates prevSwerveModuleState,
double gyroRateRad_S) {
// This is the extra correction angle ...
Rotation2d angle = new Rotation2d(VeeringCorrection.correctionRad(gyroRateRad_S));
// ... which is subtracted here; this isn't really a field-relative
// transformation it's just a rotation.
ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
in.vxMetersPerSecond,
in.vyMetersPerSecond,
in.omegaRadiansPerSecond,
angle);
ChassisSpeeds descretized = ChassisSpeeds.discretize(chassisSpeeds, TimedRobot100.LOOP_PERIOD_S);

ChassisSpeeds prevChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
prevIn.vxMetersPerSecond,
prevIn.vyMetersPerSecond,
prevIn.omegaRadiansPerSecond,
angle);
ChassisSpeeds prevDescretized = ChassisSpeeds.discretize(prevChassisSpeeds, TimedRobot100.LOOP_PERIOD_S);

ChassisSpeeds acceleration = (chassisSpeeds.minus(prevDescretized)).div(TimedRobot100.LOOP_PERIOD_S);

return m_kinematics.toSwerveModuleStates(descretized, acceleration, prevSwerveModuleState);
}

/**
* The resulting state speeds are always positive.
*/
public SwerveModuleStates toSwerveModuleStatesWithoutDiscretization(ChassisSpeeds speeds,
ChassisSpeeds prevChassisSpeeds, SwerveModuleStates prevModuleStates) {
return m_kinematics.toSwerveModuleStates(speeds, prevChassisSpeeds, prevModuleStates);
}

/**
* The resulting state speeds are always positive.
*/
Expand Down
13 changes: 3 additions & 10 deletions lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModule100.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@
import java.util.OptionalDouble;

import org.team100.lib.dashboard.Glassy;
import org.team100.lib.experiments.Experiment;
import org.team100.lib.experiments.Experiments;
import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100;
import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
import org.team100.lib.motion.servo.AngularPositionServo;
Expand Down Expand Up @@ -36,7 +34,7 @@ protected SwerveModule100(
void setDesiredState(SwerveModuleState100 desiredState) {
OptionalDouble position = m_turningServo.getPosition();

if(position.isPresent()){
if (position.isPresent()) {
previousPosition = new Rotation2d(position.getAsDouble());
}

Expand All @@ -63,13 +61,8 @@ void setRawDesiredState(SwerveModuleState100 state) {
return;
// throw new IllegalArgumentException();
}
if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) {
m_driveServo.setVelocity(state.speedMetersPerSecond, state.accelMetersPerSecond_2);
m_turningServo.setPositionWithVelocity(state.angle.get().getRadians(), state.omega, 0);
} else {
m_driveServo.setVelocityM_S(state.speedMetersPerSecond);
m_turningServo.setPosition(state.angle.get().getRadians(), 0);
}
m_driveServo.setVelocityM_S(state.speedMetersPerSecond);
m_turningServo.setPosition(state.angle.get().getRadians(), 0);
}

/** For testing */
Expand Down
43 changes: 5 additions & 38 deletions lib/src/main/java/org/team100/lib/swerve/AsymSwerveSetpointGenerator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@
import java.util.function.DoubleSupplier;

import org.team100.lib.dashboard.Glassy;
import org.team100.lib.experiments.Experiment;
import org.team100.lib.experiments.Experiments;
import org.team100.lib.framework.TimedRobot100;
import org.team100.lib.geometry.GeometryUtil;
import org.team100.lib.logging.LoggerFactory;
Expand Down Expand Up @@ -72,14 +70,8 @@ public SwerveSetpoint generateSetpoint(
ChassisSpeeds desiredState) {
SwerveModuleStates prevModuleStates = prevSetpoint.getModuleStates();
// the desired module state speeds are always positive.
SwerveModuleStates desiredModuleStates;
if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) {
desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization(
desiredState, prevSetpoint.getChassisSpeeds(), prevModuleStates);
} else {
desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization(
desiredState);
}
SwerveModuleStates desiredModuleStates = m_limits.toSwerveModuleStatesWithoutDiscretization(
desiredState);
desiredState = desaturate(desiredState, desiredModuleStates);
boolean desiredIsStopped = SwerveUtil.desiredIsStopped(desiredState, desiredModuleStates, prevModuleStates);

Expand All @@ -93,7 +85,6 @@ public SwerveSetpoint generateSetpoint(
double[] desired_vy = computeVy(desiredModuleStates);
// elements may be null.
Rotation2d[] desired_heading = computeHeading(desiredModuleStates);
double[] desired_heading_velocity = computeHeadingVelocity(desiredModuleStates);

boolean shouldStopAndReverse = shouldStopAndReverse(prev_heading, desired_heading);
if (shouldStopAndReverse
Expand Down Expand Up @@ -150,7 +141,6 @@ public SwerveSetpoint generateSetpoint(
desired_vx,
desired_vy,
desired_heading,
desired_heading_velocity,
overrideSteering);
min_s = Math.min(min_s, steering_min_s);
}
Expand Down Expand Up @@ -227,18 +217,6 @@ private Rotation2d[] computeHeading(SwerveModuleStates states) {
return heading;
}

/**
* Which way each module is actually going, taking speed polarity into account.
*/
private double[] computeHeadingVelocity(SwerveModuleStates states) {
return new double[] {
states.frontLeft().omega,
states.frontRight().omega,
states.rearLeft().omega,
states.rearRight().omega,
};
}

/**
* If we want to go back the way we came, it might be faster to stop
* and then reverse. This is certainly true for near-180 degree turns, but
Expand Down Expand Up @@ -284,20 +262,10 @@ private SwerveSetpoint makeSetpoint(
dy,
dtheta,
min_s);
SwerveModuleStates setpointStates;
// the speeds in these states are always positive.
if (Experiments.instance.enabled(Experiment.UseSecondDerivativeSwerve)) {
setpointStates = m_limits.toSwerveModuleStates(
setpointSpeeds,
prevSetpoint.getChassisSpeeds(),
prevModuleStates,
setpointSpeeds.omegaRadiansPerSecond);
} else {
setpointStates = m_limits.toSwerveModuleStates(
setpointSpeeds,
setpointSpeeds.omegaRadiansPerSecond);
}

SwerveModuleStates setpointStates = m_limits.toSwerveModuleStates(
setpointSpeeds,
setpointSpeeds.omegaRadiansPerSecond);
applyOverrides(overrideSteering, setpointStates);
flipIfRequired(prevModuleStates, setpointStates);

Expand All @@ -322,7 +290,6 @@ private void applyOverrides(Rotation2d[] overrides, SwerveModuleStates states) {
}
}


private void flipIfRequired(SwerveModuleStates prevStates, SwerveModuleStates setpointStates) {
SwerveModuleState100[] prevStatesAll = prevStates.all();
SwerveModuleState100[] setpointStatesAll = setpointStates.all();
Expand Down
Loading

0 comments on commit 2de7aaa

Please sign in to comment.