tank_kinematics_constraint.h

#pragma once
 
#include <cmath>
#include <memory>
 
#include "core/math/kinematics/tank_kinematics.h"
#include "core/math/trajectory/constraints/trajectory_constraint.h"
#include "core/math/units.h"
 
using namespace units::literals;

class TankKinematicsConstraint : public TrajectoryConstraint {
 public:
  TankKinematicsConstraint(units::Length track_width, units::Velocity max_speed,
                           units::Acceleration max_acceleration = 10000_inps2)
      : kinematics_(track_width), max_speed_(max_speed), max_acceleration_(max_acceleration) {}

  TankKinematicsConstraint(const TankKinematics& kinematics, units::Velocity max_speed,
                           units::Acceleration max_acceleration = 10000_inps2)
      : kinematics_(kinematics), max_speed_(max_speed), max_acceleration_(max_acceleration) {}

  units::Velocity max_velocity(const Pose2d& pose, units::Curvature curvature,
                               units::Velocity velocity) const override {
    units::AngularVelocity omega = velocity * curvature;
    ChassisVelocities chassis_speeds{velocity, 0_inps, omega};
 
    TankWheelVelocities wheel_speeds = kinematics_.to_wheel_velocities(chassis_speeds);
 
    units::Velocity real_max_speed =
        units::max(units::abs(wheel_speeds.left), units::abs(wheel_speeds.right));
 
    if (real_max_speed > max_speed_) {
      return velocity * (max_speed_ / real_max_speed);
    }
 
    return velocity;
  }

  MinMax min_max_acceleration(const Pose2d& pose, units::Curvature curvature,
                              units::Velocity speed) const override {
    // Linear acceleration a mapping to wheel accelerations:
    // alpha_kinematic = a * curvature
    // wheel_accels = kinematics_.to_wheel_accelerations({a, 0, alpha_kinematic})
    // Since it's a linear mapping: a_left = a * left_factor, a_right = a * right_factor
 
    double left_factor =
        1.0 - (curvature.to(units::radpm) * kinematics_.track_width.to(units::m)) / 2.0;
    double right_factor =
        1.0 + (curvature.to(units::radpm) * kinematics_.track_width.to(units::m)) / 2.0;
 
    units::Acceleration max_a = max_acceleration_;
    units::Acceleration min_a = -max_acceleration_;
 
    double factors[2] = {left_factor, right_factor};
    for (int i = 0; i < 2; ++i) {
      double factor = factors[i];
      if (std::abs(factor) > 1e-6) {
        units::Acceleration limit = max_acceleration_ / std::abs(factor);
        max_a = units::min(max_a, limit);
        min_a = units::max(min_a, -limit);
      }
    }
 
    return {min_a, max_a};
  }

  std::unique_ptr<TrajectoryConstraint> clone() const override {
    return std::unique_ptr<TrajectoryConstraint>(new TankKinematicsConstraint(*this));
  }
 
 private:
  TankKinematics kinematics_;             
  units::Velocity max_speed_;             
  units::Acceleration max_acceleration_;  
};