SwerveDriveKinematics.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/swerve/utility/Kinematics.hpp"
#include <memory>
#include <span>
#include <vector>
 
namespace ctre {
namespace phoenix6 {
namespace swerve {
namespace impl {
 
/**
 * \brief Class that converts a chassis velocity (dx, dy, and dtheta components)
 * into individual module states (velocity and angle).
 *
 * The inverse kinematics (converting from a desired chassis velocity to
 * individual module states) uses the relative locations of the modules with
 * respect to the center of rotation. The center of rotation for inverse
 * kinematics is also variable. This means that you can set your set your center
 * of rotation in a corner of the robot to perform special evasion maneuvers.
 *
 * Forward kinematics (converting an array of module states into the overall
 * chassis motion) is performs the exact opposite of what inverse kinematics
 * does. Since this is an overdetermined system (more equations than variables),
 * we use a least-squares approximation.
 *
 * The inverse kinematics: [moduleVelocities] = [moduleLocations] * [chassisVelocities]
 * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
 * multiply by [moduleVelocities] to get our chassis velocities.
 *
 * Forward kinematics is also used for odometry -- determining the position of
 * the robot on the field using encoders and a gyro.
 */
class SwerveDriveKinematics {
    std::vector<Translation2d> m_modules;
 
    struct KinematicsMatrices;
    std::unique_ptr<KinematicsMatrices> m_matrices;
 
    mutable std::vector<Rotation2d> m_moduleHeadings;
    mutable Translation2d m_previousCoR{};
 
public:
    using WheelPositions = std::vector<SwerveModulePosition>;
    using WheelVelocities = std::vector<SwerveModuleVelocity>;
    using WheelAccelerations = std::vector<SwerveModuleAcceleration>;
 
    /**
     * \brief Constructs a swerve drive kinematics object. This takes in a variable
     * number of module locations as Translation2ds. The order in which you pass
     * in the module locations is the same order that you will receive the module
     * states when performing inverse kinematics. It is also expected that you
     * pass in the module states in the same order when calling the forward
     * kinematics methods.
     *
     * \param moduleLocations The locations of the modules relative to the
     *                        physical center of the robot.
     */
    SwerveDriveKinematics(std::vector<Translation2d> moduleLocations);
 
    SwerveDriveKinematics(SwerveDriveKinematics const &);
    SwerveDriveKinematics(SwerveDriveKinematics &&);
    ~SwerveDriveKinematics();
 
    SwerveDriveKinematics &operator=(SwerveDriveKinematics const &);
    SwerveDriveKinematics &operator=(SwerveDriveKinematics &&);
 
    /**
     * \brief Reset the internal swerve module headings.
     *
     * \param moduleHeadings The swerve module headings. The order of the module
     * headings should be same as passed into the constructor of this class.
     */
    void ResetHeadings(std::span<Rotation2d const> moduleHeadings)
    {
        for (size_t i = 0; i < m_modules.size() && i < moduleHeadings.size(); ++i) {
            m_moduleHeadings[i] = moduleHeadings[i];
        }
    }
 
    /**
     * \brief Performs inverse kinematics to return the module states from a desired
     * chassis velocity. This method is often used to convert joystick values into
     * module velocities and angles.
     *
     * This function also supports variable centers of rotation. During normal
     * operations, the center of rotation is usually the same as the physical
     * center of the robot; therefore, the argument is defaulted to that use case.
     * However, if you wish to change the center of rotation for evasive
     * maneuvers, vision alignment, or for any other use case, you can do so.
     *
     * In the case that the desired chassis velocities are zero (i.e. the robot
     * will be stationary), the previously calculated module angle will be
     * maintained.
     *
     * \param chassisVelocities The desired chassis velocity.
     * \param centerOfRotation The center of rotation. For example, if you set the
     * center of rotation at one corner of the robot and provide a chassis velocity
     * that only has a dtheta component, the robot will rotate around that corner.
     *
     * \returns A vector containing the module velocities. Use caution because these
     * module velocities are not normalized. Sometimes, a user input may cause one of
     * the module velocities to go above the attainable max velocity. Use the
     * DesaturateWheelVelocities(WheelVelocities const &, wpi::units::meters_per_second_t)
     * function to rectify this issue.
     */
    WheelVelocities ToSwerveModuleVelocities(ChassisVelocities const &chassisVelocities, Translation2d const &centerOfRotation = Translation2d{}) const;
 
