data/panda/panda_on_rail.urdf

@@ -15,12 +15,12 @@
   <link name="track_y">
     <visual>
       <geometry>
-        <box size="0.1 1 0.1"/>
+        <box size="0.1 1 0.2"/>
       </geometry>
     </visual>
   </link>
   <joint name="move_x" type="prismatic">
-    <origin rpy="0 0 0" xyz="0 0 0.01"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="track_x"/>
     <child link="track_y"/>
     <axis xyz="1 0 0"/>
@@ -39,7 +39,7 @@
     </collision>
   </link>
   <joint name="move_y" type="prismatic">
-    <origin rpy="0 0 0" xyz="0 0 0.01"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="track_y"/>
     <child link="panda_link0"/>
     <axis xyz="0 1 0"/>

include/mplib/collision_detection/collision_matrix.h

@@ -154,8 +154,7 @@ class AllowedCollisionMatrix {
   void clear();
 
   /**
-   * Get sorted names of all existing elements (including
-   * default_entries_)
+   * Get sorted names of all existing elements (including ``default_entries_``)
    */
   std::vector<std::string> getAllEntryNames() const;
 

include/mplib/planning_world.h

@@ -98,7 +98,7 @@ class PlanningWorldTpl {
   void addArticulation(const ArticulatedModelPtr &model, bool planned = false);
 
   /**
-   * Removes the articulation with given name if exists. Updates acm_
+   * Removes the articulation with given name if exists. Updates ``acm_``
    *
    * @param name: name of the articulated model
    * @return: ``true`` if success, ``false`` if articulation with given name does not
@@ -177,7 +177,7 @@ class PlanningWorldTpl {
 
   /**
    * Removes (and detaches) the collision object with given name if exists.
-   * Updates acm_
+   * Updates ``acm_``
    *
    * @param name: name of the non-articulated collision object
    * @return: ``true`` if success, ``false`` if the non-articulated object
@@ -211,7 +211,7 @@ class PlanningWorldTpl {
    * at its current pose. If the object is currently attached, disallow collision
    * between the object and previous touch_links.
    *
-   * Updates acm_ to allow collisions between attached object and touch_links.
+   * Updates ``acm_`` to allow collisions between attached object and touch_links.
    *
    * @param name: name of the non-articulated object to attach
    * @param art_name: name of the planned articulation to attach to
@@ -229,10 +229,10 @@ class PlanningWorldTpl {
    * sets touch_links as the name of self links that collide with the object
    * in the current state.
    *
-   * Updates acm_ to allow collisions between attached object and touch_links.
+   * Updates ``acm_`` to allow collisions between attached object and touch_links.
    *
    * If the object is already attached, the touch_links of the attached object
-   * is preserved and acm_ remains unchanged.
+   * is preserved and ``acm_`` remains unchanged.
    *
    * @param name: name of the non-articulated object to attach
    * @param art_name: name of the planned articulation to attach to
@@ -247,7 +247,7 @@ class PlanningWorldTpl {
    * at given pose. If the object is currently attached, disallow collision
    * between the object and previous touch_links.
    *
-   * Updates acm_ to allow collisions between attached object and touch_links.
+   * Updates ``acm_`` to allow collisions between attached object and touch_links.
    *
    * @param name: name of the non-articulated object to attach
    * @param art_name: name of the planned articulation to attach to
@@ -266,10 +266,10 @@ class PlanningWorldTpl {
    * sets touch_links as the name of self links that collide with the object
    * in the current state.
    *
-   * Updates acm_ to allow collisions between attached object and touch_links.
+   * Updates ``acm_`` to allow collisions between attached object and touch_links.
    *
    * If the object is already attached, the touch_links of the attached object
-   * is preserved and acm_ remains unchanged.
+   * is preserved and ``acm_`` remains unchanged.
    *
    * @param name: name of the non-articulated object to attach
    * @param art_name: name of the planned articulation to attach to
@@ -284,42 +284,35 @@ class PlanningWorldTpl {
   /**
    * Attaches given object (w/ p_geom) to specified link of articulation at given pose.
    * This is done by removing the object and then adding and attaching object.
-   * As a result, all previous acm_ entries with the object are removed
+   * As a result, all previous ``acm_`` entries with the object are removed
    *
    * @param name: name of the non-articulated object to attach
-   * @param p_geom: pointer to a CollisionGeometry object
+   * @param obj_geom: pointer to a CollisionGeometry object
    * @param art_name: name of the planned articulation to attach to
    * @param link_id: index of the link of the planned articulation to attach to
    * @param pose: attached pose (relative pose from attached link to object)
    * @param touch_links: link names that the attached object touches
    */
-  void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom,
+  void attachObject(const std::string &name, const CollisionGeometryPtr &obj_geom,
                     const std::string &art_name, int link_id, const Pose<S> &pose,
                     const std::vector<std::string> &touch_links);
 
   /**
    * Attaches given object (w/ p_geom) to specified link of articulation at given pose.
    * This is done by removing the object and then adding and attaching object.
-   * As a result, all previous acm_ entries with the object are removed.
+   * As a result, all previous ``acm_`` entries with the object are removed.
    * Automatically sets touch_links as the name of self links
    * that collide with the object in the current state (auto touch_links).
    *
    * @param name: name of the non-articulated object to attach
-   * @param p_geom: pointer to a CollisionGeometry object
+   * @param obj_geom: pointer to a CollisionGeometry object
    * @param art_name: name of the planned articulation to attach to
    * @param link_id: index of the link of the planned articulation to attach to
    * @param pose: attached pose (relative pose from attached link to object)
    */
-  void attachObject(const std::string &name, const CollisionGeometryPtr &p_geom,
+  void attachObject(const std::string &name, const CollisionGeometryPtr &obj_geom,
                     const std::string &art_name, int link_id, const Pose<S> &pose);
 
-  /**
-   * Attaches the object the articulation is currently touching
-   * 
-   * @param art_name name of the planned articulation to attach to
-   */
-  void attachCurrentlyTouchingObject(const std::string &art_name);
-
   /**
    * Attaches given sphere to specified link of articulation (auto touch_links)
    *
@@ -356,7 +349,7 @@ class PlanningWorldTpl {
 
   /**
    * Detaches object with given name.
-   * Updates acm_ to disallow collision between the object and touch_links.
+   * Updates ``acm_`` to disallow collision between the object and touch_links.
    *
    * @param name: name of the non-articulated object to detach
    * @param also_remove: whether to also remove object from world
@@ -366,8 +359,8 @@ class PlanningWorldTpl {
   bool detachObject(const std::string &name, bool also_remove = false);
 
   /**
-   * Detaches all attached objects. Updates acm_ to disallow collision between
-   * the object and touch_links.
+   * Detaches all attached objects.
+   * Updates ``acm_`` to disallow collision between the object and touch_links.
    *
    * @param also_remove: whether to also remove objects from world
    * @return: ``true`` if success, ``false`` if there are no attached objects

mplib/examples/collision_avoidance.py

@@ -58,6 +58,28 @@ def __init__(self):
 
         planning_world = SapienPlanningWorld(self.scene, [self.robot])
         self.planner = SapienPlanner(planning_world, "panda_hand")
+        # self.planner.IK(
+        #     goal_pose=Pose([0.7, 0, 0.32], [0, 1, 0, 0]),
+        #     start_qpos=np.array([0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]),
+        #     verbose=True,
+        # )
+        # # self.setup_planner(
+        # #     urdf_path="./data/panda/panda.urdf", srdf_path="./data/panda/panda.srdf"
+        # # )
+        # # self.planner.robot.set_qpos(init_qpos, full=True)
+        # # for obj in self.planner.robot.get_fcl_model().get_collision_objects():
+        # #     print(obj.name, obj.pose)
+        # # for i, link in enumerate(
+        # #     self.planner.robot.get_pinocchio_model().get_link_names()
+        # # ):
+        # #     print(link, self.planner.robot.get_pinocchio_model().get_link_pose(i))
+        # # for obj in self.planner2.robot.get_fcl_model().get_collision_objects():
+        # #     print(obj.name, obj.pose)
+        # # for i, link in enumerate(
+        # #     self.planner2.robot.get_pinocchio_model().get_link_names()
+        # # ):
+        # #     print(link, self.planner2.robot.get_pinocchio_model().get_link_pose(i))
+        # exit(0)
 
     def demo(self, with_screw=True, use_attach=True):
         """

mplib/pymp/__init__.pyi

@@ -420,7 +420,7 @@ class PlanningWorld:
         its current pose. If the object is currently attached, disallow collision
         between the object and previous touch_links.
 
-        Updates acm_ to allow collisions between attached object and touch_links.
+        Updates ``acm_`` to allow collisions between attached object and touch_links.
 
         :param name: name of the non-articulated object to attach
         :param art_name: name of the planned articulation to attach to
@@ -437,10 +437,10 @@ class PlanningWorld:
         touch_links as the name of self links that collide with the object in the
         current state.
 
-        Updates acm_ to allow collisions between attached object and touch_links.
+        Updates ``acm_`` to allow collisions between attached object and touch_links.
 
         If the object is already attached, the touch_links of the attached object is
-        preserved and acm_ remains unchanged.
+        preserved and ``acm_`` remains unchanged.
 
         :param name: name of the non-articulated object to attach
         :param art_name: name of the planned articulation to attach to
@@ -457,7 +457,7 @@ class PlanningWorld:
         given pose. If the object is currently attached, disallow collision between the
         object and previous touch_links.
 
-        Updates acm_ to allow collisions between attached object and touch_links.
+        Updates ``acm_`` to allow collisions between attached object and touch_links.
 
         :param name: name of the non-articulated object to attach
         :param art_name: name of the planned articulation to attach to
@@ -475,10 +475,10 @@ class PlanningWorld:
         touch_links as the name of self links that collide with the object in the
         current state.
 
-        Updates acm_ to allow collisions between attached object and touch_links.
+        Updates ``acm_`` to allow collisions between attached object and touch_links.
 
         If the object is already attached, the touch_links of the attached object is
-        preserved and acm_ remains unchanged.
+        preserved and ``acm_`` remains unchanged.
 
         :param name: name of the non-articulated object to attach
         :param art_name: name of the planned articulation to attach to
@@ -491,7 +491,7 @@ class PlanningWorld:
     def attach_object(
         self,
         name: str,
-        p_geom: collision_detection.fcl.CollisionGeometry,
+        obj_geom: collision_detection.fcl.CollisionGeometry,
         art_name: str,
         link_id: int,
         pose: Pose,
@@ -500,10 +500,10 @@ class PlanningWorld:
         """
         Attaches given object (w/ p_geom) to specified link of articulation at given
         pose. This is done by removing the object and then adding and attaching object.
-        As a result, all previous acm_ entries with the object are removed
+        As a result, all previous ``acm_`` entries with the object are removed
 
         :param name: name of the non-articulated object to attach
-        :param p_geom: pointer to a CollisionGeometry object
+        :param obj_geom: pointer to a CollisionGeometry object
         :param art_name: name of the planned articulation to attach to
         :param link_id: index of the link of the planned articulation to attach to
         :param pose: attached pose (relative pose from attached link to object)
@@ -513,20 +513,20 @@ class PlanningWorld:
     def attach_object(
         self,
         name: str,
-        p_geom: collision_detection.fcl.CollisionGeometry,
+        obj_geom: collision_detection.fcl.CollisionGeometry,
         art_name: str,
         link_id: int,
         pose: Pose,
     ) -> None:
         """
         Attaches given object (w/ p_geom) to specified link of articulation at given
         pose. This is done by removing the object and then adding and attaching object.
-        As a result, all previous acm_ entries with the object are removed.
+        As a result, all previous ``acm_`` entries with the object are removed.
         Automatically sets touch_links as the name of self links that collide with the
         object in the current state (auto touch_links).
 
         :param name: name of the non-articulated object to attach
-        :param p_geom: pointer to a CollisionGeometry object
+        :param obj_geom: pointer to a CollisionGeometry object
         :param art_name: name of the planned articulation to attach to
         :param link_id: index of the link of the planned articulation to attach to
         :param pose: attached pose (relative pose from attached link to object)
@@ -572,10 +572,18 @@ class PlanningWorld:
         :param request: collision request params.
         :return: List of ``WorldCollisionResult`` objects
         """
+    def detach_all_objects(self, also_remove: bool = False) -> bool:
+        """
+        Detaches all attached objects. Updates ``acm_`` to disallow collision between
+        the object and touch_links.
+
+        :param also_remove: whether to also remove objects from world
+        :return: ``True`` if success, ``False`` if there are no attached objects
+        """
     def detach_object(self, name: str, also_remove: bool = False) -> bool:
         """
-        Detaches object with given name. Updates acm_ to disallow collision between the
-        object and touch_links.
+        Detaches object with given name. Updates ``acm_`` to disallow collision between
+        the object and touch_links.
 
         :param name: name of the non-articulated object to detach
         :param also_remove: whether to also remove object from world
@@ -714,7 +722,7 @@ class PlanningWorld:
         """
     def remove_articulation(self, name: str) -> bool:
         """
-        Removes the articulation with given name if exists. Updates acm_
+        Removes the articulation with given name if exists. Updates ``acm_``
 
         :param name: name of the articulated model
         :return: ``True`` if success, ``False`` if articulation with given name does not
@@ -723,7 +731,7 @@ class PlanningWorld:
     def remove_object(self, name: str) -> bool:
         """
         Removes (and detaches) the collision object with given name if exists. Updates
-        acm_
+        ``acm_``
 
         :param name: name of the non-articulated collision object
         :return: ``True`` if success, ``False`` if the non-articulated object with given

mplib/pymp/collision_detection/__init__.pyi

@@ -77,7 +77,7 @@ class AllowedCollisionMatrix:
         """
     def get_all_entry_names(self) -> list[str]:
         """
-        Get sorted names of all existing elements (including default_entries_)
+        Get sorted names of all existing elements (including ``default_entries_``)
         """
     def get_allowed_collision(self, name1: str, name2: str) -> AllowedCollision | None:
         """