diff --git a/armour/ArmourGoal.py b/armour/ArmourGoal.py index f5ad142..68ee8a9 100644 --- a/armour/ArmourGoal.py +++ b/armour/ArmourGoal.py @@ -8,8 +8,8 @@ import numpy as np from trimesh import Trimesh from typing import OrderedDict -from nptyping import NDArray from math import pi +from rtd.util.mixins.Typings import Matnp # define top level module logger import logging @@ -79,7 +79,7 @@ def reset(self, **options): def create_plot_data(self, time: float = None) -> list[Actor]: # generate mesh config = self.goal_position - fk: OrderedDict[Trimesh, NDArray] = self.arm_agent.info.robot.visual_trimesh_fk(cfg=config) + fk: OrderedDict[Trimesh, Matnp] = self.arm_agent.info.robot.visual_trimesh_fk(cfg=config) meshes = [mesh.copy().apply_transform(transform) for mesh, transform in fk.items()] self.plot_data: list[Actor] = list() diff --git a/armour/agent/ArmourAgentCollision.py b/armour/agent/ArmourAgentCollision.py index d7d539e..fb25e44 100644 --- a/armour/agent/ArmourAgentCollision.py +++ b/armour/agent/ArmourAgentCollision.py @@ -3,7 +3,7 @@ from armour.agent import ArmourAgentInfo, ArmourAgentState import numpy as np from collections import OrderedDict -from nptyping import NDArray +from rtd.util.mixins.Typings import Matnp @@ -25,7 +25,7 @@ def reset(self): pass - def getCollisionObject(self, q: NDArray = None, time: float = None) -> CollisionObject: + def getCollisionObject(self, q: Matnp = None, time: float = None) -> CollisionObject: ''' Generates a CollisionObject for a given time `time` or configuration `q` (only one or none must be provided) @@ -36,7 +36,7 @@ def getCollisionObject(self, q: NDArray = None, time: float = None) -> Collision elif time is not None and q is None: config = self.arm_state.get_state(np.array([time])).position # position at given time - fk: OrderedDict[Trimesh, NDArray] = self.arm_info.robot.collision_trimesh_fk(cfg=config) + fk: OrderedDict[Trimesh, Matnp] = self.arm_info.robot.collision_trimesh_fk(cfg=config) meshes = [mesh.copy().apply_transform(transform) for mesh, transform in fk.items()] return CollisionObject(meshes, id(self.arm_info)) diff --git a/armour/agent/ArmourAgentVisual.py b/armour/agent/ArmourAgentVisual.py index f649534..ebd771d 100644 --- a/armour/agent/ArmourAgentVisual.py +++ b/armour/agent/ArmourAgentVisual.py @@ -6,8 +6,7 @@ import numpy as np from trimesh import Trimesh from typing import OrderedDict -from nptyping import NDArray - +from rtd.util.mixins.Typings import Matnp class ArmourAgentVisual(PyvistaVisualObject, Options): @@ -56,7 +55,7 @@ def create_plot_data(self, time: float = None) -> list[Actor]: # generate mesh config = self.arm_state.get_state(np.array([time])).position - fk: OrderedDict[Trimesh, NDArray] = self.arm_info.robot.visual_trimesh_fk(cfg=config) + fk: OrderedDict[Trimesh, Matnp] = self.arm_info.robot.visual_trimesh_fk(cfg=config) meshes = [mesh.copy().apply_transform(transform) for mesh, transform in fk.items()] self.plot_data: list[Actor] = list() @@ -87,7 +86,7 @@ def plot(self, time: float = None): # generate mesh config = self.arm_state.get_state(np.array([time])).position - fk: OrderedDict[Trimesh, NDArray] = self.arm_info.robot.visual_trimesh_fk(cfg=config) + fk: OrderedDict[Trimesh, Matnp] = self.arm_info.robot.visual_trimesh_fk(cfg=config) meshes = [mesh.copy().apply_transform(transform) for mesh, transform in fk.items()] for actor, mesh in zip(self.plot_data, meshes): diff --git a/armour/agent/ArmourIdealController.py b/armour/agent/ArmourIdealController.py index 485c846..b103753 100644 --- a/armour/agent/ArmourIdealController.py +++ b/armour/agent/ArmourIdealController.py @@ -5,7 +5,6 @@ from armour.agent import ArmourAgentInfo, ArmourAgentState from armour.trajectory import ZeroHoldArmTrajectory import numpy as np -from nptyping import NDArray # define top level module logger import logging diff --git a/armour/legacy/StraightLineHLP.py b/armour/legacy/StraightLineHLP.py index 1b3efb8..709b51d 100644 --- a/armour/legacy/StraightLineHLP.py +++ b/armour/legacy/StraightLineHLP.py @@ -1,14 +1,14 @@ from armour.agent import ArmourAgentInfo -from nptyping import NDArray import numpy as np +from rtd.util.mixins.Typings import Vecnp class StraightLineHLP(): def __init__(self): self.default_lookahead_distance: float = 1 - self.goal: NDArray = None - self.joint_state_indices: NDArray = None + self.goal: Vecnp = None + self.joint_state_indices: Vecnp = None def setup(self, agent_info: ArmourAgentInfo, world_info: dict): @@ -16,7 +16,7 @@ def setup(self, agent_info: ArmourAgentInfo, world_info: dict): self.arm_joint_state_indices = agent_info.joint_state_indices - def get_waypoint(self, state: NDArray, lookahead_distance: float = None): + def get_waypoint(self, state: Vecnp, lookahead_distance: float = None) -> Vecnp: if lookahead_distance is None: lookahead_distance = self.default_lookahead_distance q_cur = state[self.arm_joint_state_indices] diff --git a/armour/legacy/bernstein_to_poly.py b/armour/legacy/bernstein_to_poly.py index 0fb7b0c..d81cbdd 100644 --- a/armour/legacy/bernstein_to_poly.py +++ b/armour/legacy/bernstein_to_poly.py @@ -1,9 +1,10 @@ from math import comb import numpy as np +from rtd.util.mixins.Typings import Vec, Vecnp -def bernstein_to_poly(beta: list[float], n: int): +def bernstein_to_poly(beta: Vec, n: int) -> Vecnp: ''' converts bernstein polynomial coefficients to monomial coefficients diff --git a/armour/legacy/match_deg5_bernstein_coefficients.py b/armour/legacy/match_deg5_bernstein_coefficients.py index 04202c9..ce1392c 100644 --- a/armour/legacy/match_deg5_bernstein_coefficients.py +++ b/armour/legacy/match_deg5_bernstein_coefficients.py @@ -1,4 +1,8 @@ -def match_deg5_bernstein_coefficients(traj_constraints: list[float], T: float = 1): +from rtd.util.mixins.Typings import Vec + + + +def match_deg5_bernstein_coefficients(traj_constraints: Vec, T: float = 1) -> Vec: ''' match coefficients to initial position, velocity, acceleration (t=0) and final position, velocity, and acceleration (t=1) diff --git a/armour/trajectory/ArmTrajectoryFactory.py b/armour/trajectory/ArmTrajectoryFactory.py index dfdfca9..3997382 100644 --- a/armour/trajectory/ArmTrajectoryFactory.py +++ b/armour/trajectory/ArmTrajectoryFactory.py @@ -4,10 +4,7 @@ from rtd.planner.trajopt import TrajOptProps from armour.reachsets import JRSInstance from armour.trajectory import PiecewiseArmTrajectory, ZeroHoldArmTrajectory, BernsteinArmTrajectory -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -21,7 +18,7 @@ def __init__(self, trajOptProps: TrajOptProps, traj_type: str = "piecewise"): def createTrajectory(self, robotState: EntityState, rsInstances: dict[str, ReachSetInstance] = None, - trajectoryParams: RowVec = None, jrsInstance: JRSInstance = None, + trajectoryParams: Vecnp = None, jrsInstance: JRSInstance = None, traj_type: str = None) -> ZeroHoldArmTrajectory | PiecewiseArmTrajectory | BernsteinArmTrajectory: ''' Create a new trajectory object for the given state diff --git a/armour/trajectory/BernsteinArmTrajectory.py b/armour/trajectory/BernsteinArmTrajectory.py index 14ba6ad..957fb63 100644 --- a/armour/trajectory/BernsteinArmTrajectory.py +++ b/armour/trajectory/BernsteinArmTrajectory.py @@ -5,10 +5,7 @@ from armour.reachsets import JRSInstance from armour.legacy import bernstein_to_poly, match_deg5_bernstein_coefficients import numpy as np -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -31,7 +28,7 @@ def __init__(self, trajOptProps: TrajOptProps, startState: ArmRobotState, jrsIns self.jrsInstance = jrsInstance - def setParameters(self, trajectoryParams: RowVec, startState: ArmRobotState = None, + def setParameters(self, trajectoryParams: Vecnp, startState: ArmRobotState = None, jrsInstance: JRSInstance = None): ''' A validated method to set the parameters for the trajectory @@ -97,7 +94,7 @@ def internalUpdate(self): self.q_end = q_goal - def getCommand(self, time: RowVec) -> EntityState: + def getCommand(self, time: Vecnp) -> EntityState: # Do a parameter check and time check, and throw if anything is # invalid. self.validate(throwOnError=True) diff --git a/armour/trajectory/PieceWiseArmTrajectory.py b/armour/trajectory/PieceWiseArmTrajectory.py index 0afca16..da167a0 100644 --- a/armour/trajectory/PieceWiseArmTrajectory.py +++ b/armour/trajectory/PieceWiseArmTrajectory.py @@ -4,10 +4,7 @@ from armour.reachsets import JRSInstance from rtd.functional.vectools import rescale import numpy as np -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vec, Mat, Vecnp, Matnp, Bound, Bounds, Boundsnp @@ -38,7 +35,7 @@ class to update all other internal parameters once fully self.q_end: float = None - def setParameters(self, trajectoryParams: RowVec, startState: ArmRobotState = None, + def setParameters(self, trajectoryParams: Vecnp, startState: ArmRobotState = None, jrsInstance: JRSInstance = None): ''' Set the parameters of the trajectory, with a focus on the @@ -97,7 +94,7 @@ def internalUpdate(self): + 0.5*self.q_ddot_to_stop*self.trajOptProps.planTime**2) - def getCommand(self, time: RowVec) -> ArmRobotState: + def getCommand(self, time: Vecnp) -> ArmRobotState: ''' Computes the actual input commands for the given time. throws InvalidTrajectory if the trajectory isn't set diff --git a/armour/trajectory/ZeroHoldArmTrajectory.py b/armour/trajectory/ZeroHoldArmTrajectory.py index cdffde3..810e584 100644 --- a/armour/trajectory/ZeroHoldArmTrajectory.py +++ b/armour/trajectory/ZeroHoldArmTrajectory.py @@ -1,10 +1,7 @@ from rtd.planner.trajectory import Trajectory, InvalidTrajectory from rtd.entity.states import ArmRobotState import numpy as np -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -23,7 +20,7 @@ class to update all other internal parameters once fully self.startState = startState - def setParameters(self, trajectoryParams: RowVec, startState: ArmRobotState = None): + def setParameters(self, trajectoryParams: Vecnp, startState: ArmRobotState = None): ''' Set the parameters of the trajectory, with a focus on the parameters as the state should be set from the constructor @@ -46,7 +43,7 @@ def validate(self, throwOnError: bool = False) -> bool: return valid - def getCommand(self, time: RowVec) -> ArmRobotState: + def getCommand(self, time: Vecnp) -> ArmRobotState: ''' Computes the actual input commands for the given time. throws InvalidTrajectory if the trajectory isn't set diff --git a/rtd/planner/reachsets/ReachSetInstance.py b/rtd/planner/reachsets/ReachSetInstance.py index c31da37..43ab3ab 100644 --- a/rtd/planner/reachsets/ReachSetInstance.py +++ b/rtd/planner/reachsets/ReachSetInstance.py @@ -1,10 +1,7 @@ from abc import ABCMeta, abstractmethod from rtd.sim.world import WorldState from typing import Callable -from nptyping import NDArray, Shape, Float64 - -# type hinting -BoundsVec = NDArray[Shape['N,2'], Float64] +from rtd.util.mixins.Typings import Boundsnp @@ -21,7 +18,7 @@ class ReachSetInstance(metaclass=ABCMeta): def __init__(self): # A 2-column vector denoting the input minimum and maximums for the # reachable set on the left and right, respectively - self.input_range: BoundsVec = None + self.input_range: Boundsnp = None # The number of main shared parameters used by this set. Generally, # this should match the size of the final trajectory parameters diff --git a/rtd/planner/trajectory/Trajectory.py b/rtd/planner/trajectory/Trajectory.py index f45ed4e..8f3f74b 100644 --- a/rtd/planner/trajectory/Trajectory.py +++ b/rtd/planner/trajectory/Trajectory.py @@ -1,10 +1,7 @@ from abc import ABCMeta, abstractmethod from rtd.entity.states import EntityState from rtd.planner.trajopt import TrajOptProps -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -23,7 +20,7 @@ def __init__(self): self.trajOptProps: TrajOptProps = None # The parameters used for this trajectory - self.trajectoryParams: RowVec = None + self.trajectoryParams: Vecnp = None # The time at which this trajectory is valid self.startTime: float = None @@ -52,7 +49,7 @@ def validate(self, throwOnError: bool = False) -> bool: @abstractmethod - def setParameters(self, trajectoryParams: RowVec, **options): + def setParameters(self, trajectoryParams: Vecnp, **options): ''' Set the parameters for the trajectory @@ -67,7 +64,7 @@ def setParameters(self, trajectoryParams: RowVec, **options): @abstractmethod - def getCommand(self, time: float | RowVec) -> EntityState: + def getCommand(self, time: float | Vecnp) -> EntityState: ''' Computes the actual state to track for the given time diff --git a/rtd/planner/trajectory/TrajectoryContainer.py b/rtd/planner/trajectory/TrajectoryContainer.py index 7a7efeb..8787f4d 100644 --- a/rtd/planner/trajectory/TrajectoryContainer.py +++ b/rtd/planner/trajectory/TrajectoryContainer.py @@ -2,7 +2,6 @@ from rtd.entity.states import EntityState from rtd.functional.sequences import toSequence import numpy as np -from nptyping import NDArray class BadTrajectoryException(Exception): diff --git a/rtd/planner/trajectory/TrajectoryFactory.py b/rtd/planner/trajectory/TrajectoryFactory.py index 6dcab83..8641093 100644 --- a/rtd/planner/trajectory/TrajectoryFactory.py +++ b/rtd/planner/trajectory/TrajectoryFactory.py @@ -3,10 +3,7 @@ from rtd.planner.trajopt import TrajOptProps from rtd.planner.trajectory import Trajectory from rtd.planner.reachsets import ReachSetInstance -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -25,7 +22,7 @@ def __init__(self): @abstractmethod def createTrajectory(self, robotState: EntityState, rsInstances: dict[str, ReachSetInstance] = None, - trajectoryParams: RowVec = None, **options) -> Trajectory: + trajectoryParams: Vecnp = None, **options) -> Trajectory: ''' Factory method to create the trajectory diff --git a/rtd/planner/trajopt/GenericArmObjective.py b/rtd/planner/trajopt/GenericArmObjective.py index 2f4d30b..9b80539 100644 --- a/rtd/planner/trajopt/GenericArmObjective.py +++ b/rtd/planner/trajopt/GenericArmObjective.py @@ -4,10 +4,7 @@ from rtd.planner.trajopt import Objective, TrajOptProps from rtd.planner.trajectory import TrajectoryFactory, Trajectory import numpy as np -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -43,7 +40,7 @@ def genObjective(self, robotState: EntityState, waypoint, reachableSets: dict[st @staticmethod - def evalTrajectory(trajectoryParams: RowVec, trajectoryObj: Trajectory, q_des, t_cost: float | RowVec) -> float: + def evalTrajectory(trajectoryParams: Vecnp, trajectoryObj: Trajectory, q_des, t_cost: float | Vecnp) -> float: ''' Helper function purely accessible to this class without any class state which a handle can be made to to evaluate the trajectory for the cost. diff --git a/rtd/planner/trajopt/OptimizationEngine.py b/rtd/planner/trajopt/OptimizationEngine.py index e4fd114..63aef2c 100644 --- a/rtd/planner/trajopt/OptimizationEngine.py +++ b/rtd/planner/trajopt/OptimizationEngine.py @@ -1,9 +1,6 @@ from abc import ABCMeta, abstractmethod from typing import Callable -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -12,8 +9,8 @@ class OptimizationEngine(metaclass=ABCMeta): Base class for any sort of nonlinear optimizer used ''' @abstractmethod - def performOptimization(self, initialGuess: RowVec, objectiveCallback: Callable, - constraintCallback: Callable, bounds: dict) -> tuple[bool, RowVec, float]: + def performOptimization(self, initialGuess: Vecnp, objectiveCallback: Callable, + constraintCallback: Callable, bounds: dict) -> tuple[bool, Vecnp, float]: ''' Use the given optimizer to perform the optimization RowVector diff --git a/rtd/planner/trajopt/RtdTrajOpt.py b/rtd/planner/trajopt/RtdTrajOpt.py index 092eb54..c4dc618 100644 --- a/rtd/planner/trajopt/RtdTrajOpt.py +++ b/rtd/planner/trajopt/RtdTrajOpt.py @@ -1,21 +1,18 @@ from rtd.planner.trajopt import TrajOptProps, Objective, OptimizationEngine from rtd.planner.trajectory import TrajectoryFactory from rtd.planner.reachsets import ReachSetGenerator -from rtd.sim.world import WorldEntity, WorldState +from rtd.sim.world import WorldState from rtd.entity.states import EntityState from rtd.planner.trajectory import Trajectory from rtd.planner.reachsets import ReachSetInstance import numpy as np from typing import Callable -from nptyping import NDArray, Shape, Float64 +from rtd.util.mixins.Typings import Vecnp # define top level module logger import logging logger = logging.getLogger(__name__) -# type hinting -RowVec = NDArray[Shape['N'], Float64] - class RtdTrajOpt: @@ -102,7 +99,7 @@ def solveTrajOpt(self, robotState: EntityState, worldState: WorldState, waypoint # generate nonlinear constraints successes: dict[int, bool] = dict() - parameters: dict[int, RowVec] = dict() + parameters: dict[int, Vecnp] = dict() costs: dict[int, float] = dict() for (rs_id, rsInstances) in rsInstances_dict.items(): diff --git a/rtd/planner/trajopt/ScipyOptimizationEngine.py b/rtd/planner/trajopt/ScipyOptimizationEngine.py index 8adb3d6..91e77de 100644 --- a/rtd/planner/trajopt/ScipyOptimizationEngine.py +++ b/rtd/planner/trajopt/ScipyOptimizationEngine.py @@ -2,10 +2,7 @@ from scipy.optimize import minimize import numpy as np from typing import Callable -from nptyping import NDArray, Shape, Float64 - -# type hinting -RowVec = NDArray[Shape['N'], Float64] +from rtd.util.mixins.Typings import Vecnp @@ -20,19 +17,19 @@ def __init__(self, trajOptProps: TrajOptProps, **options): self.trajOptProps: TrajOptProps = trajOptProps - def performOptimization(self, initialGuess: RowVec, objectiveCallback: Callable, - constraintCallback: Callable, bounds: dict) -> tuple[bool, RowVec, float]: + def performOptimization(self, initialGuess: Vecnp, objectiveCallback: Callable, + constraintCallback: Callable, bounds: dict) -> tuple[bool, Vecnp, float]: ''' Use scipy solve to perform the optimization Arguments: - initialGuess: An initial guess RowVec used for the optimization. May not be the correct size + initialGuess: An initial guess Vecnp used for the optimization. May not be the correct size objectiveCallback: A callback for the objective function of this specific optimization constraintCallback: A callback for the nonlinear constraints, where the return time is expected to be [c, ceq, gc, gceq]. bounds: A dict containing input and output bounds Returns: - (success: bool, parameters: RowVec, cost: float): `success` + (success: bool, parameters: Vecnp, cost: float): `success` is if the optimization was successful or didn't time out. `parameters` are the trajectory parameters to use. `cost` is the final cost for the parameters found diff --git a/rtd/util/mixins/Typings.py b/rtd/util/mixins/Typings.py index c1f8596..a504c0a 100644 --- a/rtd/util/mixins/Typings.py +++ b/rtd/util/mixins/Typings.py @@ -1,8 +1,8 @@ from nptyping import NDArray, Shape, Float # 1D float vector -Vecnp = NDArray[Shape['N'], Float] #: 1D NDArray -Vec = list[float] | Vecnp #: 1D list or NDArray +Vecnp = NDArray[Shape['N'], Float] #: 1D NDArray +Vec = list[float] | Vecnp #: 1D list or NDArray # Bounds Boundsnp = NDArray[Shape['2,N'], Float] #: list of bounds (a 2-row NDArray) @@ -10,5 +10,5 @@ Bounds = list[Bound] | Boundsnp #: list of bounds (a 2-row list or NDArray) # Matrix -Matnp = NDArray[Shape, Float] #: 2D NDArray -Mat = list[list[float]] | Matnp #: 2D list of NDArray \ No newline at end of file +Matnp = NDArray[Shape['N,M'], Float] #: 2D NDArray +Mat = list[list[float]] | Matnp #: 2D list of NDArray \ No newline at end of file