Merge from kolinguo/MPlib

VERSION

@@ -1 +1 @@
-0.1.1
+0.2.0

dev/README.md

@@ -13,6 +13,8 @@ and use it to build wheel for the specified python version.
 The wheel will be generated in the `wheelhouse/` and the generated pybind11 docstring
 will be in [`./pybind/docstring/`](../pybind/docstring/).
 
+If you want to start the documentation webserver, run `./dev/start_doc_webserver.sh`
+
 If you want to start a docker container for debugging, run `./dev/docker_setup.sh`
 
 ### Versioning & Release Process
@@ -89,7 +91,7 @@ where the `clang-version` should match your local LLVM installation
 
 ## Stubs & Documentation Generation
 
-To generate stubs and documentations, run [`./dev/generate_stub_and_doc.sh`](./generate_stub_and_doc.sh).
+To generate stubs and documentations, run [`./dev/generate_stubs.sh`](./generate_stubs.sh).
 By default it uses `python3.10` in docker image [`kolinguo/mplib-build`](https://hub.docker.com/r/kolinguo/mplib-build).
 
 The script does the following:

dev/generate_stub_and_doc.sh → dev/generate_stubs.sh

@@ -1,8 +1,8 @@
 #!/bin/bash
 
-# Python version to build wheels and generate stubs / docs
+# Python version to build wheels and generate stubs
 PY_VERSION=310
-# Docker image name to install wheel and generate stubs / docs
+# Docker image name to install wheel and generate stubs
 IMGNAME="kolinguo/mplib-build:latest"
 
 ############################################################
@@ -54,12 +54,14 @@ BUILD_STUB_CMD="\
   && python3 -m pip install wheelhouse/mplib*.whl \
   && python3 dev/stubgen.py \
   && python3 -m pip install ruff \
-  && ruff check --select I --fix ./stubs \
-  && ruff format ./stubs
+  && ruff check --no-cache --select I --fix ./stubs \
+  && ruff format --no-cache ./stubs \
+  && chown -R \${USER_UID}:\${USER_UID} ./stubs
 "
 
 echo_info "Building stubs in docker '${IMGNAME}'"
 docker run -it --rm \
+  -e USER_UID="$UID" \
   -v "$REPO_DIR":/${REPO_NAME} \
   "$IMGNAME" \
   bash -c "$BUILD_STUB_CMD"

dev/mkdoc.py

@@ -221,6 +221,8 @@ def process_comment(comment: str) -> str:
 
     s = s.replace("``true``", "``True``")
     s = s.replace("``false``", "``False``")
+    s = s.replace("``nullptr``", "``None``")  # nullptr is None in Python
+    s = s.replace("``std::nullopt``", "``None``")  # std::nullopt is None in Python
 
     # Exceptions
     s = replace_exceptions(s)

dev/start_doc_webserver.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+
+############################################################
+# Section 0: Bash Error Handling                           #
+############################################################
+set -eEu -o pipefail
+trap 'catch' ERR  # Trap all errors (status != 0) and call catch()
+catch() {
+  local err="$?"
+  local err_command="$BASH_COMMAND"
+  set +xv  # disable trace printing
+
+  echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2
+  echo "Traceback (most recent call last, command might not be complete):" >&2
+  for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do
+    local funcname="${FUNCNAME[$i]}"
+    [ "$i" -eq "0" ] && funcname=$err_command
+    echo -e "  ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2
+  done
+  exit "$err"
+}
+
+############################################################
+# Section 1: Build python wheels                           #
+############################################################
+# Move to the repo folder, so later commands can use relative paths
+SCRIPT_PATH=$(readlink -f "$0")
+REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")")
+cd "$REPO_DIR"
+
+# Build wheel and stubs
+if [ ! -f ./wheelhouse/mplib*.whl ] ; then
+  dev/generate_stubs.sh
+fi
+
+# Start docs webserver
+cd docs/
+docker compose up

docs/compose.yaml

