From fd53ea6d7ad31e43d2f0f22a80af09aef7ba689c Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 17:42:38 -0700 Subject: [PATCH 1/2] Override SapienPlanningWorld.attach_sphere/box/mesh to accept SAPIEN objects --- mplib/sapien_utils/conversion.py | 235 +++++++++++++++++++++++-------- 1 file changed, 177 insertions(+), 58 deletions(-) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 8f7bab0..b58a3d8 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,6 +1,6 @@ from __future__ import annotations -from typing import Optional, Sequence +from typing import Literal, Optional, Sequence import numpy as np from sapien import Entity, Scene @@ -288,13 +288,77 @@ def _get_collision_obj( "(The scene might have changed since last update)" ) + @staticmethod + def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: + """ + Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. + All shapes in the returned FCLObject are already set at their world poses. + + :param comp: a SAPIEN physx.PhysxRigidBaseComponent. + :return: an FCLObject containing all collision shapes in the Physx component. + If the component has no collision shapes, return ``None``. + """ + shapes: list[CollisionObject] = [] + shape_poses: list[Pose] = [] + for shape in comp.collision_shapes: + shape_poses.append(shape.local_pose) # type: ignore + + if isinstance(shape, PhysxCollisionShapeBox): + c_geom = Box(side=shape.half_size * 2) + elif isinstance(shape, PhysxCollisionShapeCapsule): + c_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Capsule has x-axis along capsule height + # FCL Capsule has z-axis along capsule height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapeConvexMesh): + assert np.allclose( + shape.scale, 1.0 + ), f"Not unit scale {shape.scale}, need to rescale vertices?" + c_geom = Convex(vertices=shape.vertices, faces=shape.triangles) + elif isinstance(shape, PhysxCollisionShapeCylinder): + c_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) + # NOTE: physx Cylinder has x-axis along cylinder height + # FCL Cylinder has z-axis along cylinder height + shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) + elif isinstance(shape, PhysxCollisionShapePlane): + # PhysxCollisionShapePlane are actually a halfspace + # https://nvidia-omniverse.github.io/PhysX/physx/5.3.1/docs/Geometry.html#planes + # PxPlane's Pose determines its normal and offert (normal is +x) + n = shape_poses[-1].to_transformation_matrix()[:3, 0] + d = n.dot(shape_poses[-1].p) + c_geom = Halfspace(n=n, d=d) + shape_poses[-1] = Pose() + elif isinstance(shape, PhysxCollisionShapeSphere): + c_geom = Sphere(radius=shape.radius) + elif isinstance(shape, PhysxCollisionShapeTriangleMesh): + c_geom = BVHModel() + c_geom.begin_model() + c_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) # type: ignore + c_geom.end_model() + else: + raise TypeError(f"Unknown shape type: {type(shape)}") + shapes.append(CollisionObject(c_geom)) + + if len(shapes) == 0: + return None + + return FCLObject( + comp.name + if isinstance(comp, PhysxArticulationLinkComponent) + else convert_object_name(comp.entity), + comp.entity.pose, # type: ignore + shapes, + shape_poses, + ) + + # ----- Overloaded PlanningWorld methods to accept SAPIEN objects ----- # def attach_object( # type: ignore self, obj: Entity | str, articulation: PhysxArticulation | str, link: PhysxArticulationLinkComponent | int, - *, pose: Optional[Pose] = None, + *, touch_links: Optional[list[PhysxArticulationLinkComponent | str]] = None, obj_geom: Optional[CollisionGeometry] = None, ) -> None: @@ -382,68 +446,123 @@ def detach_object(self, obj: Entity | str, also_remove: bool = False) -> bool: obj = convert_object_name(obj) return super().detach_object(obj, also_remove=also_remove) - @staticmethod - def convert_physx_component(comp: PhysxRigidBaseComponent) -> FCLObject | None: + def attach_sphere( # type: ignore + self, + radius: float, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + ) -> None: """ - Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. - All shapes in the returned FCLObject are already set at their world poses. + Attaches given sphere to specified link of articulation (auto touch_links) - :param comp: a SAPIEN physx.PhysxRigidBaseComponent. - :return: an FCLObject containing all collision shapes in the Physx component. - If the component has no collision shapes, return ``None``. + :param radius: sphere radius + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_sphere() + + method + .. automethod:: mplib.PlanningWorld.attach_sphere + .. raw:: html +
""" - shapes: list[CollisionObject] = [] - shape_poses: list[Pose] = [] - for shape in comp.collision_shapes: - shape_poses.append(shape.local_pose) # type: ignore + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_sphere(radius, articulation, link, pose) - if isinstance(shape, PhysxCollisionShapeBox): - c_geom = Box(side=shape.half_size * 2) - elif isinstance(shape, PhysxCollisionShapeCapsule): - c_geom = Capsule(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Capsule has x-axis along capsule height - # FCL Capsule has z-axis along capsule height - shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) - elif isinstance(shape, PhysxCollisionShapeConvexMesh): - assert np.allclose( - shape.scale, 1.0 - ), f"Not unit scale {shape.scale}, need to rescale vertices?" - c_geom = Convex(vertices=shape.vertices, faces=shape.triangles) - elif isinstance(shape, PhysxCollisionShapeCylinder): - c_geom = Cylinder(radius=shape.radius, lz=shape.half_length * 2) - # NOTE: physx Cylinder has x-axis along cylinder height - # FCL Cylinder has z-axis along cylinder height - shape_poses[-1] *= Pose(q=euler2quat(0, np.pi / 2, 0)) - elif isinstance(shape, PhysxCollisionShapePlane): - # PhysxCollisionShapePlane are actually a halfspace - # https://nvidia-omniverse.github.io/PhysX/physx/5.3.1/docs/Geometry.html#planes - # PxPlane's Pose determines its normal and offert (normal is +x) - n = shape_poses[-1].to_transformation_matrix()[:3, 0] - d = n.dot(shape_poses[-1].p) - c_geom = Halfspace(n=n, d=d) - shape_poses[-1] = Pose() - elif isinstance(shape, PhysxCollisionShapeSphere): - c_geom = Sphere(radius=shape.radius) - elif isinstance(shape, PhysxCollisionShapeTriangleMesh): - c_geom = BVHModel() - c_geom.begin_model() - c_geom.add_sub_model(vertices=shape.vertices, faces=shape.triangles) # type: ignore - c_geom.end_model() - else: - raise TypeError(f"Unknown shape type: {type(shape)}") - shapes.append(CollisionObject(c_geom)) + 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, + pose: Pose, + ) -> None: + """ + Attaches given box to specified link of articulation (auto touch_links) - if len(shapes) == 0: - return None + :param size: box side length + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) - return FCLObject( - comp.name - if isinstance(comp, PhysxArticulationLinkComponent) - else convert_object_name(comp.entity), - comp.entity.pose, # type: ignore - shapes, - shape_poses, - ) + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_box() + + method + .. automethod:: mplib.PlanningWorld.attach_box + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_box(size, articulation, link, pose) # type: ignore + + def attach_mesh( # type: ignore + self, + mesh_path: str, + articulation: PhysxArticulation | str, + link: PhysxArticulationLinkComponent | int, + pose: Pose, + *, + convex: bool = False, + ) -> None: + """ + Attaches given mesh to specified link of articulation (auto touch_links) + + :param mesh_path: path to a mesh file + :param articulation: the planned articulation (or its name) to attach to + :param link: the link of the planned articulation (or its index) to attach to + :param pose: attached pose (relative pose from attached link to object) + :param convex: whether to load mesh as a convex mesh. Default: ``False``. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.attach_mesh() + + method + .. automethod:: mplib.PlanningWorld.attach_mesh + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + if isinstance(link, PhysxArticulationLinkComponent): + link = ( + self.get_articulation(articulation) + .get_pinocchio_model() + .get_link_names() + .index(link.name) + ) + super().attach_mesh(mesh_path, articulation, link, pose, convex=convex) class SapienPlanner(Planner): From 382f663a69088b2a7a3bcfd9f8e958dbb88b9cf8 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 19 Apr 2024 17:58:23 -0700 Subject: [PATCH 2/2] Override 3 more SapienPlanningWorld methods to accept SAPIEN objects * is_articulation_planned * set_articulation_planned * is_object_attached --- mplib/sapien_utils/conversion.py | 71 ++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index b58a3d8..bd4b607 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -352,6 +352,77 @@ 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 + """ + Check whether the given articulation is being planned + + :param articulation: the articulation (or its name) to check + :return: ``True`` if the articulation is being planned, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.is_articulation_planned() + + method + .. automethod:: mplib.PlanningWorld.is_articulation_planned + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + return super().is_articulation_planned(articulation) + + def set_articulation_planned( # type: ignore + self, articulation: PhysxArticulation | str, planned: bool + ) -> None: + """ + Sets the given articulation as being planned or not + + :param articulation: the articulation (or its name) + :param planned: whether the articulation should be planned + + .. raw:: html + +
+ Overloaded + + PlanningWorld.set_articulation_planned() + + method + .. automethod:: mplib.PlanningWorld.set_articulation_planned + .. raw:: html +
+ """ + if isinstance(articulation, PhysxArticulation): + articulation = convert_object_name(articulation) + super().set_articulation_planned(articulation, planned) + + def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore + """ + Check whether the given non-articulated object is attached + + :param obj: the non-articulated object (or its name) to check + :return: ``True`` if the articulation is attached, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.is_object_attached() + + method + .. automethod:: mplib.PlanningWorld.is_object_attached + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().is_object_attached(obj) + def attach_object( # type: ignore self, obj: Entity | str,