haosulab · KolinGuo · Apr 16, 2024 · Apr 12, 2024 · Apr 12, 2024 · Apr 12, 2024
diff --git a/.readthedocs.yaml b/.readthedocs.yaml
@@ -4,6 +4,8 @@ build:
   os: "ubuntu-22.04"
   tools:
     python: "3.10"
+  apt_packages:
+    - libx11-6
   jobs:
     post_install:
       - python docs/get_wheel_artifact.py haosulab/MPlib --py cp310 --wait-sec 600

diff --git a/docs/compose.yaml b/docs/compose.yaml
@@ -12,8 +12,11 @@ services:
     command:
       - >-
         python3 -m pip install ./wheelhouse/mplib*.whl
-        && rm -rf ./docs/build
-        && sphinx-autobuild ./docs/source/ ./docs/build/html/
+        && apt update && apt install -y libx11-6
+        && python3 -m pip install sapien~=3.0.0.dev
+        && cd ./docs
+        && rm -rf ./build
+        && sphinx-autobuild ./source/ ./build/html/
     image: kolinguo/sphinx
     build:
       network: host
@@ -27,4 +30,5 @@ services:
         COPY ./requirements.txt /tmp
         RUN python3 -m pip install -r /tmp/requirements.txt \
           && rm -r /tmp/*
+        RUN python3 -m pip install sphinx-autobuild watchfiles
         RUN git config --global --add safe.directory /MPlib
diff --git a/docs/requirements.txt b/docs/requirements.txt
@@ -2,3 +2,4 @@ myst-parser
 furo
 sphinx-copybutton
 sphinxext-opengraph
+sapien~=3.0.0.dev
diff --git a/docs/source/conf.py b/docs/source/conf.py
@@ -36,6 +36,17 @@
 templates_path = ["_templates"]
 exclude_patterns = []
 
+maximum_signature_line_length = 88  # limit maximum method/function signature length
+autodoc_preserve_defaults = True
+autodoc_default_options = {
+    "members": True,
+    "member-order": "bysource",
+    "inherited-members": True,
+    "show-inheritance": True,
+    "undoc-members": True,
+    "special-members": "__init__",
+}
+
 
 # -- Options for HTML output -------------------------------------------------
 # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output

diff --git a/docs/source/reference/Planner.rst b/docs/source/reference/Planner.rst
@@ -1,7 +1,4 @@
 ``Planner``
 -------------
 
-.. automodule:: mplib.planner
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. autoclass:: mplib.Planner
diff --git a/docs/source/reference/PlanningWorld.rst b/docs/source/reference/PlanningWorld.rst
@@ -1,7 +1,4 @@
 ``PlanningWorld``
 -------------------
 
-.. autoclass:: mplib.pymp.PlanningWorld
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. autoclass:: mplib.PlanningWorld
diff --git a/docs/source/reference/collision_detection/fcl.rst b/docs/source/reference/collision_detection/fcl.rst
@@ -1,7 +1,4 @@
 fcl
 ----------
 
-.. automodule:: mplib.pymp.collision_detection.fcl
-  :members:
-  :undoc-members:
-  :show-inheritance:
+.. automodule:: mplib.collision_detection.fcl
diff --git a/docs/source/reference/collision_detection/index.rst b/docs/source/reference/collision_detection/index.rst
@@ -1,10 +1,7 @@
 collision_detection
 ------------------------------------------
 
-.. automodule:: mplib.pymp.collision_detection
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. automodule:: mplib.collision_detection
 
 .. toctree::
    :maxdepth: 2

diff --git a/docs/source/reference/core/ArticulatedModel.rst b/docs/source/reference/core/ArticulatedModel.rst
@@ -1,7 +1,4 @@
 ``ArticulatedModel``
 -------------------------
 
-.. autoclass:: mplib.pymp.ArticulatedModel
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. autoclass:: mplib.ArticulatedModel
diff --git a/docs/source/reference/core/AttachedBody.rst b/docs/source/reference/core/AttachedBody.rst
@@ -1,7 +1,4 @@
 ``AttachedBody``
 -------------------------
 
-.. autoclass:: mplib.pymp.AttachedBody
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. autoclass:: mplib.AttachedBody
diff --git a/docs/source/reference/index.rst b/docs/source/reference/index.rst
@@ -10,6 +10,8 @@ API Reference
    core/AttachedBody
    utils/pose
    utils/random
+   urdf_utils
+   sapien_utils
 
 .. toctree::
    :maxdepth: 3

diff --git a/docs/source/reference/kinematics/kdl.rst b/docs/source/reference/kinematics/kdl.rst
@@ -1,7 +1,4 @@
 kdl
 -------
 
-.. automodule:: mplib.pymp.kinematics.kdl
-  :members:
-  :undoc-members:
-  :show-inheritance:
+.. automodule:: mplib.kinematics.kdl
diff --git a/docs/source/reference/kinematics/pinocchio.rst b/docs/source/reference/kinematics/pinocchio.rst
@@ -1,7 +1,4 @@
 pinocchio
 --------------
 
-.. automodule:: mplib.pymp.kinematics.pinocchio
-  :members:
-  :undoc-members:
-  :show-inheritance:
+.. automodule:: mplib.kinematics.pinocchio
diff --git a/docs/source/reference/planning/ompl.rst b/docs/source/reference/planning/ompl.rst
@@ -1,7 +1,4 @@
 ompl
 ------------
 
-.. automodule:: mplib.pymp.planning.ompl
-  :members:
-  :undoc-members:
-  :show-inheritance:
+.. automodule:: mplib.planning.ompl
diff --git a/docs/source/reference/sapien_utils.rst b/docs/source/reference/sapien_utils.rst
@@ -0,0 +1,6 @@
+``sapien_utils``
+------------------------------------------
+
+.. autoclass:: mplib.sapien_utils.SapienPlanningWorld
+
+.. autoclass:: mplib.sapien_utils.SapienPlanner
diff --git a/docs/source/reference/urdf_utils.rst b/docs/source/reference/urdf_utils.rst
@@ -0,0 +1,4 @@
+``urdf_utils``
+-------------------
+
+.. automodule:: mplib.urdf_utils
diff --git a/docs/source/reference/utils/pose.rst b/docs/source/reference/utils/pose.rst
@@ -1,7 +1,4 @@
 ``Pose``
 -------------------------
 
-.. autoclass:: mplib.pymp.Pose
-   :members:
-   :undoc-members:
-   :show-inheritance:
+.. autoclass:: mplib.Pose
diff --git a/docs/source/reference/utils/random.rst b/docs/source/reference/utils/random.rst
@@ -1,4 +1,4 @@
 ``set_global_seed``
 -------------------------
 
-.. autofunction:: mplib.pymp.set_global_seed
+.. autofunction:: mplib.set_global_seed
diff --git a/docs/source/tutorials/collision_avoidance.rst b/docs/source/tutorials/collision_avoidance.rst
@@ -34,14 +34,6 @@ One way to model the environment and avoid collision is through point clouds. Th
 
 ``planner.update_point_cloud()`` takes two arguments. The first one is a NumPy array of shape :math:`(n \times 3)`, which describes the coordinates of the points. **The coordinates should be represented in the world frame**. The second (optional) argument is ``resolution``, which describes the resolution of each point. This can be used to create a buffer around the collision object.
 
-After adding the point cloud, we can avoid collisions between the robot and the point cloud by setting ``use_point_cloud`` to be True. Both ``planner.plan_qpos_to_pose()`` and ``planner.plan_screw()`` support this flag:
-
-.. literalinclude:: ../../../mplib/examples/demo_setup.py
-   :dedent: 0
-   :start-after: # plan_qpos_to_pose ankor
-   :end-before: # plan_qpos_to_pose ankor end
-   :emphasize-lines: 5
-
 You don't need to provide the point cloud for each ``planner.plan()`` or ``planner.plan_screw()`` call. You can use  ``planner.update_point_cloud()`` to update the point cloud once it's changed.
 
 .. note::
@@ -62,7 +54,7 @@ As shown in the above figure (middle one), after adding the point cloud of the b
 - ``pose``: a list with seven elements indicates the relative pose from the box to the attached link. The first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part.
 - ``link_id = -1``: optional, an integer indicates the id of the link that the box is attached to. The link id is determined by the ``user_link_names`` (during Configuration), and starts from 0. The default value -1 indicates the ``move_group`` link.
 
-After adding the attached box, we can avoid collisions between the attached box and the point cloud by setting both ``use_point_cloud`` and ``use_attach`` to be True. Both ``planner.plan_qpos_to_pose()`` and ``planner.plan_screw()`` support the flags.
+After adding the attached box, we can avoid collisions between the attached box and the point cloud by setting both ``use_point_cloud`` and ``use_attach`` to be True. Both ``planner.plan_pose()`` and ``planner.plan_screw()`` support the flags.
 
 You can use  ``planner.update_attached_box()`` again to update the box once it's changed.
 

diff --git a/docs/source/tutorials/detect_collision.rst b/docs/source/tutorials/detect_collision.rst
@@ -22,7 +22,7 @@ We will also create a floor as one of the collision objects to demonstrate the `
     :start-after: # floor ankor
     :end-before: # floor ankor end
 
-Note that we call floor a "normal" object because it is not an articulated object. The function to add a normal object to the planning world is `set_normal_object`. This can also be used to update the pose of the object or change it our entirely.
+Note that we call floor an object because it is not an articulated object. The function to add an object to the planning world is `add_object`. This can also be used to update the pose of the object or change it our entirely.
 
 Collision Time!
 ---------------

diff --git a/docs/source/tutorials/plan_a_path.rst b/docs/source/tutorials/plan_a_path.rst
@@ -19,18 +19,18 @@ In this tutorial, we will talk about how to plan paths for the agent. As shown i
 Plan with sampling-based algorithms
 --------------------------------------
 
-``mplib`` supports state-of-the-art sampling-based motion planning algorithms by leveraging `OMPL <https://github.com/ompl/ompl>`_. You can call ``planner.plan_qpos_to_pose()`` to plan a path for moving the ``move_group`` link to a target pose: 
+``mplib`` supports state-of-the-art sampling-based motion planning algorithms by leveraging `OMPL <https://github.com/ompl/ompl>`_. You can call ``planner.plan_pose()`` to plan a path for moving the ``move_group`` link to a target pose: 
 
 .. literalinclude:: ../../../mplib/examples/demo_setup.py
    :dedent: 0
-   :start-after: # plan_qpos_to_pose ankor
-   :end-before: # plan_qpos_to_pose ankor end
+   :start-after: # plan_pose ankor
+   :end-before: # plan_pose ankor end
 
-Specifically, ``planner.plan_qpos_to_pose()`` takes two required arguments as input. The first one is the target pose of the ``move_group`` link. It's a 7-dim list, where the first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. **Note that the pose is relative to the world frame**. Normally, the base link of the robot is the world frame unless you have called ``set_base_pose(new_pose)`` in on the planner. You can also temporarily plan w.r.t. the robot base by passing in ``wrt_world=False``.
+Specifically, ``planner.plan_pose()`` takes two required arguments as input. The first one is the target pose of the ``move_group`` link. It's a 7-dim list, where the first three elements describe the position part, and the remaining four elements describe the quaternion (wxyz) for the rotation part. **Note that the pose is relative to the world frame**. Normally, the base link of the robot is the world frame unless you have called ``set_base_pose(new_pose)`` in on the planner. You can also temporarily plan w.r.t. the robot base by passing in ``wrt_world=False``.
 
-The second argument is the current joint positions of all the active joints (not just all the active joints in the movegroup). The ``planner.plan_qpos_to_pose()`` function first solves the inverse kinematics to get the joint positions for the target pose. It then calls the RRTConnect algorithm to find a path in the joint space. Finally, it simplifies the path and parameterizes the path to generate time, velocity, and acceleration information.
+The second argument is the current joint positions of all the active joints (not just all the active joints in the movegroup). The ``planner.plan_pose()`` function first solves the inverse kinematics to get the joint positions for the target pose. It then calls the RRTConnect algorithm to find a path in the joint space. Finally, it simplifies the path and parameterizes the path to generate time, velocity, and acceleration information.
 
-``planner.plan_qpos_to_pose()`` returns a dict which includes:   
+``planner.plan_pose()`` returns a dict which includes:   
 
 - ``status``: a string indicates the status:
 
@@ -44,14 +44,14 @@ The second argument is the current joint positions of all the active joints (not
 - ``acceleration``: a NumPy array of shape :math:`(n \times m)` describing the joint accelerations of the waypoints. 
 
 
-``planner.plan_qpos_to_pose()`` also takes other optional arguments with default values:
+``planner.plan_pose()`` also takes other optional arguments with default values:
 
-.. automethod:: mplib.planner.Planner.plan_qpos_to_pose
+.. automethod:: mplib.Planner.plan_pose
    :no-index:
 
 Follow a path
 --------------------------------------
-``plan_qpos_to_pose()`` outputs a time-parameterized path, and we need to drive the robot to follow the path. In this demo, we use ``sapien`` to simulate and drive the robot.
+``plan_pose()`` outputs a time-parameterized path, and we need to drive the robot to follow the path. In this demo, we use ``sapien`` to simulate and drive the robot.
 
 .. literalinclude:: ../../../mplib/examples/demo_setup.py
    :dedent: 0
@@ -72,14 +72,14 @@ Compared to the sampling-based algorithms, planning with screw motion has the fo
 - `straighter` path: there is no guarantee for sampling-based algorithms to generate `straight` paths even it's a simple lifting task since it connects states in the joint space. In contrast, the returned path by the exponential coordinates and the Jacobian matrix can sometimes be more reasonable. See the above figures for comparison. 
 
 
-You can call ``planner.plan_screw()`` to plan a path with screw motion. Similar to ``planner.plan_qpos_to_pose()``, it also takes two required arguments: target pose and current joint positions, and returns a dict containing the same set of elements. 
+You can call ``planner.plan_screw()`` to plan a path with screw motion. Similar to ``planner.plan_pose()``, it also takes two required arguments: target pose and current joint positions, and returns a dict containing the same set of elements. 
 
 .. literalinclude:: ../../../mplib/planner.py
    :dedent: 0
    :start-after: # plan_screw ankor
    :end-before: # plan_screw ankor end
 
-However, planning with screw motion only succeeds when there is no collision during the planning since it can not detour or replan. We thus recommend use ``planner.plan_screw()`` for some simple tasks or combined with ``planner.plan_qpos_to_pose()``. As shown in the code, we first try ``planner.plan_screw()``, if it fails (e.g., collision during the planning), we then turn to the sampling-based algorithms. Other arguments are the same with ``planner.plan_qpos_to_pose()``.
+However, planning with screw motion only succeeds when there is no collision during the planning since it can not detour or replan. We thus recommend use ``planner.plan_screw()`` for some simple tasks or combined with ``planner.plan_pose()``. As shown in the code, we first try ``planner.plan_screw()``, if it fails (e.g., collision during the planning), we then turn to the sampling-based algorithms. Other arguments are the same with ``planner.plan_pose()``.
 
 
 Move the boxes

diff --git a/docs/source/tutorials/planning_with_fixed_joints.rst b/docs/source/tutorials/planning_with_fixed_joints.rst
@@ -19,14 +19,14 @@ The optional `link_names` and `joint_names` parameters used to order the joints
    :language: python
    :start-after: # pickup ankor
    :end-before: # pickup ankor end
-   :emphasize-lines: 14
+   :emphasize-lines: 12
 
 Notice we have abstracted away how to decouple this motion into two stages. Here is the function definition:
 
 .. literalinclude:: ../../../mplib/examples/two_stage_motion.py
    :language: python
    :start-after: # move_in_two_stage ankor
    :end-before: # move_in_two_stage ankor end
-   :emphasize-lines: 18
+   :emphasize-lines: 16
 
-The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9.
+The highlighted line is how we ignore the arm joints during planning. We ignore joints 2-9, keeping only joint 0 and 1 active. We then do the same thing except the joints fixed are 0 and 1, and the active joints are 2-9.
diff --git a/include/mplib/collision_detection/collision_common.h b/include/mplib/collision_detection/collision_common.h
@@ -14,6 +14,22 @@ struct WorldCollisionResultTpl {
   // TODO: Update with
   // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1CollisionResult.html
 
+  /// @brief Default constructor
+  WorldCollisionResultTpl() {};
+
+  /// Constructor with all members
+  WorldCollisionResultTpl(const ::fcl::CollisionResult<S> &res,
+                          const std::string &collision_type,
+                          const std::string &object_name1,
+                          const std::string &object_name2,
+                          const std::string &link_name1, const std::string &link_name2)
+      : res(res),
+        collision_type(collision_type),
+        object_name1(object_name1),
+        object_name2(object_name2),
+        link_name1(link_name1),
+        link_name2(link_name2) {};
+
   ::fcl::CollisionResult<S> res;  ///< the fcl CollisionResult
   std::string collision_type,     ///< type of the collision
       object_name1,               ///< name of the first object
@@ -28,6 +44,23 @@ struct WorldDistanceResultTpl {
   // TODO: Update with
   // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1DistanceResult.html
 
+  /// @brief Default constructor
+  WorldDistanceResultTpl() {};
+
+  /// Constructor with all members
+  WorldDistanceResultTpl(const ::fcl::DistanceResult<S> &res, S min_distance,
+                         const std::string &distance_type,
+                         const std::string &object_name1,
+                         const std::string &object_name2, const std::string &link_name1,
+                         const std::string &link_name2)
+      : res(res),
+        min_distance(min_distance),
+        distance_type(distance_type),
+        object_name1(object_name1),
+        object_name2(object_name2),
+        link_name1(link_name1),
+        link_name2(link_name2) {};
+
   ::fcl::DistanceResult<S> res;  ///< the fcl DistanceResult
   /// minimum distance between the two objects
   S min_distance {std::numeric_limits<S>::max()};