euclidean_to_wheel.h

#pragma once
 
#include <Eigen/Dense>
 
#include "proto/primitive.pb.h"
#include "shared/robot_constants.h"
#include "software/geom/angular_velocity.h"
#include "software/geom/vector.h"
 
typedef Eigen::Vector3d EuclideanSpace_t;
 
typedef Eigen::Vector4d WheelSpace_t;
 
static constexpr int FRONT_RIGHT_WHEEL_SPACE_INDEX = 0;
static constexpr int FRONT_LEFT_WHEEL_SPACE_INDEX  = 1;
static constexpr int BACK_LEFT_WHEEL_SPACE_INDEX   = 2;
static constexpr int BACK_RIGHT_WHEEL_SPACE_INDEX  = 3;
 
class EuclideanToWheel
{
   public:
    EuclideanToWheel() = delete;
 
    explicit EuclideanToWheel(const RobotConstants_t& robot_constants);
 
    WheelSpace_t getWheelVelocity(EuclideanSpace_t euclidean_velocity) const;
 
    EuclideanSpace_t getEuclideanVelocity(const WheelSpace_t& wheel_velocity) const;
 
 
    WheelSpace_t rampWheelVelocity(const WheelSpace_t& current_wheel_velocity,
                                   const WheelSpace_t& target_wheel_velocity,
                                   const double& time_to_ramp);
 
   private:
    const double robot_radius_m_{};
    const RobotConstants_t robot_constants_;
 
    Eigen::Matrix<double, 4, 3> euclidean_to_wheel_velocity_D_;
 
    Eigen::Matrix<double, 3, 4> wheel_to_euclidean_velocity_D_inverse_;
};