181 lines
5.6 KiB
Python
181 lines
5.6 KiB
Python
"""This module contains a collection of cost functions."""
|
|
|
|
# Support for abstract classes
|
|
from abc import ABC
|
|
from abc import abstractmethod
|
|
|
|
# Static Typing
|
|
from typing import Optional
|
|
from typing import Tuple
|
|
|
|
# Scientific Computing
|
|
import numpy as np
|
|
from scipy import linalg
|
|
|
|
# Backend
|
|
from .evaluated_timing_frame import EvaluatedTimingFrame
|
|
|
|
|
|
class CostFunction(ABC):
|
|
|
|
"""Class to encapsulate different types of cost functions for different types of obstacles.
|
|
|
|
A cost function describes the cost of passing by an obstacle ad a given distance in a unit-less
|
|
measurement. Cost functions must be differentiable with respect to its argument dist.
|
|
"""
|
|
|
|
#: A human-readable name of the cost function
|
|
name: str
|
|
|
|
@abstractmethod
|
|
def cost(self, dist: float) -> np.floating:
|
|
"""Calculates the cost of an obstacle based on a distance.
|
|
|
|
Args:
|
|
dist: distance to evaluate the cost from
|
|
|
|
Returns:
|
|
The evaluated cost
|
|
"""
|
|
|
|
@abstractmethod
|
|
def cost_grad(self, dist: float) -> Tuple[np.floating, np.floating]:
|
|
"""Calculates the cost AND the derivative of it w.r.t. to the distance.
|
|
|
|
Args:
|
|
dist: distance to base calculation upon
|
|
|
|
Returns:
|
|
A tuple of ``(cost, gradient)``
|
|
"""
|
|
|
|
|
|
class CostFunctionLinear(CostFunction):
|
|
|
|
"""Class that represents a linear Cost Function.
|
|
|
|
This function is bounded by the parameter attr:`maximum_cost`.
|
|
Reproduces the values of the function :math:`f(x) = maximum_cost - fact*x`.
|
|
|
|
Args:
|
|
fact: factor for cost decay
|
|
maximum_cost: maximal cost
|
|
"""
|
|
|
|
def __init__(self, fact: float = 1.0, maximum_cost: float = 100) -> None:
|
|
self.fact: np.floating = np.float64(fact)
|
|
self.maximum_cost = maximum_cost
|
|
self.name = "linear"
|
|
|
|
def cost(self, dist: float) -> np.floating:
|
|
return np.float64(-self.fact * np.float64(dist) + self.maximum_cost)
|
|
|
|
def cost_grad(self, dist: float) -> Tuple[np.floating, np.floating]:
|
|
return np.float64(-self.fact * dist + self.maximum_cost), np.float64(-self.fact)
|
|
|
|
|
|
class CostFunctionInverse(CostFunction):
|
|
|
|
"""Class that represents a inversely proportional cost function.
|
|
|
|
This functions produces values in the interval :math:`[0, ∞)`.
|
|
Reproduces the values of the function :math:`f(x) = 1/x`.
|
|
|
|
Args:
|
|
fact: undecayed cost for a unit of distance
|
|
"""
|
|
|
|
def __init__(self, fact: float = 1.0) -> None:
|
|
self.fact: np.floating = np.float64(fact)
|
|
self.name = "linear"
|
|
|
|
def cost(self, dist: float) -> np.floating:
|
|
return np.float64(self.fact / np.float64(dist))
|
|
|
|
def cost_grad(self, dist: float) -> Tuple[np.floating, np.floating]:
|
|
return np.float64(self.fact / np.float64(dist)), np.float64(self.fact / np.float64(dist**2))
|
|
|
|
|
|
class CostFunctionExp(CostFunction):
|
|
|
|
"""Simple Exponential cost function for use with gradient and cost calculations in TimingFrame
|
|
|
|
Args:
|
|
safety_dist: safety distance the ship should hold
|
|
clip: clip to determine accuracy
|
|
"""
|
|
|
|
def __init__(
|
|
self, safety_dist: float = 3.0, clip: float = 100000, linear_scale: float = 100, scale: float = 1
|
|
) -> None:
|
|
self.name = "Exponential cost"
|
|
|
|
self.clip: np.floating = np.float64(clip)
|
|
self.scale: np.floating = np.float64(scale)
|
|
self.linear_scale: np.floating = np.float64(linear_scale)
|
|
self.safety_dist: np.floating = np.float64(safety_dist)
|
|
|
|
def cost(self, dist: float) -> np.floating:
|
|
if dist == 0:
|
|
return self.clip
|
|
|
|
with np.errstate(over="ignore"):
|
|
temp = np.clip(np.exp(self.scale * self.safety_dist / dist), None, self.clip)
|
|
|
|
return np.float64(temp * self.linear_scale)
|
|
|
|
def cost_grad(self, dist: float) -> Tuple[np.floating, np.floating]:
|
|
with np.errstate(over="ignore"):
|
|
cost = np.exp(self.scale * self.safety_dist / dist) if dist > 0 else self.clip
|
|
|
|
grad = -cost * self.scale * self.safety_dist / dist**2 if dist > 0 else 0
|
|
cost = np.clip(cost, None, self.clip)
|
|
grad = np.clip(grad, -self.clip, self.clip)
|
|
|
|
assert not np.isnan(grad).any()
|
|
assert not np.isnan(cost).any()
|
|
|
|
return np.float64(cost), np.float64(grad)
|
|
|
|
|
|
def default_cache_metric(frame: EvaluatedTimingFrame, lock: Optional[EvaluatedTimingFrame]) -> float:
|
|
|
|
"""This is a simple demo cost function for the route cache.
|
|
|
|
It determines exactly what is the 'best' TimingFrame with respect to an environment (already
|
|
captured in ``frame.actual_cost``) and in this case the route we are currently pursuing.
|
|
|
|
Args:
|
|
frame: frame that is to be judged
|
|
lock: frame the route cache is locked onto (last recommended frame)
|
|
|
|
Returns:
|
|
score for the TimingFrame
|
|
"""
|
|
|
|
angle = 0
|
|
if lock:
|
|
list_of_locations = frame.route.locations
|
|
heading = np.array(
|
|
[
|
|
list_of_locations[1].east - list_of_locations[0].east,
|
|
list_of_locations[1].north - list_of_locations[0].north,
|
|
]
|
|
)
|
|
|
|
list_of_locations2 = lock.route.locations
|
|
heading2 = np.array(
|
|
[
|
|
list_of_locations2[1].east - list_of_locations2[0].east,
|
|
list_of_locations2[1].north - list_of_locations2[0].north,
|
|
]
|
|
)
|
|
# normalize
|
|
heading2 = heading2 / linalg.norm(heading2)
|
|
|
|
angle = np.degrees(np.arccos(np.dot(heading, heading2)))
|
|
|
|
assert frame.actual_cost != 0.0
|
|
|
|
return float(frame.actual_cost + np.clip(angle * 10, a_min=0, a_max=frame.actual_cost * 0.2))
|