From da9e061418f6aa2a98cf2b3e08a825c53756ba0d Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 21 Oct 2024 13:16:04 -0700 Subject: [PATCH] remove bar separated typing hints; ruff format --- mplib/planner.py | 28 +++++++------ mplib/sapien_utils/conversion.py | 72 +++++++++++++++++--------------- mplib/urdf_utils.py | 6 ++- 3 files changed, 58 insertions(+), 48 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index 19cb387..595fff9 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -2,7 +2,7 @@ from collections.abc import Callable from pathlib import Path -from typing import Literal, Optional, Sequence +from typing import Literal, Optional, Sequence, Union import numpy as np import toppra as ta @@ -24,16 +24,16 @@ class Planner: # constructor ankor def __init__( self, - urdf: str | Path, + urdf: Union[str, Path], move_group: str, *, - srdf: Optional[str | Path] = None, + srdf: Union[str, Path, None] = None, new_package_keyword: str = "", use_convex: bool = False, user_link_names: Sequence[str] = [], user_joint_names: Sequence[str] = [], - joint_vel_limits: Optional[Sequence[float] | np.ndarray] = None, - joint_acc_limits: Optional[Sequence[float] | np.ndarray] = None, + joint_vel_limits: Union[Sequence[float], np.ndarray, None] = None, + joint_acc_limits: Union[Sequence[float], np.ndarray, None] = None, objects: list[FCLObject] = [], # noqa: B006 verbose: bool = False, ): @@ -240,13 +240,13 @@ def IK( self, goal_pose: Pose, start_qpos: np.ndarray, - mask: Optional[Sequence[bool] | np.ndarray] = None, + mask: Union[Sequence[bool], np.ndarray, None] = None, *, n_init_qpos: int = 20, threshold: float = 1e-3, return_closest: bool = False, verbose: bool = False, - ) -> tuple[str, list[np.ndarray] | np.ndarray | None]: + ) -> tuple[str, Union[list[np.ndarray], np.ndarray, None]]: """ Compute inverse kinematics @@ -460,8 +460,10 @@ def update_attached_sphere( def update_attached_box( self, - size: Sequence[float] - | np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], + size: Union[ + Sequence[float], + np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], + ], pose: Pose, art_name=None, link_id=-1, @@ -544,7 +546,7 @@ def plan_qpos( constraint_jacobian: Optional[Callable[[np.ndarray, np.ndarray], None]] = None, constraint_tolerance: float = 1e-3, verbose: bool = False, - ) -> dict[str, str | np.ndarray | np.float64]: + ) -> dict[str, Union[str, np.ndarray, np.float64]]: """ Plan a path from a specified joint position to a goal pose @@ -630,7 +632,7 @@ def plan_pose( self, goal_pose: Pose, current_qpos: np.ndarray, - mask: Optional[list[bool] | np.ndarray] = None, + mask: Union[list[bool], np.ndarray, None] = None, *, time_step: float = 0.1, rrt_range: float = 0.1, @@ -642,7 +644,7 @@ def plan_pose( constraint_jacobian: Optional[Callable] = None, constraint_tolerance: float = 1e-3, verbose: bool = False, - ) -> dict[str, str | np.ndarray | np.float64]: + ) -> dict[str, Union[str, np.ndarray, np.float64]]: """ plan from a start configuration to a goal pose of the end-effector @@ -708,7 +710,7 @@ def plan_screw( time_step: float = 0.1, wrt_world: bool = True, verbose: bool = False, - ) -> dict[str, str | np.ndarray | np.float64]: + ) -> dict[str, Union[str, np.ndarray, np.float64]]: # plan_screw ankor end """ Plan from a start configuration to a goal pose of the end-effector using diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index a3f5a23..c47d461 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,7 +1,7 @@ from __future__ import annotations import warnings -from typing import Literal, Optional, Sequence +from typing import Literal, Optional, Sequence, Union import numpy as np from sapien import Entity, Scene @@ -51,7 +51,7 @@ # TODO: link names? -def convert_object_name(obj: PhysxArticulation | Entity) -> str: +def convert_object_name(obj: Union[PhysxArticulation, Entity]) -> str: """ Constructs a unique name for the corresponding mplib object. This is necessary because mplib objects assume unique names. @@ -183,8 +183,8 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None def check_collision_between( self, - obj_A: PhysxArticulation | Entity, - obj_B: PhysxArticulation | Entity, + obj_A: Union[PhysxArticulation, Entity], + obj_B: Union[PhysxArticulation, Entity], *, acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), # noqa: B008 ) -> list[WorldCollisionResult]: @@ -231,12 +231,12 @@ def check_collision_between( def distance_between( self, - obj_A: PhysxArticulation | Entity, - obj_B: PhysxArticulation | Entity, + obj_A: Union[PhysxArticulation, Entity], + obj_B: Union[PhysxArticulation, Entity], *, acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), # noqa: B008 return_distance_only: bool = True, - ) -> WorldDistanceResult | float: + ) -> Union[WorldDistanceResult, float]: """ Check distance-to-collision between two objects, which can either be a PhysxArticulation or an Entity. @@ -281,8 +281,8 @@ def distance_between( def _get_collision_obj( self, - obj: PhysxArticulation | Entity, - ) -> FCLModel | FCLObject | None: + obj: Union[PhysxArticulation, Entity], + ) -> Union[FCLModel, FCLObject, None]: """Helper function to get mplib collision object from sapien object""" if isinstance(obj, PhysxArticulation) and ( articulation := self.get_articulation(convert_object_name(obj)) @@ -300,7 +300,9 @@ def _get_collision_obj( ) @staticmethod - def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: + def convert_physx_component( + comp: PhysxRigidBaseComponent, + ) -> Union[FCLObject, None]: """ Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. All shapes in the returned FCLObject are already set at their world poses. @@ -363,7 +365,9 @@ def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: ) # ----- Overloaded PlanningWorld methods to accept SAPIEN objects ----- # - def is_articulation_planned(self, articulation: PhysxArticulation | str) -> bool: # type: ignore + def is_articulation_planned( + self, articulation: Union[PhysxArticulation, str] + ) -> bool: # type: ignore """ Check whether the given articulation is being planned @@ -388,7 +392,7 @@ def is_articulation_planned(self, articulation: PhysxArticulation | str) -> bool return super().is_articulation_planned(articulation) def set_articulation_planned( # type: ignore - self, articulation: PhysxArticulation | str, planned: bool + self, articulation: Union[PhysxArticulation, str], planned: bool ) -> None: """ Sets the given articulation as being planned or not @@ -413,7 +417,7 @@ def set_articulation_planned( # type: ignore articulation = convert_object_name(articulation) super().set_articulation_planned(articulation, planned) - def has_object(self, obj: Entity | str) -> bool: + def has_object(self, obj: Union[Entity, str]) -> bool: """ Check whether the given non-articulated object exists. @@ -438,7 +442,7 @@ def has_object(self, obj: Entity | str) -> bool: obj = convert_object_name(obj) return super().has_object(obj) - def add_object(self, obj: FCLObject | Entity) -> None: + def add_object(self, obj: Union[FCLObject, Entity]) -> None: """ Adds a non-articulated object to the PlanningWorld. @@ -469,7 +473,7 @@ def add_object(self, obj: FCLObject | Entity) -> None: else: super().add_object(obj) - def remove_object(self, obj: Entity | str) -> None: + def remove_object(self, obj: Union[Entity, str]) -> None: """ Removes a non-articulated object from the PlanningWorld. @@ -492,7 +496,7 @@ def remove_object(self, obj: Entity | str) -> None: obj = convert_object_name(obj) super().remove_object(obj) - def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore + def is_object_attached(self, obj: Union[Entity, str]) -> bool: # type: ignore """ Check whether the given non-articulated object is attached @@ -518,12 +522,12 @@ def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore def attach_object( # type: ignore self, - obj: Entity | str, - articulation: PhysxArticulation | str, - link: PhysxArticulationLinkComponent | int, + obj: Union[Entity, str], + articulation: Union[PhysxArticulation, str], + link: Union[PhysxArticulationLinkComponent, int], pose: Optional[Pose] = None, *, - touch_links: Optional[list[PhysxArticulationLinkComponent | str]] = None, + touch_links: Optional[list[Union[PhysxArticulationLinkComponent, str]]] = None, obj_geom: Optional[CollisionGeometry] = None, ) -> None: """ @@ -585,7 +589,7 @@ def attach_object( # type: ignore super().attach_object(**kwargs) - def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: # type: ignore + def detach_object(self, obj: Union[Entity, str], also_remove: bool = False) -> bool: # type: ignore """ Detaches the given object. @@ -615,8 +619,8 @@ def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: def attach_sphere( # type: ignore self, radius: float, - articulation: PhysxArticulation | str, - link: PhysxArticulationLinkComponent | int, + articulation: Union[PhysxArticulation, str], + link: Union[PhysxArticulationLinkComponent, int], pose: Pose, ) -> None: """ @@ -653,10 +657,12 @@ def attach_sphere( # type: ignore def attach_box( # type: ignore self, - size: Sequence[float] - | np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], - articulation: PhysxArticulation | str, - link: PhysxArticulationLinkComponent | int, + size: Union[ + Sequence[float], + np.ndarray[tuple[Literal[3], Literal[1]], np.dtype[np.floating]], + ], + articulation: Union[PhysxArticulation, str], + link: Union[PhysxArticulationLinkComponent, int], pose: Pose, ) -> None: """ @@ -694,8 +700,8 @@ def attach_box( # type: ignore def attach_mesh( # type: ignore self, mesh_path: str, - articulation: PhysxArticulation | str, - link: PhysxArticulationLinkComponent | int, + articulation: Union[PhysxArticulation, str], + link: Union[PhysxArticulationLinkComponent, int], pose: Pose, *, convex: bool = False, @@ -735,8 +741,8 @@ def attach_mesh( # type: ignore def set_allowed_collision( self, - obj1: Entity | PhysxArticulationLinkComponent | str, - obj2: Entity | PhysxArticulationLinkComponent | str, + obj1: Union[Entity, PhysxArticulationLinkComponent, str], + obj2: Union[Entity, PhysxArticulationLinkComponent, str], allowed: bool, ) -> None: """ @@ -775,8 +781,8 @@ def __init__( sapien_planning_world: SapienPlanningWorld, move_group: str, *, - joint_vel_limits: Optional[Sequence[float] | np.ndarray] = None, - joint_acc_limits: Optional[Sequence[float] | np.ndarray] = None, + joint_vel_limits: Union[Sequence[float], np.ndarray, None] = None, + joint_acc_limits: Union[Sequence[float], np.ndarray, None] = None, ): """ Creates an mplib.planner.Planner from a SapienPlanningWorld. diff --git a/mplib/urdf_utils.py b/mplib/urdf_utils.py index 14d967b..318a3f8 100644 --- a/mplib/urdf_utils.py +++ b/mplib/urdf_utils.py @@ -7,6 +7,8 @@ from .collision_detection import AllowedCollisionMatrix from .pymp import ArticulatedModel +from typing import Union + def compute_default_collisions( robot: ArticulatedModel, *, num_samples=100000, verbose=False @@ -111,7 +113,7 @@ def compute_default_collisions( def replace_urdf_package_keyword( - urdf_path: str | Path, + urdf_path: Union[str, Path], new_package_keyword: str = "", ) -> Path: """ @@ -133,7 +135,7 @@ def replace_urdf_package_keyword( def generate_srdf( - urdf_path: str | Path, + urdf_path: Union[str, Path], new_package_keyword: str = "", *, num_samples=100000,