Merge pull request #84 from haosulab/example-update

Example update and two convenience functions

data/panda/panda_on_rail.urdf

@@ -15,12 +15,12 @@
   <link name="track_y">
     <visual>
       <geometry>
-        <box size="0.1 1 0.2"/>
+        <box size="0.1 1 0.1"/>
       </geometry>
     </visual>
   </link>
   <joint name="move_x" type="prismatic">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <origin rpy="0 0 0" xyz="0 0 0.01"/>
     <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"/>
+    <origin rpy="0 0 0" xyz="0 0 0.01"/>
     <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,33 +284,33 @@ 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);
 
   /**
@@ -349,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
@@ -358,6 +358,15 @@ 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.
+   *
+   * @param also_remove: whether to also remove objects from world
+   * @return: ``true`` if success, ``false`` if there are no attached objects
+   */
+  bool detachAllObjects(bool also_remove = false);
+
   /// @brief Prints global pose of all attached bodies
   void printAttachedBodyPose() const;
 
@@ -375,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).

mplib/examples/collision_avoidance.py

@@ -1,5 +1,7 @@
+import numpy as np
 import sapien.core as sapien
 
+from mplib import Pose
 from mplib.examples.demo_setup import DemoSetup
 from mplib.sapien_utils import SapienPlanner, SapienPlanningWorld
 
@@ -36,56 +38,45 @@ def __init__(self):
         # red box is the target we want to grab
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-        builder.add_box_visual(half_size=[0.02, 0.02, 0.06])
-        red_cube = builder.build(name="red_cube")
-        red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0])
+        self.red_cube = builder.build(name="red_cube")
+        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
 
         # green box is the landing pad on which we want to place the red box
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
-        builder.add_box_visual(half_size=[0.04, 0.04, 0.005])
+        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], material=[0, 1, 0])
         green_cube = builder.build(name="green_cube")
         green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
 
         # blue box is the obstacle we want to avoid
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
-        builder.add_box_visual(half_size=[0.05, 0.2, 0.1])
+        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], material=[0, 0, 1])
         blue_cube = builder.build(name="blue_cube")
         blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
 
         planning_world = SapienPlanningWorld(self.scene, [self.robot])
         self.planner = SapienPlanner(planning_world, "panda_hand")
+        # disable collision between the base and the ground
+        self.planner.planning_world.get_allowed_collision_matrix().set_entry(
+            "panda_link0", "ground_1", True
+        )
 
-    def add_point_cloud(self):
-        """We tell the planner about the obstacle through a point cloud"""
-        import trimesh
-
-        # add_point_cloud ankor
-        box = trimesh.creation.box([0.1, 0.4, 0.2])
-        points, _ = trimesh.sample.sample_surface(box, 1000)
-        points += [0.55, 0, 0.1]
-        self.planner.update_point_cloud(points, resolution=0.02)
-        # add_point_cloud ankor end
-
-    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
+    def demo(self, with_screw=True, use_attach=True):
         """
         We pick up the red box while avoiding the blue box and
         place it back down on top of the green box.
         """
-        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-
-        # tell the planner where the obstacle is
-        if use_point_cloud:
-            self.add_point_cloud()
+        pickup_pose = Pose([0.7, 0, 0.12], [0, 1, 0, 0])
+        delivery_pose = Pose([0.4, 0.3, 0.13], [0, 1, 0, 0])
 
         # move to the pickup pose
-        pickup_pose[2] += 0.2
+        pickup_pose.p[2] += 0.2
         # no need to check collision against attached object since nothing picked up yet
         self.move_to_pose(pickup_pose, with_screw)
         self.open_gripper()
-        pickup_pose[2] -= 0.12
+        pickup_pose.p[2] -= 0.12
         # no attach since nothing picked up yet
         self.move_to_pose(pickup_pose, with_screw)
         self.close_gripper()
@@ -94,24 +85,21 @@ def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
 
         # use_attach ankor
         if use_attach:
-            self.planner.update_attached_box(
-                [0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]
-            )
+            idx = self.planner.user_link_names.index("panda_hand")
+            self.planner.planning_world.attach_object(self.red_cube, self.robot, idx)
         # use_attach ankor end
 
         # move to the delivery pose
-        pickup_pose[2] += 0.12
+        pickup_pose.p[2] += 0.12
         self.move_to_pose(pickup_pose, with_screw)
