Source code for pyfrc.physics.tankmodel

"""
    .. versionadded:: 2018.4.0

    .. note:: The equations used in our :class:`TankModel` is derived from
              `Noah Gleason and Eli Barnett's motor characterization whitepaper
              <https://www.chiefdelphi.com/media/papers/3402>`_. It is
              recommended that users of this model read the paper so they can
              more fully understand how this works.
              
              In the interest of making progress, this API may receive
              backwards-incompatible changes before the start of the 2019
              FRC season.
"""

import math
import typing

from wpilib.geometry import Transform2d

from .motor_cfgs import MotorModelConfig
from .units import units, Helpers

import logging

logger = logging.getLogger("pyfrc.physics")

# default parameters for a kitbot
_bumper_length = 3.25 * units.inch

_kitbot_wheelbase = 21.0 * units.inch
_kitbot_width = _kitbot_wheelbase + _bumper_length * 2
_kitbot_length = 30.0 * units.inch + _bumper_length * 2

_inertia_units = (units.foot ** 2) * units.pound
_bm_units = units.foot * units.pound


[docs]class MotorModel: """ Motor model used by the :class:`TankModel`. You should not need to create this object if you're using the :class:`TankModel` class. """ @units.wraps(None, (None, None, "tm_kv", "tm_ka", "volts")) def __init__( self, motor_config: MotorModelConfig, kv: units.tm_kv, ka: units.tm_ka, vintercept: units.volts, ): """ :param motor_config: The specification data for your motor :param kv: Computed ``kv`` for your robot :param ka: Computed ``ka`` for your robot :param vintercept: The minimum voltage required to generate enough torque to overcome steady-state friction (see the paper for more details) """ #: Current computed acceleration (in ft/s^2) self.acceleration = 0 #: Current computed velocity (in ft/s) self.velocity = 0 #: Current computed position (in ft) self.position = 0 self._nominalVoltage = units.volts.m_from( motor_config.nominalVoltage, strict=False, name="motor_config.nominalVoltage", ) self._vintercept = vintercept self._kv = kv self._ka = ka
[docs] def compute(self, motor_pct: float, tm_diff: float) -> float: """ :param motor_pct: Percentage of power for motor in range [1..-1] :param tm_diff: Time elapsed since this function was last called :returns: velocity """ appliedVoltage = self._nominalVoltage * motor_pct appliedVoltage = math.copysign( max(abs(appliedVoltage) - self._vintercept, 0), appliedVoltage ) # Heun's method (taken from Ether's drivetrain calculator) # -> yn+1 = yn + (h/2) (f(xn, yn) + f(xn + h, yn + h f(xn, yn))) a0 = self.acceleration v0 = self.velocity # initial estimate for next velocity/acceleration v1 = v0 + a0 * tm_diff a1 = (appliedVoltage - self._kv * v1) / self._ka # corrected trapezoidal estimate v1 = v0 + (a0 + a1) * 0.5 * tm_diff a1 = (appliedVoltage - self._kv * v1) / self._ka self.position += (v0 + v1) * 0.5 * tm_diff self.velocity = v1 self.acceleration = a1 return self.velocity
[docs]class TankModel: """ This is a model of a FRC tankdrive-style drivetrain that will provide vaguely realistic motion for the simulator. This drivetrain model makes a number of assumptions: * N motors per side * Constant gearing * Motors are geared together * Wheels do not 'slip' on the ground * Each side of the robot moves in unison There are two ways to construct this model. You can use the theoretical model via :func:`TankModel.theory` and provide robot parameters such as gearing, total mass, etc. Alternatively, if you measure ``kv``, ``ka``, and ``vintercept`` as detailed in the paper mentioned above, you can plug those values in directly instead using the :class:`TankModel` constructor instead. For more information about measuring your own values, see the paper and `this thread on ChiefDelphi <https://www.chiefdelphi.com/forums/showthread.php?t=161539>`_. .. note:: You must specify the You can use whatever units you would like to specify the input parameter for your robot, RobotPy will convert them all to the correct units for computation. Output units for velocity and acceleration are in ft/s and ft/s^2 Example usage for a 90lb robot with 2 CIM motors on each side with 6 inch wheels:: from pyfrc.physics import motors, tankmodel from pyfrc.physics.units import units class PhysicsEngine: def __init__(self, physics_controller): self.physics_controller = physics_controller self.l_motor = hal.simulation.PWMSim(1) self.r_motor = hal.simulation.PWMSim(2) self.drivetrain = tankmodel.TankModel.theory(motors.MOTOR_CFG_CIM_IMP, robot_mass=90 * units.lbs, gearing=10.71, nmotors=2, x_wheelbase=2.0*feet, wheel_diameter=6*units.inch) def update_sim(self, now, tm_diff): l_motor = self.l_motor.getSpeed() r_motor = self.r_motor.getSpeed() transform = self.drivetrain.calculate(l_motor, r_motor, tm_diff) self.physics_controller.move_robot(transform) # optional: compute encoder # l_encoder = self.drivetrain.l_position * ENCODER_TICKS_PER_FT # r_encoder = self.drivetrain.r_position * ENCODER_TICKS_PER_FT """
[docs] @classmethod def theory( cls, motor_config: MotorModelConfig, robot_mass: units.Quantity, gearing: float, nmotors: int = 1, x_wheelbase: units.Quantity = _kitbot_wheelbase, robot_width: units.Quantity = _kitbot_width, robot_length: units.Quantity = _kitbot_length, wheel_diameter: units.Quantity = 6 * units.inch, vintercept: units.volts = 1.3 * units.volts, timestep: int = 5 * units.ms, ): r""" Use this to create the drivetrain model when you haven't measured ``kv`` and ``ka`` for your robot. :param motor_config: Specifications for your motor :param robot_mass: Mass of the robot :param gearing: Gear ratio .. so for a 10.74:1 ratio, you would pass 10.74 :param nmotors: Number of motors per side :param x_wheelbase: Wheelbase of the robot :param robot_width: Width of the robot :param robot_length: Length of the robot :param wheel_diameter: Diameter of the wheel :param vintercept: The minimum voltage required to generate enough torque to overcome steady-state friction (see the paper for more details) :param timestep_ms: Model computation timestep Computation of ``kv`` and ``ka`` are done as follows: * :math:`\omega_{free}` is the free speed of the motor * :math:`\tau_{stall}` is the stall torque of the motor * :math:`n` is the number of drive motors * :math:`m_{robot}` is the mass of the robot * :math:`d_{wheels}` is the diameter of the robot's wheels * :math:`r_{gearing}` is the total gear reduction between the motors and the wheels * :math:`V_{max}` is the nominal max voltage of the motor .. math:: velocity_{max} = \frac{\omega_{free} \cdot \pi \cdot d_{wheels} }{r_{gearing}} acceleration_{max} = \frac{2 \cdot n \cdot \tau_{stall} \cdot r_{gearing} }{d_{wheels} \cdot m_{robot}} k_{v} = \frac{V_{max}}{velocity_{max}} k_{a} = \frac{V_{max}}{acceleration_{max}} """ # Check input units # -> pint doesn't seem to support default args in check() Helpers.ensure_mass(robot_mass) Helpers.ensure_length(x_wheelbase) Helpers.ensure_length(robot_width) Helpers.ensure_length(robot_length) Helpers.ensure_length(wheel_diameter) max_velocity = (motor_config.freeSpeed * math.pi * wheel_diameter) / gearing max_acceleration = (2.0 * nmotors * motor_config.stallTorque * gearing) / ( wheel_diameter * robot_mass ) Helpers.ensure_velocity(max_velocity) Helpers.ensure_acceleration(max_acceleration) kv = motor_config.nominalVoltage / max_velocity ka = motor_config.nominalVoltage / max_acceleration kv = units.tm_kv.from_(kv, name="kv") ka = units.tm_ka.from_(ka, name="ka") logger.info( "Motor config: %d %s motors @ %.2f gearing with %.1f diameter wheels", nmotors, motor_config.name, gearing, wheel_diameter.m, ) logger.info( "- Theoretical: vmax=%.3f ft/s, amax=%.3f ft/s^2, kv=%.3f, ka=%.3f", max_velocity.m_as(units.foot / units.second), max_acceleration.m_as(units.foot / units.second ** 2), kv.m, ka.m, ) return cls( motor_config, robot_mass, x_wheelbase, robot_width, robot_length, kv, ka, vintercept, kv, ka, vintercept, timestep, )
def __init__( self, motor_config: MotorModelConfig, robot_mass: units.Quantity, x_wheelbase: units.Quantity, robot_width: units.Quantity, robot_length: units.Quantity, l_kv: units.Quantity, l_ka: units.Quantity, l_vi: units.volts, r_kv: units.Quantity, r_ka: units.Quantity, r_vi: units.volts, timestep: units.Quantity = 5 * units.ms, ): """ Use the constructor if you have measured ``kv``, ``ka``, and ``Vintercept`` for your robot. Use the :func:`.theory` function if you haven't. ``Vintercept`` is the minimum voltage required to generate enough torque to overcome steady-state friction (see the paper for more details). The robot width/length is used to compute the moment of inertia of the robot. Don't forget about bumpers! :param motor_config: Motor specification :param robot_mass: Mass of robot :param x_wheelbase: Wheelbase of the robot :param robot_width: Width of the robot :param robot_length: Length of the robot :param l_kv: Left side ``kv`` :param l_ka: Left side ``ka`` :param l_vi: Left side ``Vintercept`` :param r_kv: Right side ``kv`` :param r_ka: Right side ``ka`` :param r_vi: Right side ``Vintercept`` :param timestep: Model computation timestep """ # check input parameters Helpers.ensure_mass(robot_mass) Helpers.ensure_length(x_wheelbase) Helpers.ensure_length(robot_width) Helpers.ensure_length(robot_length) Helpers.ensure_time(timestep) logger.info( "Robot base: %.1fx%.1f frame, %.1f wheelbase, %.1f mass", robot_width.m, robot_length.m, x_wheelbase.m, robot_mass.m, ) self._lmotor = MotorModel(motor_config, l_kv, l_ka, l_vi) self._rmotor = MotorModel(motor_config, r_kv, r_ka, r_vi) self.inertia = (1 / 12.0) * robot_mass * (robot_length ** 2 + robot_width ** 2) # This is used to compute the rotational velocity self._bm = _bm_units.m_from((x_wheelbase / 2.0) * robot_mass) self._timestep = units.milliseconds.m_from(timestep, name="timestep") * 100 @property def l_velocity(self): """The velocity of the left side (in ft/s)""" return self._lmotor.velocity @property def r_velocity(self): """The velocity of the right side (in ft/s)""" return self._rmotor.velocity @property def l_position(self): """The linear position of the left side wheel (in feet)""" return self._lmotor.position @property def r_position(self): """The linear position of the right side wheel (in feet)""" return self._rmotor.position @property def inertia(self): """ The model computes a moment of inertia for your robot based on the given mass and robot width/length. If you wish to use a different moment of inertia, set this property after constructing the object Units are ``[mass] * [length] ** 2`` """ return self._inertia * _inertia_units @inertia.setter @units.wraps(None, (None, _inertia_units)) def inertia(self, value): self._inertia = value
[docs] def calculate(self, l_motor: float, r_motor: float, tm_diff: float) -> Transform2d: """ Given motor values and the amount of time elapsed since this was last called, retrieves the x,y,angle that the robot has moved. Pass these values to :meth:`PhysicsInterface.distance_drive`. To update your encoders, use the ``l_position`` and ``r_position`` attributes of this object. :param l_motor: Left motor value (-1 to 1); 1 is forward :param r_motor: Right motor value (-1 to 1); -1 is forward :param tm_diff: Elapsed time since last call to this function :returns: transform containing x/y/angle offsets of robot travel .. note:: If you are using more than 2 motors, it is assumed that all motors on each side are set to the same speed. Only pass in one of the values from each side .. versionadded:: 2020.1.0 """ # This isn't quite right, the right way is to use matrix math. However, # this is Good Enough for now... x = 0 y = 0 angle = 0 # split the time difference into timestep_ms steps total_time = int(tm_diff * 100000) steps = total_time // self._timestep remainder = total_time % self._timestep step = self._timestep / 100000.0 if remainder: last_step = remainder / 100000.0 steps += 1 else: last_step = step while steps != 0: if steps == 1: tm_diff = last_step else: tm_diff = step steps -= 1 l = self._lmotor.compute(l_motor, tm_diff) r = self._rmotor.compute(-r_motor, tm_diff) # Tank drive motion equations velocity = (l + r) * 0.5 # Thanks to Tyler Veness for fixing the rotation equation, via conservation # of angular momentum equations # -> omega = b * m * (l - r) / J rotation = self._bm * (r - l) / self._inertia distance = velocity * tm_diff turn = rotation * tm_diff x += distance * math.cos(angle) y += distance * math.sin(angle) angle += turn return Transform2d.fromFeet(x, y, angle)