@@ -0,0 +1,30 @@
+x-common: &common
+  entrypoint: ["/bin/bash", "-c"]
+  volumes:
+    - "/etc/localtime:/etc/localtime:ro"
+    - "../:/MPlib"
+  working_dir: /MPlib
+  network_mode: host
+
+services:
+  sphinx:
+    <<: *common
+    command:
+      - >-
+        python3 -m pip install ./wheelhouse/mplib*.whl
+        && rm -rf ./docs/build
+        && sphinx-autobuild ./docs/source/ ./docs/build/html/
+    image: kolinguo/sphinx
+    build:
+      network: host
+      dockerfile_inline: |
+        FROM ubuntu:latest
+        RUN apt-get update && apt-get install -y --no-install-recommends \
+            git python3 python3-pip \
+          && apt-get upgrade -y \
+          && rm -rf /var/lib/apt/lists/* \
+          && python3 -m pip install --upgrade pip
+        COPY ./requirements.txt /tmp
+        RUN python3 -m pip install -r /tmp/requirements.txt \
+          && rm -r /tmp/*
+        RUN git config --global --add safe.directory /MPlib

docs/source/reference/core/AttachedBody.rst

@@ -0,0 +1,7 @@
+``AttachedBody``
+-------------------------
+
+.. autoclass:: mplib.pymp.AttachedBody
+   :members:
+   :undoc-members:
+   :show-inheritance:

docs/source/reference/index.rst

@@ -5,8 +5,10 @@ API Reference
    :maxdepth: 2
 
    Planner
-   ArticulatedModel
    PlanningWorld
+   core/ArticulatedModel
+   core/AttachedBody
+   utils/random
 
 .. toctree::
    :maxdepth: 3

docs/source/reference/utils/random.rst

@@ -0,0 +1,4 @@
+``set_global_seed``
+-------------------------
+
+.. autofunction:: mplib.pymp.set_global_seed

docs/source/tutorials/collision_avoidance.rst

@@ -32,7 +32,7 @@ One way to model the environment and avoid collision is through point clouds. Th
    :end-before: # add_point_cloud ankor end
    :emphasize-lines: 4
 
-``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 ``radius``, which describes the radius of each point. This can be used to create a buffer around the collision object.
+``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:
 

include/mplib/collision_detection/collision_common.h

@@ -1,8 +1,10 @@
 #pragma once
 
+#include <limits>
 #include <string>
 
 #include <fcl/narrowphase/collision_result.h>
+#include <fcl/narrowphase/distance_result.h>
 
 namespace mplib::collision_detection {
 
@@ -20,4 +22,20 @@ struct WorldCollisionResultTpl {
       link_name2;                 ///< link name of the second object in collision
 };
 
+/// Result of minimum distance-to-collision query.
+template <typename S>
+struct WorldDistanceResultTpl {
+  // TODO: Update with
+  // https://moveit.picknik.ai/main/api/html/structcollision__detection_1_1DistanceResult.html
+
+  ::fcl::DistanceResult<S> res;  ///< the fcl DistanceResult
+  /// minimum distance between the two objects
+  S min_distance {std::numeric_limits<S>::max()};
+  std::string distance_type,  ///< type of the distance result
+      object_name1,           ///< name of the first object
+      object_name2,           ///< name of the second object
+      link_name1,             ///< link name of the first object
+      link_name2;             ///< link name of the second object
+};
+
 }  // namespace mplib::collision_detection

include/mplib/collision_detection/collision_matrix.h

@@ -0,0 +1,188 @@
+#pragma once
+
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "mplib/macros/class_forward.h"
+
+namespace mplib::collision_detection {
+
+/// AllowedCollision Enum class
+enum class AllowedCollision {
+  NEVER,   ///< Collision is never allowed
+  ALWAYS,  ///< Collision is always allowed
+  // unused for now
+  CONDITIONAL,  ///< Collision contact is allowed depending on a predicate
+};
+
+// AllowedCollisionMatrixPtr
+MPLIB_CLASS_FORWARD(AllowedCollisionMatrix);
+
+/**
+ * AllowedCollisionMatrix for collision checking
+ *
+ * All elements in the collision world are referred to by their names.
+ * This class represents which collisions are allowed to happen and which are not.
+ *
+ * Mimicking MoveIt2's ``collision_detection::AllowedCollisionMatrix``
+ *
+ * https://moveit.picknik.ai/main/api/html/classcollision__detection_1_1AllowedCollisionMatrix.html
+ */
+class AllowedCollisionMatrix {
+ public:
+  AllowedCollisionMatrix();
+
+  /**
+   * Get the type of the allowed collision between two elements
+   *
+   * @param name1: name of the first element
+   * @param name2: name of the second element
+   * @return: an AllowedCollision Enum or ``std::nullopt`` if the entry does not exist.
+   */
+  std::optional<AllowedCollision> getEntry(const std::string &name1,
+                                           const std::string &name2) const;
+
+  /// @brief Check if an entry exists for an element
+  bool hasEntry(const std::string &name) const;
+
+  /// @brief Check if an entry exists for a pair of elements
+  bool hasEntry(const std::string &name1, const std::string &name2) const;
+
+  /// @brief Set an entry for a pair of elements
+  void setEntry(const std::string &name1, const std::string &name2, bool allowed);
+
+  /// @brief Set the entries between the element and each element in other_names
+  void setEntry(const std::string &name, const std::vector<std::string> &other_names,
+                bool allowed);
+
+  /// @brief Set the entries for all possible pairs among two sets of elements
+  void setEntry(const std::vector<std::string> &names1,
+                const std::vector<std::string> &names2, bool allowed);
+
+  /**
+   * Set the entries for all possible pairs between the element and
+   * existing elements. As the set of elements might change in the future,
+   * consider using setDefaultEntry() instead.
+   */
+  void setEntry(const std::string &name, bool allowed);
+
+  /**
+   * Set the entries for all possible pairs between each of the elements
+   * and existing elements. As the set of elements might change in the
+   * future, consider using setDefaultEntry() instead.
+   */
+  void setEntry(const std::vector<std::string> &names, bool allowed);
+
+  /// @brief Set the entries for all possible pairs among all existing elements
+  void setEntry(bool allowed);
+
+  /// @brief Remove the entry for a pair of elements if exists
+  void removeEntry(const std::string &name1, const std::string &name2);
+
+  /**
+   * Remove existing entries between the element and each element in
+   * other_names
+   */
+  void removeEntry(const std::string &name,
+                   const std::vector<std::string> &other_names);
+
+  /**
+   * Remove any existing entries for all possible pairs among two sets of
+   * elements
+   */
+  void removeEntry(const std::vector<std::string> &names1,
+                   const std::vector<std::string> &names2);
+
+  /**
+   * Remove all entries for all possible pairs between the element and
+   * existing elements
+   */
+  void removeEntry(const std::string &name);
+
+  /**
+   * Remove all entries for all possible pairs between each of the
+   * elements and existing elements
+   */
+  void removeEntry(const std::vector<std::string> &names);
+
+  /// @brief Get the size of the allowed collision matrix (number of entries)
+  size_t getSize() const { return entries_.size(); }
+
+  /**
+   * Get the default type of the allowed collision for an element
+   *
+   * @param name: name of the element
+   * @return: an AllowedCollision Enum or ``std::nullopt`` if the default entry does not
+   * exist
+   */
+  std::optional<AllowedCollision> getDefaultEntry(const std::string &name) const;
+
+  /// @brief Check if a default entry exists for an element
+  bool hasDefaultEntry(const std::string &name) const;
+
+  /**
+   * Set the default value for entries that include name but are not set
+   * explicitly with setEntry(). Apply to future changes of the element set.
+   */
+  void setDefaultEntry(const std::string &name, bool allowed);
+
+  /**
+   * Set the default entries for the elements. Apply to future changes of
+   * the element set.
+   */
+  void setDefaultEntry(const std::vector<std::string> &names, bool allowed);
+
+  /// @brief Remove the default entry for the element if exists
+  void removeDefaultEntry(const std::string &name);
+
+  /// @brief Remove the existing default entries for the elements
+  void removeDefaultEntry(const std::vector<std::string> &names);
+
+  /**
+   * Get the type of the allowed collision between two elements
+   *
+   * @return: AllowedCollision. This is
+   *  * ``std::nullopt`` if the entry does not exist (collision is not allowed)
+   *  * the entry if an entry or a default entry exists.
+   */
+  std::optional<AllowedCollision> getAllowedCollision(const std::string &name1,
+                                                      const std::string &name2) const;
+
+  /// @brief Clear all data in the allowed collision matrix
+  void clear();
+
+  /**
+   * Get sorted names of all existing elements (including
+   * default_entries_)
+   */
+  std::vector<std::string> getAllEntryNames() const;
+
+  /**
+   * Print the allowed collision matrix.
+   * "01?-" corresponds to NEVER / ALWAYS / CONDITIONAL / Entry not found
+   */
+  void print(std::ostream &out) const;
+
+ private:
+  /**
+   * Get the default type of the allowed collision for a pair of elements
+   *
+   * @return: AllowedCollision
+   *  If neither element has a default entry, returns ``std::nullopt``.
+   *  If only one of the elements has a default entry, returns the entry.
+   *  If both elements have default entries, returns
+   *    * NEVER if at least one of the default types is NEVER;
+   *    * CONDITIONAL if at least one of the default types is CONDITIONAL;
+   *    * ALWAYS otherwise.
+   */
+  std::optional<AllowedCollision> getDefaultEntry(const std::string &name1,
+                                                  const std::string &name2) const;
+
+  std::unordered_map<std::string, std::unordered_map<std::string, AllowedCollision>>
+      entries_;
+  std::unordered_map<std::string, AllowedCollision> default_entries_;
+};
+
+}  // namespace mplib::collision_detection