    /**
     * \brief Performs inverse kinematics to return the wheel velocities from a desired
     * chassis velocity. This method is often used to convert joystick values into
     * wheel velocities.
     *
     * \param chassisVelocities The desired chassis velocity.
     * \returns The wheel velocities.
     */
    WheelVelocities ToWheelVelocities(ChassisVelocities const &chassisVelocities) const
    {
        return ToSwerveModuleVelocities(chassisVelocities);
    }
 
    /**
     * \brief Performs forward kinematics to return the resulting chassis state from
     * the given module states. This method is often used for odometry -- determining
     * the robot's position on the field using data from the real-world velocity
     * and angle of each module on the robot.
     *
     * \param moduleVelocities The state of the modules as a std::vector of type
     * SwerveModuleVelocity, as measured from respective  encoders and gyros. The
     * order of the swerve module states should be same as passed into the
     * constructor of this class.
     *
     * \returns The resulting chassis velocity.
     */
    ChassisVelocities ToChassisVelocities(WheelVelocities const &moduleVelocities) const;
 
    /**
     * \brief Performs inverse kinematics to return the module accelerations from a
     * desired chassis acceleration. This method is often used for dynamics
     * calculations -- converting desired robot accelerations into individual
     * module accelerations.
     *
     * This function also supports variable centers of rotation. During normal
     * operations, the center of rotation is usually the same as the physical
     * center of the robot; therefore, the argument is defaulted to that use case.
     * However, if you wish to change the center of rotation for evasive
     * maneuvers, vision alignment, or for any other use case, you can do so.
     *
     * \param chassisAccelerations The desired chassis accelerations.
     * \param angularVelocity The desired robot angular velocity.
     * \param centerOfRotation The center of rotation. For example, if you set the
     *     center of rotation at one corner of the robot and provide a chassis
     *     acceleration that only has a dtheta component, the robot will rotate
     *     around that corner.
     * \returns A vector containing the module accelerations.
     */
    WheelAccelerations ToSwerveModuleAccelerations(
        ChassisAccelerations const &chassisAccelerations,
        wpi::units::radians_per_second_t const angularVelocity = 0.0_rad_per_s,
        Translation2d const &centerOfRotation = Translation2d{}
    ) const;
 
    /**
     * \brief Performs inverse kinematics to return the wheel accelerations from a
     * desired chassis acceleration. This method is often used for dynamics
     * calculations -- converting desired robot accelerations into individual
     * wheel accelerations.
     *
     * \param chassisAccelerations The desired chassis accelerations.
     * \returns The wheel accelerations.
     */
    WheelAccelerations ToWheelAccelerations(ChassisAccelerations const &chassisAccelerations) const
    {
        return ToSwerveModuleAccelerations(chassisAccelerations);
    }
 
    /**
     * \brief Performs forward kinematics to return the resulting chassis accelerations
     * from the given module accelerations. This method is often used for dynamics
     * calculations -- determining the robot's acceleration on the field using
     * data from the real-world acceleration of each module on the robot.
     *
     * \param moduleAccelerations The accelerations of the modules as measured
     * from respective encoders and gyros. The order of the swerve module
     * accelerations should be same as passed into the constructor of this class.
     * \returns The resulting chassis accelerations.
     */
    ChassisAccelerations ToChassisAccelerations(WheelAccelerations const &moduleAccelerations) const;
 
    /**
     * \brief Performs forward kinematics to return the resulting Twist2d from the
     * given module position deltas. This method is often used for odometry --
     * determining the robot's position on the field using data from the
     * real-world position delta and angle of each module on the robot.
     *
     * \param moduleDeltas The latest change in position of the modules (as a
     * SwerveModulePosition type) as measured from respective encoders and gyros.
     * The order of the swerve module states should be same as passed into the
     * constructor of this class.
     *
     * \returns The resulting Twist2d.
     */
    Twist2d ToTwist2d(WheelPositions const &moduleDeltas) const;
 
