from dataclasses import dataclass
from typing import Any
from mpscenes.obstacles.collision_obstacle import CollisionObstacle
from mpscenes.common.errors import MissmatchDimensionError, TrajectoryNotSupported
from mpscenes.common.analytic_trajectory import AnalyticTrajectory
from mpscenes.common.spline_trajectory import SplineTrajectory
[docs]@dataclass
class DynamicGeometryConfig:
"""Configuration dataclass for geometry.
This configuration class holds information about position
and radius of a dynamic sphere obstacle.
Parameters:
------------
trajectory: Any
trajectory description of the obstacle. Can be either a spline or
a analytic trajectory.
radius: float
radius of the obstacle
"""
trajectory: Any
[docs]class DynamicObstacle(CollisionObstacle):
def __init__(self, **kwargs):
super().__init__(**kwargs)
if "controlPoints" in self.geometry()["trajectory"]:
self._trajectory_type = "spline"
elif isinstance(self.geometry()["trajectory"][0], str):
self._trajectory_type = "analytic"
else:
raise TrajectoryNotSupported(
f"Trajectory definition not supported, {self.geometry()['trajectory']}"
)
self.check_completeness()
self.check_dimensionality()
if self.trajectory_type() == "spline":
self._traj = SplineTrajectory(
self.dimension(), traj=self._config.geometry.trajectory
)
elif self.trajectory_type() == "analytic":
self._traj = AnalyticTrajectory(
self.dimension(), traj=self._config.geometry.trajectory
)
self._traj.concretize()
[docs] def trajectory_type(self) -> str:
return self._trajectory_type
[docs] def check_dimensionality(self):
dim_verification = self.dimension()
if self.dimension() != dim_verification:
raise MissmatchDimensionError(
"Dynamic Obstacle: Dimension mismatch between trajectory array and dimension"
)
[docs] def dimension(self) -> int:
if self.trajectory_type() == "spline":
return len(self.geometry()["trajectory"]["controlPoints"][0])
if self.trajectory_type() == "analytic":
return len(self.geometry()["trajectory"])
[docs] def traj(self):
return self._traj
[docs] def position(self, **kwargs):
if "t" not in kwargs:
t = 0.0
else:
t = kwargs.get("t")
return self._traj.evaluate(t)[0]
[docs] def velocity(self, **kwargs):
if "t" not in kwargs:
t = 0.0
else:
t = kwargs.get("t")
return self._traj.evaluate(t)[1]
[docs] def acceleration(self, **kwargs):
if "t" not in kwargs:
t = 0.0
else:
t = kwargs.get("t")
return self._traj.evaluate(t)[2]