"""
Class for lqrrt constraints.
An instance of this class must be given to an lqRRT planner
to fully define the search problem.
"""
################################################# DEPENDENCIES
from __future__ import division
import numpy as np
import numpy.linalg as npl
from typing import Callable
################################################# PRIMARY CLASS
[docs]class Constraints:
"""
The constraints of the lqRRT system.
Args:
nstates: int - The dimensionality of the state space.
ncontrols: int - The dimensionality of the effort space.
goal_buffer: np.ndarray - Half-edge lengths of box defining goal region.
is_feasible: np.ndarray - Function that takes a state and effort and returns
a bool.
"""
def __init__(
self,
nstates: int,
ncontrols: int,
goal_buffer: np.ndarray,
is_feasible: Callable,
):
self.nstates = nstates
self.ncontrols = ncontrols
self.set_buffers(goal_buffer)
self.set_feasibility_function(is_feasible)
#################################################
[docs] def set_buffers(self, goal_buffer: np.ndarray = None) -> None:
"""
Sets the goal buffer constraints on the lqRRT system.
Args:
goal_buffer: np.ndarray - Half-edge lengths of a box defining the
goal region.
Raises:
ValueError - The goal buffer does not have the same dimensionality
as the state, or its length is not equal to the number of states.
"""
if goal_buffer is not None:
if len(goal_buffer) == self.nstates:
self.goal_buffer = np.abs(goal_buffer).astype(np.float64)
else:
raise ValueError(
"The goal_buffer must have same dimensionality as state."
)
#################################################
[docs] def set_feasibility_function(self, is_feasible: Callable) -> None:
"""
Sets the feasibility function for the lqRRT system.
Args:
is_feasible: Callable - The method that determines the feasibility
of movement.
Raises:
ValueError - is_feasible is not a method.
"""
if hasattr(is_feasible, "__call__"):
self.is_feasible = is_feasible
else:
raise ValueError("Expected is_feasible to be a function.")