mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpimath] Add LTV controllers (#4094)
This adds a unicycle controller that's a drop-in replacement for Ramsete and a differential drive controller that controls the full pose and outputs voltages. The main benefit is LQR-like tuning knobs using a system model.
This commit is contained in:
Tyler Veness
@@ -0,0 +1,91 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math;
|
||||
|
||||
import java.util.TreeMap;
|
||||
|
||||
/**
|
||||
* Interpolating Tree Maps are used to get values at points that are not defined by making a guess
|
||||
* from points that are defined. This uses linear interpolation.
|
||||
*/
|
||||
public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
|
||||
private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
|
||||
|
||||
/**
|
||||
* Inserts a key-value pair.
|
||||
*
|
||||
* @param key The key.
|
||||
* @param value The value.
|
||||
*/
|
||||
public void put(K key, Matrix<R, C> value) {
|
||||
m_map.put(key, value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the value associated with a given key.
|
||||
*
|
||||
* <p>If there's no matching key, the value returned will be a linear interpolation between the
|
||||
* keys before and after the provided one.
|
||||
*
|
||||
* @param key The key.
|
||||
* @return The value associated with the given key.
|
||||
*/
|
||||
public Matrix<R, C> get(K key) {
|
||||
Matrix<R, C> val = m_map.get(key);
|
||||
if (val == null) {
|
||||
K ceilingKey = m_map.ceilingKey(key);
|
||||
K floorKey = m_map.floorKey(key);
|
||||
|
||||
if (ceilingKey == null && floorKey == null) {
|
||||
return null;
|
||||
}
|
||||
if (ceilingKey == null) {
|
||||
return m_map.get(floorKey);
|
||||
}
|
||||
if (floorKey == null) {
|
||||
return m_map.get(ceilingKey);
|
||||
}
|
||||
Matrix<R, C> floor = m_map.get(floorKey);
|
||||
Matrix<R, C> ceiling = m_map.get(ceilingKey);
|
||||
|
||||
return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey));
|
||||
} else {
|
||||
return val;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the value interpolated between val1 and val2 by the interpolant d.
|
||||
*
|
||||
* @param val1 The lower part of the interpolation range.
|
||||
* @param val2 The upper part of the interpolation range.
|
||||
* @param d The interpolant in the range [0, 1].
|
||||
* @return The interpolated value.
|
||||
*/
|
||||
public Matrix<R, C> interpolate(Matrix<R, C> val1, Matrix<R, C> val2, double d) {
|
||||
var dydx = val2.minus(val1);
|
||||
return dydx.times(d).plus(val1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return where within interpolation range [0, 1] q is between down and up.
|
||||
*
|
||||
* @param up Upper part of interpolation range.
|
||||
* @param q Query.
|
||||
* @param down Lower part of interpolation range.
|
||||
* @return Interpolant in range [0, 1].
|
||||
*/
|
||||
public double inverseInterpolate(K up, K q, K down) {
|
||||
double upperToLower = up.doubleValue() - down.doubleValue();
|
||||
if (upperToLower <= 0) {
|
||||
return 0.0;
|
||||
}
|
||||
double queryToLower = q.doubleValue() - down.doubleValue();
|
||||
if (queryToLower <= 0) {
|
||||
return 0.0;
|
||||
}
|
||||
return queryToLower / upperToLower;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,289 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.InterpolatingMatrixTreeMap;
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.StateSpaceUtil;
|
||||
import edu.wpi.first.math.Vector;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
/**
|
||||
* The linear time-varying differential drive controller has a similar form to the LQR, but the
|
||||
* model used to compute the controller gain is the nonlinear model linearized around the
|
||||
* drivetrain's current state. We precomputed gains for important places in our state-space, then
|
||||
* interpolated between them with a LUT to save computational resources.
|
||||
*
|
||||
* <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
|
||||
* shown in theorem 8.7.4.
|
||||
*/
|
||||
public class LTVDifferentialDriveController {
|
||||
private final double m_trackwidth;
|
||||
|
||||
// LUT from drivetrain linear velocity to LQR gain
|
||||
private final InterpolatingMatrixTreeMap<Double, N2, N5> m_table =
|
||||
new InterpolatingMatrixTreeMap<>();
|
||||
|
||||
private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
|
||||
/** Motor voltages for a differential drive. */
|
||||
@SuppressWarnings("MemberName")
|
||||
public static class WheelVoltages {
|
||||
public double left;
|
||||
public double right;
|
||||
|
||||
public WheelVoltages() {}
|
||||
|
||||
public WheelVoltages(double left, double right) {
|
||||
this.left = left;
|
||||
this.right = right;
|
||||
}
|
||||
}
|
||||
|
||||
/** States of the drivetrain system. */
|
||||
enum State {
|
||||
kX(0),
|
||||
kY(1),
|
||||
kHeading(2),
|
||||
kLeftVelocity(3),
|
||||
kRightVelocity(4);
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final int value;
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
State(int i) {
|
||||
this.value = i;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying differential drive controller.
|
||||
*
|
||||
* @param plant The drivetrain velocity plant.
|
||||
* @param trackwidth The drivetrain's trackwidth in meters.
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
* @param relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep in seconds.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public LTVDifferentialDriveController(
|
||||
LinearSystem<N2, N2, N2> plant,
|
||||
double trackwidth,
|
||||
Vector<N5> qelems,
|
||||
Vector<N2> relems,
|
||||
double dt) {
|
||||
m_trackwidth = trackwidth;
|
||||
|
||||
var A =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N5())
|
||||
.fill(
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.5,
|
||||
0.5,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
-1.0 / m_trackwidth,
|
||||
1.0 / m_trackwidth,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getA(0, 0),
|
||||
plant.getA(0, 1),
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getA(1, 0),
|
||||
plant.getA(1, 1));
|
||||
var B =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N2())
|
||||
.fill(
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
plant.getB(0, 0),
|
||||
plant.getB(0, 1),
|
||||
plant.getB(1, 0),
|
||||
plant.getB(1, 1));
|
||||
var Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
var R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
|
||||
// dx/dt = Ax + Bu
|
||||
// 0 = Ax + Bu
|
||||
// Ax = -Bu
|
||||
// x = -A⁻¹Bu
|
||||
double maxV =
|
||||
plant
|
||||
.getA()
|
||||
.solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)))
|
||||
.times(-1.0)
|
||||
.get(0, 0);
|
||||
|
||||
var x = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
|
||||
x.set(State.kLeftVelocity.value, 0, velocity);
|
||||
x.set(State.kRightVelocity.value, 0, velocity);
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (Math.abs(velocity) < 1e-4) {
|
||||
m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5()));
|
||||
} else {
|
||||
A.set(State.kY.value, State.kHeading.value, velocity);
|
||||
m_table.put(velocity, new LinearQuadraticRegulator<N5, N2, N5>(A, B, Q, R, dt).getK());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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() {
|
||||
return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0)
|
||||
&& Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0)
|
||||
&& Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0)
|
||||
&& Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0)
|
||||
&& Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with atReference().
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
* @param leftVelocityTolerance Left velocity error which is tolerable in meters per second.
|
||||
* @param rightVelocityTolerance Right velocity error which is tolerable in meters per second.
|
||||
*/
|
||||
public void setTolerance(
|
||||
Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
|
||||
m_tolerance =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N1())
|
||||
.fill(
|
||||
poseTolerance.getX(),
|
||||
poseTolerance.getY(),
|
||||
poseTolerance.getRotation().getRadians(),
|
||||
leftVelocityTolerance,
|
||||
rightVelocityTolerance);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the left and right output voltages of the LTV controller.
|
||||
*
|
||||
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
|
||||
* trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param leftVelocity The current left velocity in meters per second.
|
||||
* @param rightVelocity The current right velocity in meters per second.
|
||||
* @param poseRef The desired pose.
|
||||
* @param leftVelocityRef The desired left velocity in meters per second.
|
||||
* @param rightVelocityRef The desired right velocity in meters per second.
|
||||
* @return Left and right output voltages of the LTV controller.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public WheelVoltages calculate(
|
||||
Pose2d currentPose,
|
||||
double leftVelocity,
|
||||
double rightVelocity,
|
||||
Pose2d poseRef,
|
||||
double leftVelocityRef,
|
||||
double rightVelocityRef) {
|
||||
// This implements the linear time-varying differential drive controller in
|
||||
// theorem 9.6.3 of https://tavsys.net/controls-in-frc.
|
||||
var x =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N1())
|
||||
.fill(
|
||||
currentPose.getX(),
|
||||
currentPose.getY(),
|
||||
currentPose.getRotation().getRadians(),
|
||||
leftVelocity,
|
||||
rightVelocity);
|
||||
|
||||
var inRobotFrame = Matrix.eye(Nat.N5());
|
||||
inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0)));
|
||||
inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
|
||||
|
||||
var r =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N1())
|
||||
.fill(
|
||||
poseRef.getX(),
|
||||
poseRef.getY(),
|
||||
poseRef.getRotation().getRadians(),
|
||||
leftVelocityRef,
|
||||
rightVelocityRef);
|
||||
m_error = r.minus(x);
|
||||
m_error.set(
|
||||
State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));
|
||||
|
||||
double velocity = (leftVelocity + rightVelocity) / 2.0;
|
||||
var K = m_table.get(velocity);
|
||||
|
||||
var u = K.times(inRobotFrame).times(m_error);
|
||||
|
||||
return new WheelVoltages(u.get(0, 0), u.get(1, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the left and right output voltages of the LTV controller.
|
||||
*
|
||||
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
|
||||
* trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param leftVelocity The left velocity in meters per second.
|
||||
* @param rightVelocity The right velocity in meters per second.
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
|
||||
* @return The left and right output voltages of the LTV controller.
|
||||
*/
|
||||
public WheelVoltages calculate(
|
||||
Pose2d currentPose,
|
||||
double leftVelocity,
|
||||
double rightVelocity,
|
||||
Trajectory.State desiredState) {
|
||||
// v = (v_r + v_l) / 2 (1)
|
||||
// w = (v_r - v_l) / (2r) (2)
|
||||
// k = w / v (3)
|
||||
//
|
||||
// v_l = v - wr
|
||||
// v_l = v - (vk)r
|
||||
// v_l = v(1 - kr)
|
||||
//
|
||||
// v_r = v + wr
|
||||
// v_r = v + (vk)r
|
||||
// v_r = v(1 + kr)
|
||||
return calculate(
|
||||
currentPose,
|
||||
leftVelocity,
|
||||
rightVelocity,
|
||||
desiredState.poseMeters,
|
||||
desiredState.velocityMetersPerSecond
|
||||
* (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)),
|
||||
desiredState.velocityMetersPerSecond
|
||||
* (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.StateSpaceUtil;
|
||||
import edu.wpi.first.math.Vector;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
/**
|
||||
* The linear time-varying unicycle controller has a similar form to the LQR, but the model used to
|
||||
* compute the controller gain is the nonlinear model linearized around the drivetrain's current
|
||||
* state.
|
||||
*
|
||||
* <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
|
||||
* shown in theorem 8.9.1.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class LTVUnicycleController {
|
||||
private final Matrix<N3, N3> m_A = new Matrix<>(Nat.N3(), Nat.N3());
|
||||
private final Matrix<N3, N2> m_B =
|
||||
new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
|
||||
private final Matrix<N3, N3> m_Q;
|
||||
private final Matrix<N2, N2> m_R;
|
||||
private final double m_dt;
|
||||
|
||||
private Pose2d m_poseError;
|
||||
private Pose2d m_poseTolerance;
|
||||
private boolean m_enabled = true;
|
||||
|
||||
/** States of the drivetrain system. */
|
||||
enum State {
|
||||
kX(0),
|
||||
kY(1),
|
||||
kHeading(2);
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final int value;
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
State(int i) {
|
||||
this.value = i;
|
||||
}
|
||||
}
|
||||
|
||||
/** Inputs of the drivetrain system. */
|
||||
enum Input {
|
||||
kLeftVelocity(3),
|
||||
kRightVelocity(4);
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final int value;
|
||||
|
||||
@SuppressWarnings("ParameterName")
|
||||
Input(int i) {
|
||||
this.value = i;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller.
|
||||
*
|
||||
* @param qelems The maximum desired error tolerance for each state.
|
||||
* @param relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep in seconds.
|
||||
*/
|
||||
public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
|
||||
m_dt = dt;
|
||||
m_Q = StateSpaceUtil.makeCostMatrix(qelems);
|
||||
m_R = StateSpaceUtil.makeCostMatrix(relems);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 tolerable for use with atReference().
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(Pose2d poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the linear and angular velocity outputs of the LTV controller.
|
||||
*
|
||||
* <p>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 in meters per second.
|
||||
* @param angularVelocityRef The desired angular velocity in radians per second.
|
||||
* @return The linear and angular velocity outputs of the LTV controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
|
||||
if (!m_enabled) {
|
||||
return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
|
||||
}
|
||||
|
||||
m_poseError = poseRef.relativeTo(currentPose);
|
||||
|
||||
if (Math.abs(linearVelocityRef) < 1e-4) {
|
||||
m_A.set(State.kY.value, State.kHeading.value, 1e-4);
|
||||
} else {
|
||||
m_A.set(State.kY.value, State.kHeading.value, linearVelocityRef);
|
||||
}
|
||||
var e =
|
||||
new MatBuilder<>(Nat.N3(), Nat.N1())
|
||||
.fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
|
||||
var u = new LinearQuadraticRegulator<N3, N2, N3>(m_A, m_B, m_Q, m_R, m_dt).getK().times(e);
|
||||
|
||||
return new ChassisSpeeds(
|
||||
linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the LTV controller.
|
||||
*
|
||||
* <p>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.
|
||||
* @return The linear and angular velocity outputs of the LTV controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
|
||||
return calculate(
|
||||
currentPose,
|
||||
desiredState.poseMeters,
|
||||
desiredState.velocityMetersPerSecond,
|
||||
desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
public void setEnabled(boolean enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/controller/LTVDifferentialDriveController.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/controller/LinearQuadraticRegulator.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
/**
|
||||
* States of the drivetrain system.
|
||||
*/
|
||||
class State {
|
||||
public:
|
||||
/// X position in global coordinate frame.
|
||||
static constexpr int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
static constexpr int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
static constexpr int kHeading = 2;
|
||||
|
||||
/// Left encoder velocity.
|
||||
static constexpr int kLeftVelocity = 3;
|
||||
|
||||
/// Right encoder velocity.
|
||||
static constexpr int kRightVelocity = 4;
|
||||
};
|
||||
|
||||
LTVDifferentialDriveController::LTVDifferentialDriveController(
|
||||
const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth,
|
||||
const wpi::array<double, 5>& Qelems, const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt)
|
||||
: m_trackwidth{trackwidth} {
|
||||
Matrixd<5, 5> A{
|
||||
{0.0, 0.0, 0.0, 0.5, 0.5},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()},
|
||||
{0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)},
|
||||
{0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}};
|
||||
Matrixd<5, 2> B{{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{0.0, 0.0},
|
||||
{plant.B(0, 0), plant.B(0, 1)},
|
||||
{plant.B(1, 0), plant.B(1, 1)}};
|
||||
Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems);
|
||||
Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
|
||||
|
||||
// dx/dt = Ax + Bu
|
||||
// 0 = Ax + Bu
|
||||
// Ax = -Bu
|
||||
// x = -A⁻¹Bu
|
||||
units::meters_per_second_t maxV{
|
||||
-plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
|
||||
|
||||
Vectord<5> x = Vectord<5>::Zero();
|
||||
for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
|
||||
x(State::kLeftVelocity) = velocity.value();
|
||||
x(State::kRightVelocity) = velocity.value();
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(velocity) < 1e-4_mps) {
|
||||
m_table.insert(velocity, Matrixd<2, 5>::Zero());
|
||||
} else {
|
||||
A(State::kY, State::kHeading) = velocity.value();
|
||||
m_table.insert(velocity,
|
||||
frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool LTVDifferentialDriveController::AtReference() const {
|
||||
return std::abs(m_error(0)) < m_tolerance(0) &&
|
||||
std::abs(m_error(1)) < m_tolerance(1) &&
|
||||
std::abs(m_error(2)) < m_tolerance(2) &&
|
||||
std::abs(m_error(3)) < m_tolerance(3) &&
|
||||
std::abs(m_error(4)) < m_tolerance(4);
|
||||
}
|
||||
|
||||
void LTVDifferentialDriveController::SetTolerance(
|
||||
const Pose2d& poseTolerance,
|
||||
units::meters_per_second_t leftVelocityTolerance,
|
||||
units::meters_per_second_t rightVelocityTolerance) {
|
||||
m_tolerance =
|
||||
Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(),
|
||||
poseTolerance.Rotation().Radians().value(),
|
||||
leftVelocityTolerance.value(), rightVelocityTolerance.value()};
|
||||
}
|
||||
|
||||
LTVDifferentialDriveController::WheelVoltages
|
||||
LTVDifferentialDriveController::Calculate(
|
||||
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
|
||||
units::meters_per_second_t leftVelocityRef,
|
||||
units::meters_per_second_t rightVelocityRef) {
|
||||
// This implements the linear time-varying differential drive controller in
|
||||
// theorem 9.6.3 of https://tavsys.net/controls-in-frc.
|
||||
Vectord<5> x{currentPose.X().value(), currentPose.Y().value(),
|
||||
currentPose.Rotation().Radians().value(), leftVelocity.value(),
|
||||
rightVelocity.value()};
|
||||
|
||||
Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity();
|
||||
inRobotFrame(0, 0) = std::cos(x(State::kHeading));
|
||||
inRobotFrame(0, 1) = std::sin(x(State::kHeading));
|
||||
inRobotFrame(1, 0) = -std::sin(x(State::kHeading));
|
||||
inRobotFrame(1, 1) = std::cos(x(State::kHeading));
|
||||
|
||||
Vectord<5> r{poseRef.X().value(), poseRef.Y().value(),
|
||||
poseRef.Rotation().Radians().value(), leftVelocityRef.value(),
|
||||
rightVelocityRef.value()};
|
||||
m_error = r - x;
|
||||
m_error(State::kHeading) =
|
||||
frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value();
|
||||
|
||||
units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
|
||||
const auto& K = m_table[velocity];
|
||||
|
||||
Vectord<2> u = K * inRobotFrame * m_error;
|
||||
|
||||
return WheelVoltages{units::volt_t{u(0)}, units::volt_t{u(1)}};
|
||||
}
|
||||
|
||||
LTVDifferentialDriveController::WheelVoltages
|
||||
LTVDifferentialDriveController::Calculate(
|
||||
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity,
|
||||
const Trajectory::State& desiredState) {
|
||||
// v = (v_r + v_l) / 2 (1)
|
||||
// w = (v_r - v_l) / (2r) (2)
|
||||
// k = w / v (3)
|
||||
//
|
||||
// v_l = v - wr
|
||||
// v_l = v - (vk)r
|
||||
// v_l = v(1 - kr)
|
||||
//
|
||||
// v_r = v + wr
|
||||
// v_r = v + (vk)r
|
||||
// v_r = v(1 + kr)
|
||||
return Calculate(
|
||||
currentPose, leftVelocity, rightVelocity, desiredState.pose,
|
||||
desiredState.velocity *
|
||||
(1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)),
|
||||
desiredState.velocity *
|
||||
(1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0)));
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/controller/LTVUnicycleController.h"
|
||||
|
||||
#include "frc/StateSpaceUtil.h"
|
||||
#include "frc/controller/LinearQuadraticRegulator.h"
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
/**
|
||||
* States of the drivetrain system.
|
||||
*/
|
||||
class State {
|
||||
public:
|
||||
/// X position in global coordinate frame.
|
||||
static constexpr int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
static constexpr int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
static constexpr int kHeading = 2;
|
||||
};
|
||||
|
||||
/**
|
||||
* Inputs of the drivetrain system.
|
||||
*/
|
||||
class Input {
|
||||
public:
|
||||
/// Linear velocity.
|
||||
static constexpr int kLinearVelocity = 3;
|
||||
|
||||
/// Angular velocity.
|
||||
static constexpr int kAngularVelocity = 4;
|
||||
};
|
||||
|
||||
LTVUnicycleController::LTVUnicycleController(
|
||||
const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt)
|
||||
: m_dt{dt} {
|
||||
m_Q = frc::MakeCostMatrix(Qelems);
|
||||
m_R = frc::MakeCostMatrix(Relems);
|
||||
}
|
||||
|
||||
bool LTVUnicycleController::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 LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef) {
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
|
||||
}
|
||||
|
||||
m_poseError = poseRef.RelativeTo(currentPose);
|
||||
|
||||
if (units::math::abs(linearVelocityRef) < 1e-4_mps) {
|
||||
m_A(State::kY, State::kHeading) = 1e-4;
|
||||
} else {
|
||||
m_A(State::kY, State::kHeading) = linearVelocityRef.value();
|
||||
}
|
||||
Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(),
|
||||
m_poseError.Rotation().Radians().value()};
|
||||
Vectord<2> u =
|
||||
frc::LinearQuadraticRegulator<3, 2>{m_A, m_B, m_Q, m_R, m_dt}.K() * e;
|
||||
|
||||
return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
|
||||
0_mps,
|
||||
angularVelocityRef + units::radians_per_second_t{u(1)}};
|
||||
}
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
const Pose2d& currentPose, const Trajectory::State& desiredState) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredState.velocity * desiredState.curvature);
|
||||
}
|
||||
|
||||
void LTVUnicycleController::SetEnabled(bool enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
@@ -0,0 +1,132 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/array.h>
|
||||
#include <wpi/interpolating_map.h>
|
||||
|
||||
#include "frc/EigenCore.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/system/LinearSystem.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/length.h"
|
||||
#include "units/time.h"
|
||||
#include "units/velocity.h"
|
||||
#include "units/voltage.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* The linear time-varying differential drive controller has a similar form to
|
||||
* the LQR, but the model used to compute the controller gain is the nonlinear
|
||||
* model linearized around the drivetrain's current state. We precomputed gains
|
||||
* for important places in our state-space, then interpolated between them with
|
||||
* a LUT to save computational resources.
|
||||
*
|
||||
* See section 8.7 in Controls Engineering in FRC for a derivation of the
|
||||
* control law we used shown in theorem 8.7.4.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT LTVDifferentialDriveController {
|
||||
public:
|
||||
/**
|
||||
* Motor voltages for a differential drive.
|
||||
*/
|
||||
struct WheelVoltages {
|
||||
units::volt_t left = 0_V;
|
||||
units::volt_t right = 0_V;
|
||||
};
|
||||
|
||||
/**
|
||||
* Constructs a linear time-varying differential drive controller.
|
||||
*
|
||||
* @param plant The drivetrain velocity plant.
|
||||
* @param trackwidth The drivetrain's trackwidth.
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
|
||||
units::meter_t trackwidth,
|
||||
const wpi::array<double, 5>& Qelems,
|
||||
const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
/**
|
||||
* Move constructor.
|
||||
*/
|
||||
LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
|
||||
|
||||
/**
|
||||
* Move assignment operator.
|
||||
*/
|
||||
LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
|
||||
default;
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* @param leftVelocityTolerance Left velocity error which is tolerable.
|
||||
* @param rightVelocityTolerance Right velocity error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(const Pose2d& poseTolerance,
|
||||
units::meters_per_second_t leftVelocityTolerance,
|
||||
units::meters_per_second_t rightVelocityTolerance);
|
||||
|
||||
/**
|
||||
* Returns the left and right output voltages of the LTV controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param leftVelocity The current left velocity.
|
||||
* @param rightVelocity The current right velocity.
|
||||
* @param poseRef The desired pose.
|
||||
* @param leftVelocityRef The desired left velocity.
|
||||
* @param rightVelocityRef The desired right velocity.
|
||||
*/
|
||||
WheelVoltages Calculate(const Pose2d& currentPose,
|
||||
units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity,
|
||||
const Pose2d& poseRef,
|
||||
units::meters_per_second_t leftVelocityRef,
|
||||
units::meters_per_second_t rightVelocityRef);
|
||||
|
||||
/**
|
||||
* Returns the left and right output voltages of the LTV controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param leftVelocity The left velocity.
|
||||
* @param rightVelocity The right velocity.
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity
|
||||
* from a trajectory.
|
||||
*/
|
||||
WheelVoltages Calculate(const Pose2d& currentPose,
|
||||
units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity,
|
||||
const Trajectory::State& desiredState);
|
||||
|
||||
private:
|
||||
units::meter_t m_trackwidth;
|
||||
|
||||
// LUT from drivetrain linear velocity to LQR gain
|
||||
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
|
||||
|
||||
Vectord<5> m_error;
|
||||
Vectord<5> m_tolerance;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,111 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
#include <wpi/array.h>
|
||||
|
||||
#include "frc/EigenCore.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/time.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* The linear time-varying unicycle controller has a similar form to the LQR,
|
||||
* but the model used to compute the controller gain is the nonlinear model
|
||||
* linearized around the drivetrain's current state.
|
||||
*
|
||||
* See section 8.9 in Controls Engineering in FRC for a derivation of the
|
||||
* control law we used shown in theorem 8.9.1.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT LTVUnicycleController {
|
||||
public:
|
||||
/**
|
||||
* Constructs a linear time-varying unicycle controller.
|
||||
*
|
||||
* @param Qelems The maximum desired error tolerance for each state.
|
||||
* @param Relems The maximum desired control effort for each input.
|
||||
* @param dt Discretization timestep.
|
||||
*/
|
||||
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
|
||||
const wpi::array<double, 2>& Relems,
|
||||
units::second_t dt);
|
||||
|
||||
/**
|
||||
* Move constructor.
|
||||
*/
|
||||
LTVUnicycleController(LTVUnicycleController&&) = default;
|
||||
|
||||
/**
|
||||
* Move assignment operator.
|
||||
*/
|
||||
LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
|
||||
|
||||
/**
|
||||
* 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& poseTolerance);
|
||||
|
||||
/**
|
||||
* Returns the linear and angular velocity outputs of the LTV 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 angularVelocityRef The desired angular velocity.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef);
|
||||
|
||||
/**
|
||||
* Returns the linear and angular velocity outputs of the LTV 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.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState);
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
void SetEnabled(bool enabled);
|
||||
|
||||
private:
|
||||
Matrixd<3, 3> m_A = Matrixd<3, 3>::Zero();
|
||||
Matrixd<3, 2> m_B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<3, 3> m_Q;
|
||||
Matrixd<2, 2> m_R;
|
||||
units::second_t m_dt;
|
||||
|
||||
Pose2d m_poseError;
|
||||
Pose2d m_poseTolerance;
|
||||
bool m_enabled = true;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,128 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
import edu.wpi.first.math.system.NumericalIntegration;
|
||||
import edu.wpi.first.math.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class LTVDifferentialDriveControllerTest {
|
||||
private static final double kTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = Math.toRadians(2);
|
||||
|
||||
/** States of the drivetrain system. */
|
||||
static class State {
|
||||
/// X position in global coordinate frame.
|
||||
public static final int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
public static final int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
public static final int kHeading = 2;
|
||||
|
||||
/// Left encoder velocity.
|
||||
public static final int kLeftVelocity = 3;
|
||||
|
||||
/// Right encoder velocity.
|
||||
public static final int kRightVelocity = 4;
|
||||
}
|
||||
|
||||
private static final double kLinearV = 3.02; // V/(m/s)
|
||||
private static final double kLinearA = 0.642; // V/(m/s²)
|
||||
private static final double kAngularV = 1.382; // V/(m/s)
|
||||
private static final double kAngularA = 0.08495; // V/(m/s²)
|
||||
private static final LinearSystem<N2, N2, N2> plant =
|
||||
LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
|
||||
private static final double kTrackwidth = 0.9;
|
||||
|
||||
private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0;
|
||||
|
||||
var xdot = new Matrix<>(Nat.N5(), Nat.N1());
|
||||
xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0)));
|
||||
xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0)));
|
||||
xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth);
|
||||
xdot.assignBlock(
|
||||
3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u)));
|
||||
return xdot;
|
||||
}
|
||||
|
||||
@Test
|
||||
void testReachesReference() {
|
||||
final double kDt = 0.02;
|
||||
|
||||
final var controller =
|
||||
new LTVDifferentialDriveController(
|
||||
plant,
|
||||
kTrackwidth,
|
||||
VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
|
||||
VecBuilder.fill(12.0, 12.0),
|
||||
kDt);
|
||||
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
|
||||
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
var x =
|
||||
new MatBuilder<>(Nat.N5(), Nat.N1())
|
||||
.fill(
|
||||
robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
|
||||
|
||||
final var totalTime = trajectory.getTotalTimeSeconds();
|
||||
for (int i = 0; i < (totalTime / kDt); ++i) {
|
||||
var state = trajectory.sample(kDt * i);
|
||||
robotPose =
|
||||
new Pose2d(
|
||||
x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0)));
|
||||
final var output =
|
||||
controller.calculate(
|
||||
robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state);
|
||||
|
||||
x =
|
||||
NumericalIntegration.rkdp(
|
||||
LTVDifferentialDriveControllerTest::dynamics,
|
||||
x,
|
||||
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
|
||||
kDt);
|
||||
}
|
||||
|
||||
final var states = trajectory.getStates();
|
||||
final var endPose = states.get(states.size() - 1).poseMeters;
|
||||
|
||||
// Java lambdas require local variables referenced from a lambda expression
|
||||
// must be final or effectively final.
|
||||
final var finalRobotPose = robotPose;
|
||||
assertAll(
|
||||
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
|
||||
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
0.0,
|
||||
MathUtil.angleModulus(
|
||||
endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Twist2d;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class LTVUnicycleControllerTest {
|
||||
private static final double kTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = Math.toRadians(2);
|
||||
|
||||
@Test
|
||||
void testReachesReference() {
|
||||
final double kDt = 0.02;
|
||||
|
||||
final var controller =
|
||||
new LTVUnicycleController(
|
||||
VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
|
||||
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
|
||||
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final var totalTime = trajectory.getTotalTimeSeconds();
|
||||
for (int i = 0; i < (totalTime / kDt); ++i) {
|
||||
var state = trajectory.sample(kDt * i);
|
||||
|
||||
var output = controller.calculate(robotPose, state);
|
||||
robotPose =
|
||||
robotPose.exp(
|
||||
new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
|
||||
}
|
||||
|
||||
final var states = trajectory.getStates();
|
||||
final var endPose = states.get(states.size() - 1).poseMeters;
|
||||
|
||||
// Java lambdas require local variables referenced from a lambda expression
|
||||
// must be final or effectively final.
|
||||
final var finalRobotPose = robotPose;
|
||||
assertAll(
|
||||
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
|
||||
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
0.0,
|
||||
MathUtil.angleModulus(
|
||||
endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,101 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/controller/LTVDifferentialDriveController.h"
|
||||
#include "frc/system/NumericalIntegration.h"
|
||||
#include "frc/system/plant/LinearSystemId.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/math.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
static constexpr units::meter_t kTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
|
||||
180.0};
|
||||
|
||||
/**
|
||||
* States of the drivetrain system.
|
||||
*/
|
||||
class State {
|
||||
public:
|
||||
/// X position in global coordinate frame.
|
||||
static constexpr int kX = 0;
|
||||
|
||||
/// Y position in global coordinate frame.
|
||||
static constexpr int kY = 1;
|
||||
|
||||
/// Heading in global coordinate frame.
|
||||
static constexpr int kHeading = 2;
|
||||
|
||||
/// Left encoder velocity.
|
||||
static constexpr int kLeftVelocity = 3;
|
||||
|
||||
/// Right encoder velocity.
|
||||
static constexpr int kRightVelocity = 4;
|
||||
};
|
||||
|
||||
static constexpr auto kLinearV = 3.02_V / 1_mps;
|
||||
static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
|
||||
static constexpr auto kAngularV = 1.382_V / 1_mps;
|
||||
static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
|
||||
static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
kLinearV, kLinearA, kAngularV, kAngularA);
|
||||
static constexpr auto kTrackwidth = 0.9_m;
|
||||
|
||||
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
|
||||
double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
|
||||
|
||||
frc::Vectord<5> xdot;
|
||||
xdot(0) = v * std::cos(x(State::kHeading));
|
||||
xdot(1) = v * std::sin(x(State::kHeading));
|
||||
xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth)
|
||||
.value();
|
||||
xdot.block<2, 1>(3, 0) = plant.A() * x.block<2, 1>(3, 0) + plant.B() * u;
|
||||
return xdot;
|
||||
}
|
||||
|
||||
TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
|
||||
constexpr auto kDt = 0.02_s;
|
||||
|
||||
frc::LTVDifferentialDriveController controller{
|
||||
plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
|
||||
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
|
||||
|
||||
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
|
||||
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
frc::Vectord<5> x = frc::Vectord<5>::Zero();
|
||||
x(State::kX) = robotPose.X().value();
|
||||
x(State::kY) = robotPose.Y().value();
|
||||
x(State::kHeading) = robotPose.Rotation().Radians().value();
|
||||
|
||||
auto totalTime = trajectory.TotalTime();
|
||||
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
|
||||
auto state = trajectory.Sample(kDt * i);
|
||||
robotPose =
|
||||
frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)},
|
||||
units::radian_t{x(State::kHeading)}};
|
||||
auto [leftVoltage, rightVoltage] = controller.Calculate(
|
||||
robotPose, units::meters_per_second_t{x(State::kLeftVelocity)},
|
||||
units::meters_per_second_t{x(State::kRightVelocity)}, state);
|
||||
|
||||
x = frc::RKDP(&Dynamics, x,
|
||||
frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()},
|
||||
kDt);
|
||||
}
|
||||
|
||||
auto& endPose = trajectory.States().back().pose;
|
||||
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
|
||||
robotPose.Rotation().Radians()),
|
||||
0_rad, kAngularTolerance);
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/controller/LTVUnicycleController.h"
|
||||
#include "frc/trajectory/TrajectoryGenerator.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/math.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
static constexpr units::meter_t kTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
|
||||
180.0};
|
||||
|
||||
TEST(LTVUnicycleControllerTest, ReachesReference) {
|
||||
constexpr auto kDt = 0.02_s;
|
||||
|
||||
frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
|
||||
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
|
||||
|
||||
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
|
||||
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
auto totalTime = trajectory.TotalTime();
|
||||
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
|
||||
auto state = trajectory.Sample(kDt * i);
|
||||
auto [vx, vy, omega] = controller.Calculate(robotPose, state);
|
||||
static_cast<void>(vy);
|
||||
|
||||
robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
|
||||
}
|
||||
|
||||
auto& endPose = trajectory.States().back().pose;
|
||||
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
|
||||
EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() -
|
||||
robotPose.Rotation().Radians()),
|
||||
0_rad, kAngularTolerance);
|
||||
}
|
||||
Reference in New Issue
Block a user