kinematics.hpp

Go to the documentation of this file.

#pragma once
#include <array>
#include <cstddef>
#include <limits>
#include <vector>
 
#include "foundation/types.hpp"
#include "hal/IMotor.hpp"
 
namespace libstp::kinematics
{
    struct MotorCommands
    {
        std::vector<double> wheel_velocities{};
        bool saturated_any{false};
        std::uint32_t saturation_mask{0};
    };

    struct IKinematics
    {
        virtual ~IKinematics() = default;

        [[nodiscard]] virtual std::size_t wheelCount() const = 0;

        [[nodiscard]] virtual double maxYawRateForArcRadius(double /*radius_m*/,
                                                            bool /*lateral*/) const
        {
            return std::numeric_limits<double>::infinity();
        }

        virtual MotorCommands applyCommand(const foundation::ChassisVelocity& cmd, double dt) = 0;

        [[nodiscard]] virtual foundation::ChassisVelocity estimateState() const = 0;

        virtual void hardStop() = 0;

        [[nodiscard]] virtual bool supportsLateralMotion() const = 0;

        virtual void resetEncoders() = 0;

        [[nodiscard]] virtual double getWheelRadius() const = 0;

        [[nodiscard]] virtual std::vector<hal::motor::IMotor*> getMotors() = 0;

        struct StmOdometryConfig
        {
            std::array<std::array<float, 4>, 3> inv_matrix{};
            std::array<float, 4> ticks_to_rad{};
            std::array<std::array<float, 3>, 4> fwd_matrix{};
            std::array<float, 4> bemf_offset{};
        };
 
        // Non-const: needs to look up motor handles via getMotors() to
        // reorder kinematics-slot data into hardware-port order.
        [[nodiscard]] virtual StmOdometryConfig getStmOdometryConfig() = 0;

        virtual void setVelocityCommandGains(double /*gx*/, double /*gy*/, double /*gw*/) {}
        [[nodiscard]] virtual std::array<double, 3> getVelocityCommandGains() const
        {
            return {1.0, 1.0, 1.0};
        }

        virtual void applyPowerCommand(const foundation::ChassisVelocity& direction,
                                       int power_percent) = 0;
    };
}