mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-27 02:01:42 +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,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
|
||||
Reference in New Issue
Block a user