Source code for pyfrc.physics.core

"""
    pyfrc supports simplistic custom physics model implementations for
    simulation and testing support. It can be as simple or complex as you want
    to make it. We will continue to add helper functions (such as the
    :mod:`pyfrc.physics.drivetrains` module) to make this a lot easier
    to do. General purpose physics implementations are welcome also!

    The idea is you provide a :class:`PhysicsEngine` object that interacts with
    the simulated HAL, and modifies motors/sensors accordingly depending on the
    state of the simulation. An example of this would be measuring a motor
    moving for a set period of time, and then changing a limit switch to turn
    on after that period of time. This can help you do more complex simulations
    of your robot code without too much extra effort.

    By default, pyfrc doesn't modify any of your inputs/outputs without being
    told to do so by your code or the simulation GUI.
    
    See the `physics sample <https://github.com/robotpy/examples/tree/master/physics/src>`_
    for more details.

    Enabling physics support
    ------------------------

    You must create a python module called ``physics.py`` next to your
    ``robot.py``. A physics module must have a class called
    :class:`PhysicsEngine` which must have a function called ``update_sim``.
    When initialized, it will be passed an instance of this object.
"""

from importlib.machinery import SourceFileLoader
import inspect
import logging
import pathlib
import types
import typing

import wpilib
import wpilib.simulation

from wpimath.kinematics import ChassisSpeeds
from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d, Twist2d


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


[docs] class PhysicsInitException(Exception): pass
[docs] class PhysicsEngine: """ Your physics module must contain a class called ``PhysicsEngine``, and it must implement the same functions as this class. Alternatively, you can inherit from this object. However, that is not required. """ def __init__(self, physics_controller: "PhysicsInterface"): """ The constructor must take the following arguments: :param physics_controller: The physics controller interface :type physics_controller: :class:`.PhysicsInterface` Optionally, it may take a second argument, which is an instance of your robot class. """ self.physics_controller = physics_controller
[docs] def update_sim(self, now: float, tm_diff: float): """ Called when the simulation parameters for the program should be updated. This is called after robotPeriodic is called. :param now: The current time :type now: float :param tm_diff: The amount of time that has passed since the last time that this function was called :type tm_diff: float """ pass
[docs] class PhysicsInterface: """ An instance of this is passed to the constructor of your :class:`PhysicsEngine` object. This instance is used to communicate information to the simulation, such as moving the robot on the field displayed to the user. """ @classmethod def _create_and_attach( cls: typing.Type["PhysicsInterface"], robot_class: typing.Type[wpilib.RobotBase], robot_path: pathlib.Path, ) -> typing.Tuple[ typing.Optional["PhysicsInterface"], typing.Type[wpilib.RobotBase] ]: interface: typing.Optional["PhysicsInterface"] = None physics_module_path = robot_path / "physics.py" if physics_module_path.exists(): # Load the user's physics module if it exists try: loader = SourceFileLoader("physics", str(physics_module_path)) physics_module = types.ModuleType(loader.name) loader.exec_module(physics_module) except: logger.exception("Error loading user physics module") raise PhysicsInitException() if not hasattr(physics_module, "PhysicsEngine"): logger.error("User physics module does not have a PhysicsEngine object") raise PhysicsInitException() logger.info("Physics support successfully enabled") interface = PhysicsInterface(physics_module) # We create a robot class so we can pass the robot object to # interface._simulationInit class PhysicsRobot(robot_class): def _simulationInit(self): interface._simulationInit(self) _simulationPeriodic = interface._simulationPeriodic # The user doesn't need to know that we didn't create their class directly.. PhysicsRobot.__name__ = robot_class.__name__ PhysicsRobot.__module__ = robot_class.__module__ PhysicsRobot.__qualname__ = robot_class.__qualname__ robot_class = PhysicsRobot else: logger.warning( "Cannot enable physics support, %s not found", physics_module_path ) return interface, robot_class def __init__(self, physics_module): self.last_tm = None self.module = physics_module self.engine = None self.field = None self.log_init_errors = True def _simulationInit(self, robot): # reset state first so that the PhysicsEngine constructor can use it self.field = wpilib.Field2d() wpilib.SmartDashboard.putData("Field", self.field) self.last_tm = None # look for a class called PhysicsEngine try: PhysicsEngine = self.module.PhysicsEngine except Exception as e: raise PhysicsInitException( "physics module does not have a 'PhysicsEngine' object" ) try: # if it has two arguments, the second argument is their robot... # - TODO: always pass robot in 2023 sig = inspect.signature(PhysicsEngine) if len(sig.parameters) == 2: self.engine = PhysicsEngine(self, robot) else: self.engine = PhysicsEngine(self) except Exception: if not self.log_init_errors: raise logger.exception("Error creating user's PhysicsEngine object") raise PhysicsInitException() def _simulationPeriodic(self): now = wpilib.Timer.getFPGATimestamp() last_tm = self.last_tm if last_tm is None: self.last_tm = now else: # When using time, always do it based on a differential! You may # not always be called at a constant rate tm_diff = now - last_tm # Don't run physics calculations more than 100hz if tm_diff > 0.010: try: self.engine.update_sim(now, tm_diff) except Exception as e: raise Exception( "User physics code raised an exception (see above)" ) from e self.last_tm = now ####################################################### # # Public API # #######################################################
[docs] def drive(self, speeds: ChassisSpeeds, tm_diff: float) -> Pose2d: """Call this from your :func:`PhysicsEngine.update_sim` function. Will update the robot's position on the simulation field. You can either calculate the chassis speeds yourself, or you can use the predefined functions in :mod:`pyfrc.physics.drivetrains`. The outputs of the `drivetrains.*` functions should be passed to this function. :param speeds: Represents current speed/angle of robot travel :param tm_diff: Amount of time speed was traveled (this is the same value that was passed to update_sim) :return: current robot pose .. versionchanged:: 2020.1.0 Input parameter is ChassisSpeeds object """ twist = Twist2d( dx=speeds.vx * tm_diff, dy=speeds.vy * tm_diff, dtheta=speeds.omega * tm_diff, ) pose = self.field.getRobotPose() pose = pose.exp(twist) self.field.setRobotPose(pose) return pose
[docs] def move_robot(self, transform: Transform2d) -> Pose2d: """Call this from your :func:`PhysicsEngine.update_sim` function. Will update the robot's position on the simulation field. This moves the robot some relative distance and angle from its current position. :param transform: The distance and angle to move the robot :return: current robot pose .. versionadded:: 2020.1.0 """ pose = self.field.getRobotPose() pose = pose + transform self.field.setRobotPose(pose) return pose
[docs] def get_pose(self): """ :returns: current robot pose .. versionadded:: 2020.1.0 """ return self.field.getRobotPose()
# def get_offset(self, point: Translation2d): # """ # Computes how far away and at what angle a coordinate is # located from the robot. # :returns: distance,angle offset of the given x,y coordinate # .. versionadded:: 2018.1.7 # .. versionchanged:: 2020.1.0 # """ # tr = self.field.getRobotPose().translation() # dx = tr.x_feet - x # dy = tr.y_feet - y # distance = math.hypot(dx, dy) # angle = math.atan2(dy, dx) # return distance, math.degrees(angle)