Source code for symaware.simulators.carla.dynamical_model
from typing import Generic, TypedDict, TypeVar, overload
import carla
import numpy as np
from symaware.base import DynamicalModel as BaseDynamicalModel
from symaware.base import Identifier
A = TypeVar("T", bound="carla.Actor")
[docs]
class RacecarModelSubinputs(TypedDict):
target_velocity: np.ndarray
steering_angle: np.ndarray
[docs]
class RawModelSubinputs(TypedDict):
x: float
y: float
z: float
yaw: float
pitch: float
roll: float
[docs]
class VehicleControlSubinputs(TypedDict):
throttle: float
steer: float
brake: float
hand_brake: float
reverse: float
manual_gear_shift: float
gear: float
[docs]
class DynamicalModel(BaseDynamicalModel, Generic[A]):
"""Abstract class for dynamical models using the CARLA simulation engine.
This is the parent class for all CARLA-based dynamical models. Subclasses
implement specific models for different entity types (vehicles, walkers, etc.)
and control input formats.
Args
----
ID:
Identifier of the agent this model belongs to
control_input:
Initial control input of the agent. 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_id = -1
self._actor: A
[docs]
def initialise(self, actor: "carla.Actor"):
"""Initialize the dynamical model with a CARLA actor.
Args
----
actor:
The CARLA actor to associate with this dynamical model
"""
assert isinstance(actor, carla.Actor), f"Expected carla.Actor, got {type(actor)}"
self._actor = actor
[docs]
class StillDynamicalModel(DynamicalModel[carla.Actor]):
"""
A static dynamical model for entities that do not move.
This model takes no control inputs and keeps the entity stationary.
"""
def __init__(self, agent_id: Identifier):
super().__init__(agent_id, np.zeros(0))
@property
def subinputs_dict(self) -> RawModelSubinputs:
return {}
[docs]
class WalkDynamicalModel(DynamicalModel[carla.Walker]):
"""
Dynamical model for walker entities in CARLA.
This model controls walker movement using speed, direction, and jump parameters.
The walker moves continuously in the specified direction at the given speed.
"""
def __init__(
self,
agent_id: Identifier = -1,
speed: float = 1.0,
direction: "tuple[float, float, float]" = -1.0,
jump: bool = False,
):
super().__init__(agent_id, np.zeros(0))
self._speed = speed
self._direction = direction
self._jump = jump
@property
def subinputs_dict(self) -> RawModelSubinputs:
"""Dictionary of named control inputs (empty for walker model)."""
return {}
[docs]
def control_input_to_array() -> np.ndarray:
"""Convert control input to array representation (empty for walker model)."""
return np.array()
[docs]
def initialise(self, actor: "carla.Walker"):
super().initialise(actor)
assert isinstance(self._actor, carla.Walker), f"Expected carla.Walker, got {type(self._actor)}"
self._actor.apply_control(
carla.WalkerControl(
speed=self._speed,
direction=carla.Vector3D(*self._direction),
jump=self._jump,
)
)
[docs]
class RawDynamicalModel(DynamicalModel[carla.Actor]):
"""Raw dynamical model for entities.
Allows setting the position and orientation of an entity directly.
Takes a control input of six elements: x, y, z position and yaw, pitch, roll rotations.
"""
def __init__(self, agent_id: Identifier):
super().__init__(agent_id, np.zeros(6))
@property
def subinputs_dict(self) -> RawModelSubinputs:
return {
"x": self._control_input[0],
"y": self._control_input[1],
"z": self._control_input[2],
"yaw": self._control_input[3],
"pitch": self._control_input[4],
"roll": self._control_input[5],
}
[docs]
def set_control_input(self, x: float, y: float, z: float, yaw: float, pitch: float, roll: float):
"""Set the control input with position and rotation values.
Args
----
x:
X position coordinate
y:
Y position coordinate
z:
Z position coordinate
yaw:
Yaw rotation angle
pitch:
Pitch rotation angle
roll:
Roll rotation angle
"""
self.control_input = self.control_input_to_array(x, y, z, yaw, pitch, roll)
[docs]
def control_input_to_array(self, x: float, y: float, z: float, yaw: float, pitch: float, roll: float) -> np.ndarray:
"""Convert position and rotation values to array.
Returns
-------
Array with [x, y, z, yaw, pitch, roll]
"""
return np.array([x, y, z, yaw, pitch, roll])
[docs]
def step(self):
self._actor.set_transform(
carla.Transform(
location=carla.Location(*self._control_input[:3]), rotation=carla.Rotation(*self._control_input[3:])
)
)
[docs]
class VelocityDynamicalModel(DynamicalModel[carla.Actor]):
"""Velocity-based dynamical model for entities.
Allows setting the target velocity of an entity directly.
Takes a control input of three elements: x, y, z velocity components.
"""
def __init__(self, agent_id: Identifier):
super().__init__(agent_id, np.zeros(3))
@property
def subinputs_dict(self) -> VelocitySubinputs:
return {
"x": self._control_input[0],
"y": self._control_input[1],
"z": self._control_input[2],
}
[docs]
def set_control_input(self, x: float, y: float, z: float):
"""Set the target velocity for the entity.
Args
----
x:
Velocity in x direction
y:
Velocity in y direction
z:
Velocity in z direction
"""
self.control_input = self.control_input_to_array(x, y, z)
[docs]
def control_input_to_array(self, x: float, y: float, z: float) -> np.ndarray:
"""Convert velocity values to array.
Returns
-------
Array with [x, y, z] velocity components
"""
return np.array((x, y, z))
[docs]
class VehicleDynamicalModel(DynamicalModel[carla.Vehicle]):
"""Velocity-based dynamical model for vehicle entities.
Allows setting the target velocity of the vehicle directly.
Takes a control input of three elements: x, y, z velocity components.
"""
def __init__(self, agent_id: Identifier):
super().__init__(agent_id, np.zeros(3))
@property
def subinputs_dict(self) -> VelocitySubinputs:
return {
"x": self._control_input[0],
"y": self._control_input[1],
"z": self._control_input[2],
}
[docs]
def set_control_input(self, x: float, y: float, z: float):
"""Set the target velocity for the vehicle.
Args
----
x:
Velocity in x direction
y:
Velocity in y direction
z:
Velocity in z direction
"""
self.control_input = self.control_input_to_array(x, y, z)
[docs]
def control_input_to_array(self, x: float, y: float, z: float) -> np.ndarray:
"""Convert velocity values to array.
Returns
-------
Array with [x, y, z] velocity components
"""
return np.array((x, y, z))
[docs]
class VehicleControlModel(DynamicalModel[carla.Vehicle]):
"""
A simple model that uses the built-in vehicle control mechanism of CARLA.
"""
def __init__(self, agent_id: Identifier):
super().__init__(agent_id, np.zeros(7))
@property
def subinputs_dict(self) -> VehicleControlSubinputs:
return {
"throttle": self._control_input[0],
"steer": self._control_input[1],
"brake": self._control_input[2],
"hand_brake": self._control_input[3],
"reverse": self._control_input[4],
"manual_gear_shift": self._control_input[5],
"gear": self._control_input[6],
}
@overload
def set_control_input(
self,
throttle: float = 0.0,
steer: float = 0.0,
brake: float = 0.0,
hand_brake: bool = False,
reverse: bool = False,
manual_gear_shift: bool = False,
gear: int = 0,
) -> np.ndarray: ...
@overload
def set_control_input(self, throttle: "carla.VehicleControl") -> np.ndarray: ...
[docs]
def set_control_input(
self,
throttle: "carla.VehicleControl | float" = 0.0,
steer: float = 0.0,
brake: float = 0.0,
hand_brake: bool = False,
reverse: bool = False,
manual_gear_shift: bool = False,
gear: int = 0,
):
"""Set the vehicle control inputs.
This method supports two calling conventions: individual parameters or
a carla.VehicleControl object.
Args
----
throttle:
Throttle value (0-1) or a VehicleControl object
steer:
Steering angle (-1 to 1)
brake:
Brake value (0-1)
hand_brake:
Hand brake flag
reverse:
Reverse flag
manual_gear_shift:
Manual gear shift flag
gear:
Gear number
"""
self.control_input = self.control_input_to_array(
throttle, steer, brake, hand_brake, reverse, manual_gear_shift, gear
)
@overload
def control_input_to_array(
self,
throttle: float = 0.0,
steer: float = 0.0,
brake: float = 0.0,
hand_brake: bool = False,
reverse: bool = False,
manual_gear_shift: bool = False,
gear: int = 0,
) -> np.ndarray: ...
@overload
def control_input_to_array(self, throttle: "carla.VehicleControl") -> np.ndarray: ...
[docs]
def control_input_to_array(
throttle: "carla.VehicleControl | float" = 0.0,
steer: float = 0.0,
brake: float = 0.0,
hand_brake: bool = False,
reverse: bool = False,
manual_gear_shift: bool = False,
gear: int = 0,
) -> np.ndarray:
"""Convert vehicle control parameters to array representation.
Returns
-------
Array with [throttle, steer, brake, hand_brake, reverse, manual_gear_shift, gear]
"""
return (
np.array(
[
throttle.throttle,
throttle.steer,
throttle.brake,
throttle.hand_brake,
throttle.reverse,
throttle.manual_gear_shift,
throttle.gear,
]
)
if isinstance(throttle, carla.VehicleControl)
else np.array([throttle, steer, brake, hand_brake, reverse, manual_gear_shift, gear])
)
[docs]
def step(self):
self._actor.apply_control(
carla.VehicleControl(
throttle=self._control_input[0],
steer=self._control_input[1],
brake=self._control_input[2],
hand_brake=bool(self._control_input[3]),
reverse=bool(self._control_input[4]),
manual_gear_shift=bool(self._control_input[5]),
gear=int(self._control_input[6]),
)
)