From cd58e01e885c1ca158fb52492fd24e83c43a737a Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Mon, 22 Apr 2024 10:38:04 -0700 Subject: [PATCH] more sapien overloaded functions --- include/mplib/planning_world.h | 12 ++++ mplib/sapien_utils/conversion.py | 107 ++++++++++++++++++++++++++++++- pybind/pybind_planning_world.cpp | 3 + 3 files changed, 120 insertions(+), 2 deletions(-) diff --git a/include/mplib/planning_world.h b/include/mplib/planning_world.h index 008f339..ddf9149 100644 --- a/include/mplib/planning_world.h +++ b/include/mplib/planning_world.h @@ -384,6 +384,18 @@ class PlanningWorldTpl { /// @brief Get the current allowed collision matrix AllowedCollisionMatrixPtr getAllowedCollisionMatrix() const { return acm_; } + /** + * Set the allowed collision. For more comprehensive API, please get the + * ``AllowedCollisionMatrix`` object and use its methods. + * + * @param name1: name of the first object + * @param name2: name of the second object + */ + void setAllowedCollision(const std::string &name1, const std::string &name2, + bool allowed) { + acm_->setEntry(name1, name2, allowed); + } + /** * Check if the current state is in collision (with the environment or self * collision). diff --git a/mplib/sapien_utils/conversion.py b/mplib/sapien_utils/conversion.py index 840cb0d..8e159b8 100644 --- a/mplib/sapien_utils/conversion.py +++ b/mplib/sapien_utils/conversion.py @@ -1,5 +1,6 @@ from __future__ import annotations +import warnings from typing import Literal, Optional, Sequence import numpy as np @@ -165,9 +166,11 @@ def update_from_simulation(self, *, update_attached_object: bool = True) -> None ) > 0 ): - raise RuntimeError( + warnings.warn( f"Entity {entity.name} not found in PlanningWorld! " - "The scene might have changed since last update." + "The scene might have changed since last update. " + "Use PlanningWorld.add_object() to add the object.", + stacklevel=2, ) def check_collision_between( @@ -402,6 +405,78 @@ 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: + """ + Check whether the given non-articulated object exists. + + :param obj: the object (or its name) to check + + :return: ``True`` if the object exists, ``False`` otherwise. + + .. raw:: html + +
+ Overloaded + + PlanningWorld.has_object() + + method + .. automethod:: mplib.PlanningWorld.has_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + return super().has_object(obj) + + def add_object(self, obj: FCLObject | Entity) -> None: + """ + Adds a non-articulated object to the PlanningWorld. + + :param obj: the non-articulated object to add + + .. raw:: html + +
+ Overloaded + + PlanningWorld.add_object() + + method + .. automethod:: mplib.PlanningWorld.add_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = self.convert_physx_component(obj) + if obj is not None: + super().add_object(obj) + + def remove_object(self, obj: Entity | str) -> None: + """ + Removes a non-articulated object from the PlanningWorld. + + :param obj: the non-articulated object (or its name) to remove + + .. raw:: html + +
+ Overloaded + + PlanningWorld.remove_object() + + method + .. automethod:: mplib.PlanningWorld.remove_object + :no-index: + .. raw:: html +
+ """ + if isinstance(obj, Entity): + obj = convert_object_name(obj) + super().remove_object(obj) + def is_object_attached(self, obj: Entity | str) -> bool: # type: ignore """ Check whether the given non-articulated object is attached @@ -643,6 +718,34 @@ def attach_mesh( # type: ignore ) super().attach_mesh(mesh_path, articulation, link, pose, convex=convex) + def set_allowed_collision( + self, obj1: Entity | str, obj2: Entity | str, allowed: bool + ) -> None: + """ + Set allowed collision between two objects. + + :param obj1: the first object (or its name) + :param obj2: the second object (or its name) + + .. raw:: html + +
+ Overloaded + + PlanningWorld.set_allowed_collision() + + method + .. automethod:: mplib.PlanningWorld.set_allowed_collision + :no-index: + .. raw:: html +
+ """ + if isinstance(obj1, Entity): + obj1 = convert_object_name(obj1) + if isinstance(obj2, Entity): + obj2 = convert_object_name(obj2) + super().set_allowed_collision(obj1, obj2, allowed) + class SapienPlanner(Planner): def __init__( diff --git a/pybind/pybind_planning_world.cpp b/pybind/pybind_planning_world.cpp index 11c3ad9..a0684b3 100644 --- a/pybind/pybind_planning_world.cpp +++ b/pybind/pybind_planning_world.cpp @@ -137,6 +137,9 @@ void build_pyplanning_world(py::module &pymp) { .def("get_allowed_collision_matrix", &PlanningWorld::getAllowedCollisionMatrix, DOC(mplib, PlanningWorldTpl, getAllowedCollisionMatrix)) + .def("set_allowed_collision", &PlanningWorld::setAllowedCollision, + py::arg("name1"), py::arg("name2"), py::arg("allowed"), + DOC(mplib, PlanningWorldTpl, setAllowedCollision)) .def("is_state_colliding", &PlanningWorld::isStateColliding, DOC(mplib, PlanningWorldTpl, isStateColliding))