-        delivery_pose[2] += 0.2
+        delivery_pose.p[2] += 0.2
         self.move_to_pose(delivery_pose, with_screw)
-        delivery_pose[2] -= 0.12
+        delivery_pose.p[2] -= 0.1
         self.move_to_pose(delivery_pose, with_screw)
         self.open_gripper()
-        delivery_pose[2] += 0.12
+        delivery_pose.p[2] += 0.12
         if use_attach:
-            ret = self.planner.detach_object(
-                f"robot_{self.planner.move_group_link_id}_box", also_remove=True
-            )
+            ret = self.planner.planning_world.detach_all_objects()
             assert ret, "object is not attached"
         self.move_to_pose(delivery_pose, with_screw)
 
@@ -126,4 +114,4 @@ def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
     the delivery pose
     """
     demo = PlanningDemo()
-    demo.demo(False, True, True)
+    demo.demo(False, True)

mplib/examples/constrained_planning.py

@@ -3,6 +3,7 @@
 import numpy as np
 import transforms3d
 
+from mplib import Pose
 from mplib.examples.demo_setup import DemoSetup
 
 
@@ -36,7 +37,7 @@ def get_eef_z(self):
         """Helper function for constraint"""
         ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
         ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
-        mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
+        mat = transforms3d.quaternions.quat2mat(ee_pose.q)
         return mat[:, 2]
 
     def make_f(self):
@@ -94,9 +95,9 @@ def demo(self):
         self.planner.robot.set_qpos(starting_qpos[:7])
         # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
         poses = [
-            [-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
-            [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
-            [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0],
+            Pose([-0.4, -0.3, 0.28], [0.0704682, -0.5356872, 0.8342834, 0.1097478]),
+            Pose([0.6, 0.1, 0.44], [0.0704682, -0.5356872, -0.8342834, -0.1097478]),
+            Pose([0, -0.3, 0.5], [0.1304237, -0.9914583, 0, 0]),
         ]
 
         # add some point cloud to make the planning more challenging

mplib/examples/demo.py

@@ -1,5 +1,6 @@
 import sapien.core as sapien
 
+from mplib import Pose
 from mplib.examples.demo_setup import DemoSetup
 
 
@@ -36,19 +37,19 @@ def __init__(self):
         # boxes ankor
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-        builder.add_box_visual(half_size=[0.02, 0.02, 0.06])
+        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], material=[1, 0, 0])
         red_cube = builder.build(name="red_cube")
         red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
 
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-        builder.add_box_visual(half_size=[0.02, 0.02, 0.04])
+        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], material=[0, 1, 0])
         green_cube = builder.build(name="green_cube")
         green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
 
         builder = self.scene.create_actor_builder()
         builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-        builder.add_box_visual(half_size=[0.02, 0.02, 0.07])
+        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], material=[0, 0, 1])
         blue_cube = builder.build(name="blue_cube")
         blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
         # boxes ankor end
@@ -61,28 +62,28 @@ def demo(self):
         """
         # target poses ankor
         poses = [
-            [0.4, 0.3, 0.12, 0, 1, 0, 0],
-            [0.2, -0.3, 0.08, 0, 1, 0, 0],
-            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+            Pose([0.4, 0.3, 0.12], [0, 1, 0, 0]),
+            Pose([0.2, -0.3, 0.08], [0, 1, 0, 0]),
+            Pose([0.6, 0.1, 0.14], [0, 1, 0, 0]),
         ]
         # target poses ankor end
         # execute motion ankor
         for i in range(3):
             pose = poses[i]
-            pose[2] += 0.2
+            pose.p[2] += 0.2
             self.move_to_pose(pose)
             self.open_gripper()
-            pose[2] -= 0.12
+            pose.p[2] -= 0.12
             self.move_to_pose(pose)
             self.close_gripper()
-            pose[2] += 0.12
+            pose.p[2] += 0.12
             self.move_to_pose(pose)
-            pose[0] += 0.1
+            pose.p[0] += 0.1
             self.move_to_pose(pose)
-            pose[2] -= 0.12
+            pose.p[2] -= 0.12
             self.move_to_pose(pose)
             self.open_gripper()
-            pose[2] += 0.12
+            pose.p[2] += 0.12
             self.move_to_pose(pose)
         # execute motion ankor end