    /**
     * \brief Performs forward kinematics to return the resulting Twist2d from the given
     * change in wheel positions. This method is often used for odometry --
     * determining the robot's position on the field using changes in the distance
     * driven by each wheel on the robot.
     *
     * \param start The starting distances driven by the wheels.
     * \param end The ending distances driven by the wheels.
     *
     * \returns The resulting Twist2d in the robot's movement.
     */
    Twist2d ToTwist2d(WheelPositions const &start, WheelPositions const &end) const
    {
        WheelPositions result(m_modules.size());
        for (size_t i = 0; i < m_modules.size() && i < std::min(start.size(), end.size()); ++i) {
            result[i] = {end[i].distance - start[i].distance, end[i].angle};
        }
        return ToTwist2d(result);
    }
 
    /**
     * \brief Performs interpolation between two values.
     *
     * \param start The value to start at.
     * \param end The value to end at.
     * \param t How far between the two values to interpolate. This should be
     * bounded to [0, 1].
     * \returns The interpolated value.
     */
    WheelPositions Interpolate(WheelPositions const &start, WheelPositions const &end, double t) const
    {
        WheelPositions result(m_modules.size());
        for (size_t i = 0; i < m_modules.size() && i < std::min(start.size(), end.size()); ++i) {
            result[i] = start[i].Interpolate(end[i], t);
        }
        return result;
    }
 
    std::vector<Translation2d> const &GetModules() const { return m_modules; }
 
    /**
     * \brief Renormalizes the wheel velocities if any individual velocity is above the
     * specified maximum.
     *
     * Sometimes, after inverse kinematics, the requested velocity
     * from one or more modules may be above the max attainable velocity for the
     * driving motor on that module. To fix this issue, one can reduce all the
     * wheel velocities to make sure that all requested module speeds are
     * at-or-below the absolute threshold, while maintaining the ratio of
     * velocities between modules.
     *
     * \param moduleVelocities The vector of module velocities.
     * \param attainableMaxVelocity The absolute max velocity that a module can reach.
     * \returns The vector of desaturated module velocities.
     */
    [[nodiscard]]
    static WheelVelocities DesaturateWheelVelocities(WheelVelocities const &moduleVelocities, wpi::units::meters_per_second_t attainableMaxVelocity);
 
    /**
     * \brief Renormalizes the wheel velocities if any individual velocity is above the
     * specified maximum, as well as getting rid of joystick saturation at edges of
     * joystick.
     *
     * Sometimes, after inverse kinematics, the requested velocity
     * from one or more modules may be above the max attainable velocity for the
     * driving motor on that module. To fix this issue, one can reduce all the
     * wheel velocities to make sure that all requested module velocities are
     * at-or-below the absolute threshold, while maintaining the ratio of
     * velocities between modules.
     *
     * Scaling down the module velocities rotates the direction of net motion in
     * the opposite direction of rotational velocity, which makes discretizing the
     * chassis velocities inaccurate because the discretization did not account
     * for this translational skew.
     *
     * \param moduleVelocities The vector of module velocities.
     * \param desiredChassisVelocity The desired velocity of the robot
     * \param attainableMaxModuleVelocity The absolute max velocity a module can reach
     * \param attainableMaxRobotTranslationVelocity The absolute max velocity the
     * robot can reach while translating
     * \param attainableMaxRobotRotationVelocity The absolute max velocity the
     * robot can reach while rotating
     * \returns The vector of desaturated module velocities
     */
    static WheelVelocities DesaturateWheelVelocities(
        WheelVelocities const &moduleVelocities,
        ChassisVelocities const &desiredChassisVelocity,
        wpi::units::meters_per_second_t attainableMaxModuleVelocity,
        wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
        wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity
    );
 
private:
    void SetInverseKinematics(Translation2d const &centerOfRotation) const;
};
 
}
}
}
}