from abc import abstractmethod
from typing import TypedDict
import numpy as np
import pymunk
import pymunk.pygame_util
from symaware.base.data import Identifier
from symaware.base.models import DynamicalModel as BaseDynamicalModel
from .entities import Entity
[docs]
class DynamicalModel(BaseDynamicalModel):
"""
Abstract class for the dynamical models using the Pymunk physics engine.
Args
----
ID:
Identifier of the agent this model belongs to
control_input:
Initial control input of the agent. It also used to validate the size of future control inputs
"""
def __init__(self, ID: Identifier, control_input: np.ndarray):
super().__init__(ID, control_input=control_input)
self._entity: Entity
[docs]
def initialise(self, entity: Entity):
if not isinstance(entity, Entity):
raise ValueError(f"Expected instance of PymunkSpatialEntity, received {type(entity)}")
self._entity = entity
[docs]
@abstractmethod
def step(self):
pass
[docs]
class VelocityModel(DynamicalModel):
"""
Pymunk dynamical model.
It sets the velocity of the entity.
Todo
----
Ensure this model makes sense
Args
----
ID:
Identifier of the agent this model belongs to
control_input:
Initial control input of the agent. It also used to validate the size of future control inputs
max_force:
Maximum force that can be applied to the wheels
steering_links:
Tuple of the two links that are used to steer the car
motorized_wheels:
Tuple of the two links that are used to drive the car
"""
def __init__(self, ID: Identifier, max_force: float = 40.0):
super().__init__(ID, control_input=np.zeros(2))
self._max_force = max_force
@property
def subinputs_dict(self) -> VelocityModelSubinputs:
return {"velocity": self.control_input}
[docs]
def step(self):
f_x, f_y = self._control_input
f_x = -self._max_force if f_x < -self._max_force else self._max_force if f_x > self._max_force else f_x
f_y = -self._max_force if f_y < -self._max_force else self._max_force if f_y > self._max_force else f_y
self._entity.body.velocity = pymunk.Vec2d(f_x, f_y)
[docs]
class ForceModel(DynamicalModel):
"""
Pymunk dynamical model.
It applies an impulse force to the entity.
Todo
----
Ensure this model makes sense
Args
----
ID:
Identifier of the agent this model belongs to
control_input:
Initial control input of the agent. It also used to validate the size of future control inputs
max_force:
Maximum force that can be applied to the wheels
steering_links:
Tuple of the two links that are used to steer the car
motorized_wheels:
Tuple of the two links that are used to drive the car
"""
def __init__(self, ID: Identifier, max_force: float = 2.0):
super().__init__(ID, control_input=np.zeros(2))
self._max_force = max_force
@property
def subinputs_dict(self) -> ForceModelSubinputs:
return {"force": self.control_input}
[docs]
def step(self):
f_x, f_y = self._control_input
f_x = -self._max_force if f_x < -self._max_force else self._max_force if f_x > self._max_force else f_x
f_y = -self._max_force if f_y < -self._max_force else self._max_force if f_y > self._max_force else f_y
self._entity.body.apply_impulse_at_local_point((f_x, f_y))