From afba76a0b4461fb3e31cec009d6ce2ef031e934b Mon Sep 17 00:00:00 2001
From: Kolin Guo <ruguo@ucsd.edu>
Date: Sat, 20 Jan 2024 18:41:23 -0800
Subject: [PATCH 1/6] Fix all ruff warnings

---
 dev/test_mkdoc/drake/libclang_setup.py |  47 ++++-----
 dev/test_mkdoc/drake/mkdoc.py          |  91 +++++++++---------
 dev/test_mkdoc/drake/mkdoc_comment.py  |  22 +++--
 mplib/examples/collision_avoidance.py  |  28 ++++--
 mplib/examples/constrained_planning.py |  32 ++++---
 mplib/examples/demo.py                 |  16 ++--
 mplib/examples/demo_setup.py           |  28 +++---
 mplib/examples/detect_collision.py     |  19 ++--
 mplib/examples/moving_robot.py         |  24 +++--
 mplib/examples/two_stage_motion.py     |  17 ++--
 mplib/planner.py                       | 127 ++++++++++++++-----------
 pyproject.toml                         |  21 ++++
 setup.py                               |  18 ++--
 13 files changed, 288 insertions(+), 202 deletions(-)

diff --git a/dev/test_mkdoc/drake/libclang_setup.py b/dev/test_mkdoc/drake/libclang_setup.py
index aafc9fd..7a0acff 100644
--- a/dev/test_mkdoc/drake/libclang_setup.py
+++ b/dev/test_mkdoc/drake/libclang_setup.py
@@ -1,10 +1,9 @@
-import platform
 import os
+import platform
 import subprocess
 
 from clang import cindex
 
-
 # Alternative: Make this a function in `mkdoc.py`, and import it from mkdoc as
 # a module? (if this was really authored in `mkdoc.py`...)
 
@@ -19,35 +18,37 @@ def add_library_paths(parameters=None):
     Returns:
     """
     library_file = None
-    if platform.system() == 'Darwin':
-        completed_process = subprocess.run(['xcrun', '--find', 'clang'],
-                                           stdout=subprocess.PIPE,
-                                           encoding='utf-8')
+    if platform.system() == "Darwin":
+        completed_process = subprocess.run(
+            ["xcrun", "--find", "clang"], stdout=subprocess.PIPE, encoding="utf-8"
+        )
         if completed_process.returncode == 0:
-            toolchain_dir = os.path.dirname(os.path.dirname(
-                completed_process.stdout.strip()))
-            library_file = os.path.join(
-                toolchain_dir, 'lib', 'libclang.dylib')
-        completed_process = subprocess.run(['xcrun', '--show-sdk-path'],
-                                           stdout=subprocess.PIPE,
-                                           encoding='utf-8')
+            toolchain_dir = os.path.dirname(
+                os.path.dirname(completed_process.stdout.strip())
+            )
+            library_file = os.path.join(toolchain_dir, "lib", "libclang.dylib")
+        completed_process = subprocess.run(
+            ["xcrun", "--show-sdk-path"], stdout=subprocess.PIPE, encoding="utf-8"
+        )
         if parameters is not None and completed_process.returncode == 0:
             sdkroot = completed_process.stdout.strip()
             if os.path.exists(sdkroot):
-                parameters.append('-isysroot')
+                parameters.append("-isysroot")
                 parameters.append(sdkroot)
-    elif platform.system() == 'Linux':
+    elif platform.system() == "Linux":
         # By default we expect Clang 14 to be installed, but on Ubuntu 20.04
         # we'll use Clang 12 (because Clang 14 isn't packaged).
         version = 14
-        completed_process = subprocess.run(['lsb_release', '-sr'],
-                                           stdout=subprocess.PIPE,
-                                           encoding='utf-8')
-        if completed_process.returncode == 0:
-            if completed_process.stdout.strip() == '20.04':
-                version = 12
+        completed_process = subprocess.run(
+            ["lsb_release", "-sr"], stdout=subprocess.PIPE, encoding="utf-8"
+        )
+        if (
+            completed_process.returncode == 0
+            and completed_process.stdout.strip() == "20.04"
+        ):
+            version = 12
         arch = platform.machine()
-        library_file = f'/usr/lib/{arch}-linux-gnu/libclang-{version}.so'
+        library_file = f"/usr/lib/{arch}-linux-gnu/libclang-{version}.so"
     if not os.path.exists(library_file):
-        raise RuntimeError(f'Library file {library_file} does NOT exist')
+        raise RuntimeError(f"Library file {library_file} does NOT exist")
     cindex.Config.set_library_file(library_file)
diff --git a/dev/test_mkdoc/drake/mkdoc.py b/dev/test_mkdoc/drake/mkdoc.py
index bf03c1b..8c0e9cc 100644
--- a/dev/test_mkdoc/drake/mkdoc.py
+++ b/dev/test_mkdoc/drake/mkdoc.py
@@ -1,5 +1,3 @@
-# -*- coding: utf-8 -*-
-#
 #  Derived from https://github.com/pybind/pybind11/
 #
 #  Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>,
@@ -267,9 +265,9 @@ def extract_comment(cursor, deprecations):
 
     # Append the deprecation text.
     result += (
-        r" (Deprecated.) \deprecated {} "
-        "This will be removed from Drake on or after {}."
-    ).format(message, removal_date)
+        rf" (Deprecated.) \deprecated {message} "
+        f"This will be removed from Drake on or after {removal_date}."
+    )
 
     return result
 
@@ -442,7 +440,7 @@ def specialize_well_known_doc_var_names():
                 # overload naming set.
                 result[i] = None
                 continue
-            elif any([symbols[i].comment == x.comment for x in symbols[:i]]):
+            elif any(symbols[i].comment == x.comment for x in symbols[:i]):
                 # If a subsequent overload's API comment *exactly* matches a
                 # prior overload's comment, the first overload's name wins.
                 # This is important because when a function has separate
@@ -518,7 +516,7 @@ def specialize_well_known_doc_var_names():
     ]
 
     # The argument count might be sufficient to disambiguate.
-    result = ["doc_{}args".format(len(types)) for types in overload_arg_types]
+    result = [f"doc_{len(types)}args" for types in overload_arg_types]
     specialize_well_known_doc_var_names()
     if is_unique(result):
         return result
@@ -581,11 +579,11 @@ def iprint(s):
     name_var = sanitize_name(name_var)
     # We may get empty symbols if `libclang` produces warnings.
     assert len(name_var) > 0, node.first_symbol.sorting_key()
-    iprint("// Symbol: {}".format(full_name))
+    iprint(f"// Symbol: {full_name}")
     modifier = ""
     if level == 0:
         modifier = "constexpr "
-    iprint("{}struct /* {} */ {{".format(modifier, name_var))
+    iprint(f"{modifier}struct /* {name_var} */ {{")
 
     # Print documentation items.
     symbol_iter = sorted(node.doc_symbols, key=Symbol.sorting_key)
@@ -599,10 +597,8 @@ def iprint(s):
         delim = "\n"
         if "\n" not in comment and len(comment) < 40:
             delim = " "
-        iprint("  // Source: {}:{}".format(symbol.include, symbol.line))
-        iprint(
-            '  const char* {} ={}R"""({})""";'.format(doc_var, delim, comment.strip())
-        )
+        iprint(f"  // Source: {symbol.include}:{symbol.line}")
+        iprint(f'  const char* {doc_var} ={delim}R"""({comment.strip()})""";')
 
     # Recurse into child elements.
     keys = sorted(node.children_map.keys())
@@ -624,13 +620,13 @@ def iprint(s):
             if (node.children_map[x].first_symbol.cursor.kind == CursorKind.FIELD_DECL)
         ]
         if field_names:
-            iprint(f"  auto Serialize__fields() const {{")
-            iprint(f"    return std::array{{")
+            iprint("  auto Serialize__fields() const {")
+            iprint("    return std::array{{")
             for x in field_names:
                 iprint(f'      std::make_pair("{x}", {x}.doc),')
-            iprint(f"    }};")
-            iprint(f"  }}")
-    iprint("}} {};".format(name_var))
+            iprint("    }};")
+            iprint("  }}")
+    iprint(f"}} {name_var};")
 
 
 class FileDict:
@@ -638,7 +634,9 @@ class FileDict:
     Provides a dictionary that hashes based on a file's true path.
     """
 
-    def __init__(self, items=[]):
+    def __init__(self, items=None):
+        if items is None:
+            items = []
         self._d = {self._key(file): value for file, value in items}
 
     def _key(self, file):
@@ -694,14 +692,14 @@ def main():
         eprint("Syntax: %s -output=<file> [.. a list of header files ..]" % sys.argv[0])
         sys.exit(1)
 
-    f = open(output_filename, "w", encoding="utf-8")
+    f = open(output_filename, "w", encoding="utf-8")  # noqa: SIM115
 
     # N.B. We substitute the `GENERATED FILE...` bits in this fashion because
     # otherwise Reviewable gets confused.
     f.write(
         """#pragma once
 
-// {0} {1}
+// {} {}
 // This file contains docstrings for the Python bindings that were
 // automatically extracted by mkdoc.py.
 
@@ -719,14 +717,11 @@ def main():
     # Determine project include directories.
     # N.B. For simplicity when using with Bazel, we do not try to get canonical
     # file paths for determining include files.
-    include_paths = []
-    for param in parameters:
-        # Only check for normal include directories.
-        if param.startswith("-I"):
-            include_paths.append(param[2:])
+    include_paths = [param[2:] for param in parameters if param.startswith("-I")]
+
     # Use longest include directories first to get shortest include file
     # overall.
-    include_paths = list(sorted(include_paths, key=len))[::-1]
+    include_paths = sorted(include_paths, key=len)[::-1]
     include_files = []
     # Create mapping from filename to include file.
     include_file_map = FileDict()
@@ -739,7 +734,7 @@ def main():
                 break
         else:
             raise RuntimeError(
-                "Filename not incorporated into -I includes: {}".format(filename)
+                f"Filename not incorporated into -I includes: {filename}"
             )
         for p in ignore_patterns:
             if fnmatch(include_file, p):
@@ -765,7 +760,7 @@ def main():
         glue_f.write("#include <optional>\n")
         # Add the includes to the glue, and as comments in the output.
         for include_file in sorted(include_files):
-            line = '#include "{}"'.format(include_file)
+            line = f'#include "{include_file}"'
             glue_f.write(line + "\n")
             f.write("// " + line + "\n")
         f.write("\n")
@@ -782,21 +777,23 @@ def main():
             raise RuntimeError("Parsing headers using the clang library failed")
         # If there is an error on line 1, that means the C++ standard library
         # include paths are broken.
-        if translation_unit.diagnostics:
-            if translation_unit.diagnostics[0].location.line == 1:
-                try:
-                    # Use '###' to dump Clang's include paths to stdout.
-                    index.parse("foo", parameters + ["-###"])
-                except Exception:
-                    pass
-                raise RuntimeError(
-                    "The operating system's C++ standard library is not "
-                    "installed correctly or is only partially installed. For "
-                    "example, libgcc-??-dev is installed but libstdc++-??-dev "
-                    "is not installed (the ?? indicates a version number). "
-                    "Try re-running Drake's install_prereqs, or maybe check "
-                    "your system and install anything that's missing by hand."
-                )
+        if (
+            translation_unit.diagnostics
+            and translation_unit.diagnostics[0].location.line == 1
+        ):
+            try:
+                # Use '###' to dump Clang's include paths to stdout.
+                index.parse("foo", parameters + ["-###"])
+            except Exception:
+                pass
+            raise RuntimeError(
+                "The operating system's C++ standard library is not "
+                "installed correctly or is only partially installed. For "
+                "example, libgcc-??-dev is installed but libstdc++-??-dev "
+                "is not installed (the ?? indicates a version number). "
+                "Try re-running Drake's install_prereqs, or maybe check "
+                "your system and install anything that's missing by hand."
+            )
         severities = [
             diagnostic.severity
             for diagnostic in translation_unit.diagnostics
@@ -826,12 +823,12 @@ def main():
     except UnicodeEncodeError as e:
         # User-friendly error for #9903.
         print(
-            """
-Encountered unicode error: {}
+            f"""
+Encountered unicode error: {e}
 If you are on Ubuntu, please ensure you have en_US.UTF-8 locales generated:
     sudo apt-get install --no-install-recommends  locales
     sudo locale-gen en_US.UTF-8
-""".format(e),
+""",
             file=sys.stderr,
         )
         sys.exit(1)
diff --git a/dev/test_mkdoc/drake/mkdoc_comment.py b/dev/test_mkdoc/drake/mkdoc_comment.py
index 55db831..28a1746 100644
--- a/dev/test_mkdoc/drake/mkdoc_comment.py
+++ b/dev/test_mkdoc/drake/mkdoc_comment.py
@@ -1,5 +1,3 @@
-# -*- coding: utf-8 -*-
-#
 #  Derived from https://github.com/pybind/pybind11/
 #
 #  Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>,
@@ -97,7 +95,7 @@ def remove_cpp_comment_syntax(s):
         # http://www.doxygen.nl/manual/docblocks.html#memberdoc
         if line.startswith("///<"):
             line = line[4:]
-        if line.startswith("///") or line.startswith("//!"):
+        if line.startswith(("///", "//!")):
             line = line[3:]
         if line.startswith("*"):
             line = line[1:]
@@ -158,7 +156,7 @@ def markdown_to_restructuredtext(s):
 
 def replace_with_header(pattern, token, s, **kwargs):
     def repl(match):
-        return "\n{}\n{}\n".format(match.group(1), token * len(match.group(1)))
+        return f"\n{match.group(1)}\n{token * len(match.group(1))}\n"
 
     return re.sub(pattern, repl, s, **kwargs)
 
@@ -232,12 +230,12 @@ def process_doxygen_commands(s):
     s = re.sub(r"[@\\](?:a|e|em)\s+%s" % cpp_group, r"*\1*", s)
     s = re.sub(r"[@\\]b\s+%s" % cpp_group, r"**\1**", s)
     s = re.sub(
-        r"[@\\]param%s?\s+%s" % (param_group, cpp_group),
+        rf"[@\\]param{param_group}?\s+{cpp_group}",
         r"\n\n$Parameter ``\2``:\n\n",
         s,
     )
     s = re.sub(
-        r"[@\\]tparam%s?\s+%s" % (param_group, cpp_group),
+        rf"[@\\]tparam{param_group}?\s+{cpp_group}",
         r"\n\n$Template parameter ``\2``:\n\n",
         s,
     )
@@ -291,7 +289,7 @@ def process_doxygen_commands(s):
     #  ```
     for start_, end_ in (("code", "endcode"), ("verbatim", "endverbatim")):
         s = re.sub(
-            r"[@\\]%s(?:\{\.(\w+)\})?\s?(.*?)\s?[@\\]%s" % (start_, end_),
+            rf"[@\\]{start_}(?:\{{\.(\w+)\}})?\s?(.*?)\s?[@\\]{end_}",
             r"```\1\n\2\n```\n",
             s,
             flags=re.DOTALL,
@@ -332,9 +330,13 @@ def process_doxygen_commands(s):
     s = re.sub(r"[@\\]default\s+", r"\n$*Default:* ", s)
     s = re.sub(
         r"[@\\]experimental\s+",
-        "\n\n$Warning:\n\nThis feature is considered to be **experimental** and may change or be removed at any time, without any deprecation notice ahead of time.\n\n",
+        (
+            "\n\n$Warning:\n\nThis feature is considered to be **experimental** and "
+            "may change or be removed at any time, "
+            "without any deprecation notice ahead of time.\n\n"
+        ),
         s,
-    )  # noqa
+    )
     # In pydrake docs, "python details" are not actually details; erase the
     # markers for `<detail>` so that it is always shown.
     s = re.sub(r"[@\\]python_details_(?:begin|end)", r"", s)
@@ -496,7 +498,7 @@ def process_doxygen_commands(s):
         ("xmlonly", "endxmlonly"),
     ):
         s = re.sub(
-            r"[@\\]%s\s?(.*?)\s?[@\\]%s" % (start_, end_), r"", s, flags=re.DOTALL
+            rf"[@\\]{start_}\s?(.*?)\s?[@\\]{end_}", r"", s, flags=re.DOTALL
         )
 
         # Some command pairs may bridge multiple comment blocks, so individual
diff --git a/mplib/examples/collision_avoidance.py b/mplib/examples/collision_avoidance.py
index 465fabf..dea6528 100644
--- a/mplib/examples/collision_avoidance.py
+++ b/mplib/examples/collision_avoidance.py
@@ -4,14 +4,18 @@
 
 
 class PlanningDemo(DemoSetup):
-    """The shows the planner's ability to generate a collision free path with the straight path causes collisions"""
+    """
+    The shows the planner's ability to generate a collision free path
+    with the straight path causes collisions.
+    """
 
     def __init__(self):
         """
-        Same setup as demo.py except the boxes are of difference sizes and different use
-        Red box is the target we want to grab
-        Blue box is the obstacle we want to avoid
-        green box is the landing pad on which we want to place the red box
+        Same setup as demo.py, except the boxes are of difference sizes and
+        different uses.
+        Red box is the target we want to grab.
+        Blue box is the obstacle we want to avoid.
+        Green box is the landing pad on which we want to place the red box.
         """
         super().__init__()
         self.setup_scene()
@@ -51,7 +55,7 @@ def __init__(self):
         self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
 
     def add_point_cloud(self):
-        """we tell the planner about the obstacle through a point cloud"""
+        """We tell the planner about the obstacle through a point cloud"""
         import trimesh
 
         box = trimesh.creation.box([0.1, 0.4, 0.2])
@@ -62,7 +66,8 @@ def add_point_cloud(self):
 
     def demo(self, with_screw=True, use_point_cloud=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
+        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]
@@ -100,9 +105,12 @@ def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
 
 if __name__ == "__main__":
     """
-    As you change some of the parameters, you will see different behaviors
-    In particular, when point cloud is not used, the robot will attemt to go through the blue box
-    If attach is not used, the robot will avoid the blue box on its way to the pickup pose but will knock it over with the attached red cube on its way to the delivery pose
+    As you change some of the parameters, you will see different behaviors.
+    In particular, when point cloud is not used, the robot will attemt to go through
+    the blue box.
+    If attach is not used, the robot will avoid the blue box on its way to
+    the pickup pose but will knock it over with the attached red cube on its way to
+    the delivery pose
     """
     demo = PlanningDemo()
     demo.demo(False, True, True)
diff --git a/mplib/examples/constrained_planning.py b/mplib/examples/constrained_planning.py
index fa67164..7fa16b1 100644
--- a/mplib/examples/constrained_planning.py
+++ b/mplib/examples/constrained_planning.py
@@ -9,18 +9,19 @@
 class ConstrainedPlanningDemo(DemoSetup):
     """
     This demo shows the planner's ability to plan with constraints.
-    For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
+    For this particular demo, we move to several poses while pointing the end effector
+    roughly 15 degrees w.r.t. -z axis.
     """
 
     def __init__(self):
-        """set up the scene and load the robot"""
+        """Set up the scene and load the robot"""
         super().__init__()
         self.setup_scene()
         self.load_robot()
         self.setup_planner()
 
     def add_point_cloud(self):
-        """add some random obstacles to make the planning more challenging"""
+        """Add some random obstacles to make the planning more challenging"""
         import trimesh
 
         box = trimesh.creation.box([0.1, 0.4, 0.2])
@@ -32,7 +33,7 @@ def add_point_cloud(self):
         return
 
     def get_eef_z(self):
-        """helper function for constraint"""
+        """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:])
@@ -40,9 +41,12 @@ def get_eef_z(self):
 
     def make_f(self):
         """
-        create a constraint function that takes in a qpos and outputs a scalar
-        A valid constraint function should evaluates to 0 when the constraint is satisfied
-        See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
+        Create a constraint function that takes in a qpos and outputs a scalar.
+        A valid constraint function should evaluates to 0 when the constraint
+        is satisfied.
+
+        See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html)
+        for more details.
         """
 
         def f(x, out):
@@ -55,8 +59,9 @@ def f(x, out):
 
     def make_j(self):
         """
-        create the jacobian of the constraint function w.r.t. qpos
-        This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
+        Create the jacobian of the constraint function w.r.t. qpos.
+        This is needed because the planner uses the jacobian to project a random sample
+        to the constraint manifold.
         """
 
         def j(x, out):
@@ -74,8 +79,10 @@ def j(x, out):
 
     def demo(self):
         """
-        We first plan with constraints to three poses, then plan without constraints to the same poses
-        While not always the case, sometimes without constraints, the end effector will tilt almost upside down
+        We first plan with constraints to three poses, then plan without constraints to
+        the same poses.
+        While not always the case, sometimes without constraints,
+        the end effector will tilt almost upside down.
         """
         # this starting pose has the end effector tilted roughly 15 degrees
         starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
@@ -88,7 +95,8 @@ def demo(self):
             [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0],
         ]
 
-        # add some point cloud to make the planning more challenging so we can see the effect of no constraint
+        # add some point cloud to make the planning more challenging
+        # so we can see the effect of no constraint
         self.add_point_cloud()
 
         # with constraint
diff --git a/mplib/examples/demo.py b/mplib/examples/demo.py
index fdba6e5..91cccd0 100644
--- a/mplib/examples/demo.py
+++ b/mplib/examples/demo.py
@@ -5,16 +5,19 @@
 
 class PlanningDemo(DemoSetup):
     """
-    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
+    This is the most basic demo of the motion planning library where the robot tries to
+    shuffle three boxes around.
     """
 
     def __init__(self):
         """
-        Setting up the scene, the planner, and adding some objects to the scene
-        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+        Setting up the scene, the planner, and adding some objects to the scene.
+        Afterwards, put down a table and three boxes.
+        For details on how to do this, see the sapien documentation.
         """
         super().__init__()
-        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
+        # load the world, the robot, and then setup the planner.
+        # See demo_setup.py for more details
         self.setup_scene()
         self.load_robot()
         self.setup_planner()
@@ -51,8 +54,9 @@ def __init__(self):
 
     def demo(self):
         """
-        Declare three poses for the robot to move to, each one corresponding to the position of a box
-        Pick up the box, and set it down 0.1m to the right of its original position
+        Declare three poses for the robot to move to, each one corresponding to
+        the position of a box.
+        Pick up the box, and set it down 0.1m to the right of its original position.
         """
         poses = [
             [0.4, 0.3, 0.12, 0, 1, 0, 0],
diff --git a/mplib/examples/demo_setup.py b/mplib/examples/demo_setup.py
index 27909b9..3b9e689 100644
--- a/mplib/examples/demo_setup.py
+++ b/mplib/examples/demo_setup.py
@@ -4,12 +4,13 @@
 from sapien.utils.viewer import Viewer
 
 import mplib
-import numpy as np
+
 
 class DemoSetup:
     """
-    This class is the super class to abstract away some of the setups for the demos
-    You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer
+    This class is the super class to abstract away some of the setups for the demos.
+    You will need to install Sapien via `pip install sapien` for this to work
+    if you want to use the viewer.
     """
 
     def __init__(self):
@@ -74,7 +75,7 @@ def setup_scene(self, **kwargs):
 
     def load_robot(self, **kwargs):
         """
-        This function loads a robot from a URDF file into the Sapien Scene created above.
+        This function loads a robot from a URDF file into the Sapien Scene.
         Note that does mean that setup_scene() must be called before this function.
         """
         loader: sapien.URDFLoader = self.scene.create_urdf_loader()
@@ -97,8 +98,8 @@ def load_robot(self, **kwargs):
 
     def setup_planner(self, **kwargs):
         """
-        Create an mplib planner using the default robot
-        see planner.py for more details on the arguments
+        Create an mplib planner using the default robot.
+        See planner.py for more details on the arguments.
         """
         self.planner = mplib.Planner(
             urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
@@ -110,13 +111,15 @@ def follow_path(self, result):
         """Helper function to follow a path generated by the planner"""
         # number of waypoints in the path
         n_step = result["position"].shape[0]
-        # this makes sure the robot stays neutrally boyant instead of sagging under gravity
+        # this makes sure the robot stays neutrally boyant instead of sagging
+        # under gravity
         for i in range(n_step):
             qf = self.robot.compute_passive_force(
                 external=False, gravity=True, coriolis_and_centrifugal=True
             )
             self.robot.set_qf(qf)
-            # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
+            # set the joint positions and velocities for move group joints only.
+            # The others are not the responsibility of the planner
             for j in range(len(self.planner.move_group_joint_indices)):
                 self.active_joints[j].set_drive_target(result["position"][i][j])
                 self.active_joints[j].set_drive_velocity_target(
@@ -163,10 +166,13 @@ def move_to_pose_with_RRTConnect(
 
         Args:
             pose: [x, y, z, qx, qy, qz, qw]
-            use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
-            use_attach (optional): if to take the attach into consideration for collision checking.
+            use_point_cloud (optional): if to take the point cloud into consideration
+                for collision checking.
+            use_attach (optional): if to take the attach into consideration
+                for collision checking.
         """
-        # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
+        # result is a dictionary with keys 'status', 'time', 'position', 'velocity',
+        # 'acceleration', 'duration'
         result = self.planner.plan_qpos_to_pose(
             pose,
             self.robot.get_qpos(),
diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py
index 7ccca2a..2bf78ed 100644
--- a/mplib/examples/detect_collision.py
+++ b/mplib/examples/detect_collision.py
@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 
 import mplib
-
 from mplib.examples.demo_setup import DemoSetup
 
 
@@ -29,11 +28,16 @@ def print_collisions(self, collisions):
     def demo(self):
         """
         We test several configurations:
-        1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
-        2. Set robot to a self-collision qpos and check for self-collision returns a collision
-        3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
-        4. Set robot to a env-collision qpos and check for env-collision returns a collision
-        5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
+        1. Set robot to a self-collision-free qpos and check for self-collision returns
+           no collision
+        2. Set robot to a self-collision qpos and check for self-collision returns
+           a collision
+        3. Set robot to a env-collision-free qpos and check for env-collision returns
+           no collision
+        4. Set robot to a env-collision qpos and check for env-collision returns
+           a collision
+        5. Attempts to plan a path to a qpos is in collision with the world.
+           This will cause the planner to timeout
         6. Remove the floor and check for env-collision returns no collision
         """
         floor = mplib.planner.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
@@ -45,7 +49,8 @@ def demo(self):
         self.planner.set_normal_object("floor", floor_fcl_collision_object)
 
         print("\n----- self-collision-free qpos -----")
-        # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
+        # if the joint qpos does not include the gripper joints,
+        # it will be set to the current gripper joint angle
         self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
         self.print_collisions(
             self.planner.check_for_self_collision(
diff --git a/mplib/examples/moving_robot.py b/mplib/examples/moving_robot.py
index 02a933a..d8fc069 100644
--- a/mplib/examples/moving_robot.py
+++ b/mplib/examples/moving_robot.py
@@ -4,21 +4,28 @@
 
 
 class PlanningDemo(DemoSetup):
-    """This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively"""
+    """
+    This is identical to demo.py except the whole scene is shifted to
+    the bottom right by 1 meter respectively
+    """
 
     def __init__(self):
         """
-        Setting up the scene, the planner, and adding some objects to the scene
-        Note that we place the robot at [1,1,0] in the simulation and also tell the planner that the robot is at [1,1,0]
-        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
+        Setting up the scene, the planner, and adding some objects to the scene.
+        Note that we place the robot at [1, 1, 0] in the simulation and
+        also tell the planner that the robot is at [1, 1, 0].
+        Afterwards, put down a table and three boxes.
+        For details on how to do this, see the sapien documentation.
+        Compared to demo.py, all the props are shifted by 1 meter
+        in the x and y direction.
         """
         super().__init__()
         self.setup_scene()
         self.load_robot(robot_origin_xyz=[1, 1, 0])
         self.setup_planner()
 
-        # We also need to tell the planner where the base is since the sim and planner don't share info
+        # We also need to tell the planner where the base is
+        # since the sim and planner don't share info
         self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
 
         # Set initial joint positions
@@ -54,9 +61,10 @@ def __init__(self):
     def demo(self):
         """
         Same demo as demo.py.
-        Since we shifted the robot, we also need to shift the poses by 1 meter in the x and y direction
+        Since we shifted the robot, we also need to shift the poses
+        by 1 meter in the x and y direction.
         Alternatively, we can keep the poses the same and tell the planner
-        the poses are specified with respect to the base of the robot
+        the poses are specified with respect to the base of the robot.
         """
         poses = [
             [1.4, 1.3, 0.12, 0, 1, 0, 0],
diff --git a/mplib/examples/two_stage_motion.py b/mplib/examples/two_stage_motion.py
index 0f87649..309cc0b 100644
--- a/mplib/examples/two_stage_motion.py
+++ b/mplib/examples/two_stage_motion.py
@@ -1,18 +1,22 @@
+import sys
+
 import numpy as np
 import sapien.core as sapien
 
 import mplib
-
 from mplib.examples.demo_setup import DemoSetup
 
 
 class PlanningDemo(DemoSetup):
     """
-    This demo is the same as collision_avoidance.py except we added a track for the robot to move along
+    This demo is the same as collision_avoidance.py except we added a track
+    for the robot to move along.
+
     We reach the target in two stages:
     1. First, we move the base while fixing the arm joints
     2. Then, we move the arm while fixing the base joints
-    This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
+    This corresponds to a mobile robot which can move in the x and y direction
+    with a manipulator on top
     """
 
     def __init__(self):
@@ -108,7 +112,7 @@ def plan_without_base(self, pose, has_attach=False):
         )
         if status != "Success":
             print("IK failed")
-            exit(1)
+            sys.exit(1)
         # now fix base and plan a path to the goal
         result = self.planner.plan_qpos_to_qpos(
             goal_qposes,
@@ -129,7 +133,7 @@ def move_in_two_stage(self, pose, has_attach=False):
         status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
         if status != "Success":
             print("IK failed")
-            exit(1)
+            sys.exit(1)
         # now fix arm joints and plan a path to the goal
         result = self.planner.plan_qpos_to_qpos(
             goal_qposes,
@@ -147,7 +151,8 @@ def move_in_two_stage(self, pose, has_attach=False):
 
     def demo(self):
         """
-        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
+        We reach the pick up and drop off poses in two stages,
+        first by moving the base only and then the arm only
         """
         pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
         delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
diff --git a/mplib/planner.py b/mplib/planner.py
index 8e2dcfa..087c5a0 100644
--- a/mplib/planner.py
+++ b/mplib/planner.py
@@ -1,5 +1,7 @@
+from __future__ import annotations
+
 import os
-from typing import Sequence, Tuple, Union
+from typing import Optional, Sequence
 
 import numpy as np
 import toppra as ta
@@ -7,11 +9,11 @@
 import toppra.constraint as constraint
 from transforms3d.quaternions import mat2quat, quat2mat
 
-from mplib.pymp import *
+from mplib.pymp import articulation, ompl, planning_world
 
 
 class Planner:
-    """Motion planner."""
+    """Motion planner"""
 
     # TODO(jigu): default joint vel and acc limits
     # TODO(jigu): how does user link names and joint names are exactly used?
@@ -24,16 +26,18 @@ def __init__(
         package_keyword_replacement: str = "",
         user_link_names: Sequence[str] = [],
         user_joint_names: Sequence[str] = [],
-        joint_vel_limits: Union[Sequence[float], np.ndarray] = [],
-        joint_acc_limits: Union[Sequence[float], np.ndarray] = [],
+        joint_vel_limits: Optional[Sequence[float] | np.ndarray] = None,
+        joint_acc_limits: Optional[Sequence[float] | np.ndarray] = None,
         **kwargs,
     ):
         """Motion planner for robots.
 
         Args:
             urdf: Unified Robot Description Format file.
-            user_link_names: names of links, the order. if empty, all links will be used.
-            user_joint_names: names of the joints to plan.  if empty, all active joints will be used.
+            user_link_names: names of links, the order matters.
+                If empty, all links will be used.
+            user_joint_names: names of the joints to plan.
+                If empty, all active joints will be used.
             move_group: target link to move, usually the end-effector.
             joint_vel_limits: maximum joint velocities for time parameterization,
                 which should have the same length as
@@ -44,6 +48,10 @@ def __init__(
             http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
 
         """
+        if joint_vel_limits is None:
+            joint_vel_limits = []
+        if joint_acc_limits is None:
+            joint_acc_limits = []
         self.urdf = urdf
         if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
             self.srdf = urdf.replace(".urdf", ".srdf")
@@ -132,7 +140,7 @@ def replace_package_keyword(self, package_keyword_replacement):
             package_keyword_replacement: the string to replace 'package://' keyword
         """
         rtn_urdf = self.urdf
-        with open(self.urdf, "r") as in_f:
+        with open(self.urdf) as in_f:
             content = in_f.read()
             if "package://" in content:
                 rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
@@ -144,8 +152,9 @@ def replace_package_keyword(self, package_keyword_replacement):
 
     def generate_collision_pair(self, sample_time=1000000, echo_freq=100000):
         """
-        we read the srdf file to get the link pairs that should not collide.
-        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
+        We read the srdf file to get the link pairs that should not collide.
+        If not provided, we need to randomly sample configurations
+        to find the link pairs that will always collide.
         """
         print(
             "Since no SRDF file is provided. We will first detect link pairs that will"
@@ -178,18 +187,18 @@ def generate_collision_pair(self, sample_time=1000000, echo_freq=100000):
                     link1 = self.user_link_names[i]
                     link2 = self.user_link_names[j]
                     print(
-                        "Ignore collision pair: (%s, %s), reason:  always collide"
-                        % (link1, link2)
+                        f"Ignore collision pair: ({link1}, {link2}), "
+                        "reason: always collide"
                     )
                     collision = ET.SubElement(root, "disable_collisions")
                     collision.set("link1", link1)
                     collision.set("link2", link2)
                     collision.set("reason", "Default")
-        srdffile = open(self.srdf, "w")
-        srdffile.write(
-            minidom.parseString(ET.tostring(root)).toprettyxml(indent="    ")
-        )
-        srdffile.close()
+        with open(self.srdf, "w") as srdf_file:
+            srdf_file.write(
+                minidom.parseString(ET.tostring(root)).toprettyxml(indent="    ")
+            )
+            srdf_file.close()
         print("Saving the SRDF file to %s" % self.srdf)
 
     def distance_6D(self, p1, q1, p2, q2):
@@ -313,7 +322,8 @@ def check_for_env_collision(
             articulation: robot model. if none will be self.robot
             qpos: robot configuration. if none will be the current pose
             with_point_cloud: whether to check collision against point cloud
-            use_attach: whether to include the object attached to the end effector in collision checking
+            use_attach: whether to include the object attached to the end effector
+                in collision checking
         Returns:
             A list of collisions.
         """
@@ -332,25 +342,29 @@ def check_for_env_collision(
         self.planning_world.set_use_attach(prev_use_attach)
         return results
 
-    def IK(self, goal_pose, start_qpos, mask=[], n_init_qpos=20, threshold=1e-3):
+    def IK(self, goal_pose, start_qpos, mask=None, n_init_qpos=20, threshold=1e-3):
         """
         Inverse kinematics
 
         Args:
             goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
             start_qpos: initial configuration
-            mask: if the value at a given index is True, the joint is *not* used in the IK
+            mask: if the value at a given index is True,
+                the joint is *not* used in the IK
             n_init_qpos: number of random initial configurations
-            threshold: threshold for the distance between the goal pose and the result pose
+            threshold: threshold for the distance between the goal pose and
+                the result pose
         """
 
+        if mask is None:
+            mask = []
         index = self.link_name_2_idx[self.move_group]
         min_dis = 1e9
         idx = self.move_group_joint_indices
         qpos0 = np.copy(start_qpos)
         results = []
         self.robot.set_qpos(start_qpos, True)
-        for i in range(n_init_qpos):
+        for _ in range(n_init_qpos):
             ik_results = self.pinocchio_model.compute_IK_CLIK(
                 index, goal_pose, start_qpos, mask
             )
@@ -386,7 +400,7 @@ def IK(self, goal_pose, start_qpos, mask=[], n_init_qpos=20, threshold=1e-3):
         if len(results) != 0:
             status = "Success"
         elif min_dis != 1e9:
-            status = "IK Failed! Distance %lf is greater than threshold %lf." % (
+            status = "IK Failed! Distance {:f} is greater than threshold {:f}.".format(
                 min_dis,
                 threshold,
             )
@@ -416,6 +430,8 @@ def TOPP(self, path, step=0.1, verbose=False):
             [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
         )
         jnt_traj = instance.compute_trajectory()
+        if jnt_traj is None:
+            raise RuntimeError("Fail to parameterize path")
         ts_sample = np.linspace(0, jnt_traj.duration, int(jnt_traj.duration / step))
         qs_sample = jnt_traj(ts_sample)
         qds_sample = jnt_traj(ts_sample, 1)
@@ -426,7 +442,8 @@ def update_point_cloud(self, pc, radius=1e-3):
         """
         Args:
             pc: numpy array of shape (n, 3)
-            radius: radius of each point. This gives a buffer around each point that planner will avoid
+            radius: radius of each point. This gives a buffer around each point
+                that planner will avoid
         """
         self.planning_world.update_point_cloud(pc, radius)
 
@@ -508,7 +525,7 @@ def plan_qpos_to_qpos(
         constraint_function=None,
         constraint_jacobian=None,
         constraint_tolerance=1e-3,
-        fixed_joint_indices=[],
+        fixed_joint_indices=None,
     ):
         """
         plan a path from a specified joint position to a goal pose
@@ -516,21 +533,27 @@ def plan_qpos_to_qpos(
         Args:
             goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
             current_qpos: current joint configuration (either full or move_group joints)
-            mask: mask for IK. When set, the IK will leave certain joints out of planning
+            mask: mask for IK. When set, the IK will leave certain joints out of
+                planning
             time_step: time step for TOPP
             rrt_range: step size for RRT
             planning_time: time limit for RRT
-            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+            fix_joint_limits: if True, will clip the joint configuration to be within
+                the joint limits
             use_point_cloud: if True, will use the point cloud to avoid collision
             use_attach: if True, will consider the attached tool collision when planning
             verbose: if True, will print the log of OMPL and TOPPRA
             planner_name: planner name pick from {"RRTConnect", "RRT*"}
-            fixed_joint_indices: list of indices of joints that are fixed during planning
+            fixed_joint_indices: list of indices of joints that are fixed during
+                planning
             constraint_function: evals to 0 when constraint is satisfied
             constraint_jacobian: jacobian of constraint_function
             constraint_tolerance: tolerance for constraint_function
-            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
+            no_simplification: if true, will not simplify the path. constraint planning
+                does not support simplification
         """
+        if fixed_joint_indices is None:
+            fixed_joint_indices = []
         self.planning_world.set_use_point_cloud(use_point_cloud)
         self.planning_world.set_use_attach(use_attach)
         n = current_qpos.shape[0]
@@ -548,15 +571,11 @@ def plan_qpos_to_qpos(
         if len(collisions) != 0:
             print("Invalid start state!")
             for collision in collisions:
-                print(
-                    "%s and %s collide!" % (collision.link_name1, collision.link_name2)
-                )
+                print(f"{collision.link_name1} and {collision.link_name2} collide!")
 
         idx = self.move_group_joint_indices
 
-        goal_qpos_ = []
-        for i in range(len(goal_qposes)):
-            goal_qpos_.append(goal_qposes[i][idx])
+        goal_qpos_ = [goal_qposes[i][idx] for i in range(len(goal_qposes))]
 
         fixed_joints = set()
         for joint_idx in fixed_joint_indices:
@@ -612,7 +631,7 @@ def plan_qpos_to_pose(
         self,
         goal_pose,
         current_qpos,
-        mask=[],
+        mask=None,
         time_step=0.1,
         rrt_range=0.1,
         planning_time=1,
@@ -633,16 +652,21 @@ def plan_qpos_to_pose(
         Args:
             goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
             current_qpos: current joint configuration (either full or move_group joints)
-            mask: if the value at a given index is True, the joint is *not* used in the IK
+            mask: if the value at a given index is True, the joint is *not* used in the
+                IK
             time_step: time step for TOPPRA (time parameterization of path)
             rrt_range: step size for RRT
             planning_time: time limit for RRT
-            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+            fix_joint_limits: if True, will clip the joint configuration to be within
+                the joint limits
             use_point_cloud: if True, will use the point cloud to avoid collision
             use_attach: if True, will consider the attached tool collision when planning
             verbose: if True, will print the log of OMPL and TOPPRA
-            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+            wrt_world: if true, interpret the target pose with respect to
+                the world frame instead of the base frame
         """
+        if mask is None:
+            mask = []
         n = current_qpos.shape[0]
         if fix_joint_limits:
             for i in range(n):
@@ -654,9 +678,8 @@ def plan_qpos_to_pose(
         if wrt_world:
             goal_pose = self.transform_goal_to_wrt_base(goal_pose)
 
-        idx = (
-            self.move_group_joint_indices
-        )  # we need to take only the move_group joints when planning
+        # we need to take only the move_group joints when planning
+        # idx = self.move_group_joint_indices
 
         ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
         if ik_status != "Success":
@@ -667,9 +690,7 @@ def plan_qpos_to_pose(
             for i in range(len(goal_qpos)):
                 print(goal_qpos[i])
 
-        goal_qpos_ = []
-        for i in range(len(goal_qpos)):
-            goal_qpos_.append(goal_qpos[i][idx])
+        # goal_qpos_ = [goal_qpos[i][idx] for i in range(len(goal_qpos))]
         self.robot.set_qpos(current_qpos, True)
 
         ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
@@ -710,17 +731,19 @@ def plan_screw(
         wrt_world=True,
     ):
         """
-        plan from a start configuration to a goal pose of the end-effector using screw motion
+        Plan from a start configuration to a goal pose of the end-effector using
+        screw motion
 
         Args:
-            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+            target_pose: [x, y, z, qw, qx, qy, qz] pose of the goal
             qpos: current joint configuration (either full or move_group joints)
             qpos_step: size of the random step for RRT
             time_step: time step for the discretization
             use_point_cloud: if True, will use the point cloud to avoid collision
             use_attach: if True, will use the attached tool to avoid collision
             verbose: if True, will print the log of TOPPRA
-            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+            wrt_world: if True, interpret the target pose with respect to the
+                world frame instead of the base frame
         """
         self.planning_world.set_use_point_cloud(use_point_cloud)
         self.planning_world.set_use_attach(use_attach)
@@ -743,7 +766,7 @@ def skew(vec):
                 [-vec[1], vec[0], 0],
             ])
 
-        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
+        def pose2exp_coordinate(pose: np.ndarray) -> tuple[np.ndarray, float]:
             def rot2so3(rotation: np.ndarray):
                 assert rotation.shape == (3, 3)
                 if np.isclose(rotation.trace(), 3):
@@ -821,11 +844,7 @@ def check_joint_limit(q):
             self.planning_world.set_qpos_all(qpos[index])
             collide = self.planning_world.collide()
 
-            if (
-                np.linalg.norm(delta_twist) < 1e-4
-                or collide
-                or within_joint_limit == False
-            ):
+            if np.linalg.norm(delta_twist) < 1e-4 or collide or not within_joint_limit:
                 return {"status": "screw plan failed"}
 
             path.append(np.copy(qpos[index]))
diff --git a/pyproject.toml b/pyproject.toml
index 5e06b58..56751b3 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -87,12 +87,15 @@ select = [
   "TRY200",
   "TRY302",
   "RUF008",  # mutable dataclass default
+  "RUF013",  # implicit typing.Optional is disallowed
   "RUF015",  # access first ele in constant time
   "RUF016",  # type error non-integer index
   "RUF017",  # avoid quadratic list summation
 ]
 ignore = [
+  "UP007",   # allow typing.Optional[int], instead of forcing int | None
   "EXE002",  # ntfs has no executable bit
+  "SIM105",  # allow try-except-pass blocks
 ]
 exclude = ["third_party"]
 
@@ -101,7 +104,25 @@ exclude = ["third_party"]
   "F401",  # unused import
 ]
 "*.pyi" = [
+  "F821",  # allow undefined name
+  "E501",  # line too long
+  "PYI001",  # allow TypeVar name without '_' prefix
   "PYI021",  # allow docstrings in stub files
+  "PYI029",  # allow __str__ and __repr__ in stubs
+  "PYI032",  # allow typing.Any annotations for __eq__ and __ne__
+  "PYI054",  # allow long numeric literals
+]
+# TODO: might be fixable
+# related to fixed_joints, constraint_function, constraint_jacobian
+"mplib/pymp/ompl.pyi" = [
+  "B006",
+  "PYI011",
+  "RUF013",
+]
+# TODO: might be fixable
+# related to compute_IK_CLIK() input argument mask = []
+"mplib/pymp/pinocchio.pyi" = [
+  "B006",
 ]
 
 [tool.ruff.format]
diff --git a/setup.py b/setup.py
index c5b51d8..9332c52 100644
--- a/setup.py
+++ b/setup.py
@@ -1,4 +1,3 @@
-# -*- coding: utf-8 -*-
 import os
 import subprocess
 import sys
@@ -28,7 +27,7 @@ def build_extension(self, ext: CMakeExtension) -> None:
 
         # CMake lets you override the generator - we need to check this.
         # Can be set with Conda-Build, for example.
-        cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
+        # cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
 
         # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON
         cmake_args = [
@@ -47,13 +46,16 @@ def build_extension(self, ext: CMakeExtension) -> None:
 
         # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level
         # across all generators.
+        # self.parallel is a Python 3 only way to set parallel jobs by hand
+        # using -j in the build_ext call, not supported by pip or PyPA-build.
         self.parallel = 16
-        if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ:
-            # self.parallel is a Python 3 only way to set parallel jobs by hand
-            # using -j in the build_ext call, not supported by pip or PyPA-build.
-            if hasattr(self, "parallel") and self.parallel:
-                # CMake 3.12+ only.
-                build_args += [f"-j{self.parallel}"]
+        if (
+            "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ
+            and hasattr(self, "parallel")
+            and self.parallel
+        ):
+            # CMake 3.12+ only.
+            build_args += [f"-j{self.parallel}"]
 
         build_temp = Path(self.build_temp) / ext.name
         if not build_temp.exists():

From d903a5694bd0de9f47409cdebf36103ba569f9e0 Mon Sep 17 00:00:00 2001
From: Kolin Guo <ruguo@ucsd.edu>
Date: Sun, 21 Jan 2024 00:25:47 -0800
Subject: [PATCH 2/6] Fix all C++ compile warnings; Add -Werror to
 CMAKE_CXX_FLAGS_RELEASE

---
 CMakeLists.txt            |  2 +-
 src/articulated_model.cpp |  8 ++++----
 src/fcl_model.cpp         | 36 ++++++------------------------------
 src/kdl_model.cpp         |  6 +++---
 src/kdl_model.h           |  2 +-
 src/ompl_planner.cpp      | 15 ++++++++-------
 src/ompl_planner.h        | 12 ++++++++----
 src/pinocchio_model.cpp   | 36 +++++++++++++++++++++---------------
 src/planning_world.cpp    | 18 +++++++++---------
 src/planning_world.h      |  8 ++++----
 src/urdf_utils.cpp        |  8 ++++----
 11 files changed, 69 insertions(+), 82 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6e91b5b..6b76d86 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.12)
 project(mp LANGUAGES CXX)
 set(CMAKE_CXX_STANDARD 17)
 
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g3 -Wall -fsized-deallocation -Wno-deprecated-declarations")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g3 -Wall -Werror -fsized-deallocation -Wno-deprecated-declarations")
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
 
diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp
index 3ea3bb8..d35fe38 100644
--- a/src/articulated_model.cpp
+++ b/src/articulated_model.cpp
@@ -32,9 +32,9 @@ ArticulatedModelTpl<DATATYPE>::ArticulatedModelTpl(
     const std::string &urdf_filename, const std::string &srdf_filename,
     const Vector3 &gravity, const std::vector<std::string> &joint_names,
     const std::vector<std::string> &link_names, const bool &verbose, const bool &convex)
-    : verbose(verbose),
-      pinocchio_model(urdf_filename, gravity, verbose),
-      fcl_model(urdf_filename, verbose, convex) {
+    : pinocchio_model(urdf_filename, gravity, verbose),
+      fcl_model(urdf_filename, verbose, convex),
+      verbose(verbose) {
   user_link_names =
       link_names.size() == 0 ? pinocchio_model.getLinkNames(false) : link_names;
   user_joint_names =
@@ -85,7 +85,7 @@ void ArticulatedModelTpl<DATATYPE>::setQpos(const VectorX &qpos, const bool &ful
   if (full)
     current_qpos = qpos;
   else {
-    ASSERT(qpos.size() == move_group_qpos_dim,
+    ASSERT(static_cast<size_t>(qpos.size()) == move_group_qpos_dim,
            "Length of provided qpos " + std::to_string(qpos.size()) +
                " =/= dimension of move_group qpos: " +
                std::to_string(move_group_qpos_dim));
diff --git a/src/fcl_model.cpp b/src/fcl_model.cpp
index 27f6efe..0bbdfb9 100644
--- a/src/fcl_model.cpp
+++ b/src/fcl_model.cpp
@@ -24,7 +24,6 @@ void FCLModelTpl<DATATYPE>::dfs_parse_tree(const urdf::LinkConstSharedPtr &link,
 
   if (link->collision) {
     for (auto geom : link->collision_array) {
-      const std::string &geom_name = geom->name;
       auto geom_model = geom->geometry;
       CollisionGeometry_ptr collision_geometry = nullptr;
       auto pose = Transform3::Identity();
@@ -116,14 +115,14 @@ template <typename DATATYPE>
 FCLModelTpl<DATATYPE>::FCLModelTpl(const urdf::ModelInterfaceSharedPtr &urdfTree,
                                    const std::string &package_dir, const bool &verbose,
                                    const bool &convex)
-    : verbose(verbose), use_convex(convex) {
+    : use_convex(convex), verbose(verbose) {
   init(urdfTree, package_dir);
 }
 
 template <typename DATATYPE>
 FCLModelTpl<DATATYPE>::FCLModelTpl(const std::string &urdf_filename,
                                    const bool &verbose, const bool &convex)
-    : verbose(verbose), use_convex(convex) {
+    : use_convex(convex), verbose(verbose) {
   auto found = urdf_filename.find_last_of("/\\");
   auto urdf_dir = found != urdf_filename.npos ? urdf_filename.substr(0, found) : ".";
   urdf::ModelInterfaceSharedPtr urdfTree = urdf::parseURDFFile(urdf_filename);
@@ -168,35 +167,12 @@ void FCLModelTpl<DATATYPE>::removeCollisionPairsFromSrdf(
     if (node.first == "disable_collisions") {
       const std::string link1 = node.second.get<std::string>("<xmlattr>.link1");
       const std::string link2 = node.second.get<std::string>("<xmlattr>.link2");
-      /*
-      // Check first if the two bodies exist in model
-      if (!model.existBodyName(link1) || !model.existBodyName(link2)) {
-          if (verbose)
-              std::cout << "It seems that " << link1 << " or " << link2 <<
-                  " do not exist in model. Skip." << std::endl;
-          continue;
-      }
-
-      FrameIndex frame_id1 = model.getBodyId(link1);
-      FrameIndex frame_id2 = model.getBodyId(link2);
-
-      if ((frame_id1 == model.nframes || frame_id2 == model.nframes) && logging_level >
-      90) { std::cout << "Links do not exist " << link1 << " " << link2 << std::endl;
-          continue;
-      }
-      // Malformed SRDF
-      if (frame_id1 == frame_id2) {
-          if (verbose)
-              std::cout << "Cannot disable collision between " << link1 << " and " <<
-      link2 << std::endl; continue; } else if (frame_id1 > frame_id2)
-          std::swap(frame_id1, frame_id2);
-      */
       if (verbose) print_verbose("Try to Remove collision parts: ", link1, " ", link2);
       for (auto iter = collision_pairs.begin(); iter != collision_pairs.end();) {
-        if (collision_link_names[iter->first] == link1 &&
-                collision_link_names[iter->second] == link2 ||
-            collision_link_names[iter->first] == link2 &&
-                collision_link_names[iter->second] == link1) {
+        if ((collision_link_names[iter->first] == link1 &&
+             collision_link_names[iter->second] == link2) ||
+            (collision_link_names[iter->first] == link2 &&
+             collision_link_names[iter->second] == link1)) {
           iter = collision_pairs.erase(iter);
         } else
           iter++;
diff --git a/src/kdl_model.cpp b/src/kdl_model.cpp
index 12801bb..c81fe70 100644
--- a/src/kdl_model.cpp
+++ b/src/kdl_model.cpp
@@ -11,9 +11,9 @@ KDLModelTpl<DATATYPE>::KDLModelTpl(const std::string &urdf_filename,
                                    const std::vector<std::string> &joint_names,
                                    const std::vector<std::string> &link_names,
                                    const bool &verbose)
-    : user_joint_names(joint_names), user_link_names(link_names), verbose(verbose) {
+    : user_link_names(link_names), user_joint_names(joint_names), verbose(verbose) {
   // std::cout << "Verbose" << verbose << std::endl;
-  for (int i = 0; i < joint_names.size(); i++)
+  for (size_t i = 0; i < joint_names.size(); i++)
     user_joint_idx_mapping[joint_names[i]] = i;
   urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDFFile(urdf_filename);
   treeFromUrdfModel(urdf, tree, tree_root_name, verbose);
@@ -157,7 +157,7 @@ KDLModelTpl<DATATYPE>::TreeIKNRJL(const std::vector<std::string> endpoints,
   }
 
   KDL::Frames frames;
-  for (int i = 0; i < endpoints.size(); i++) {
+  for (size_t i = 0; i < endpoints.size(); i++) {
     frames[endpoints[i]] = KDL::Frame(
         KDL::Rotation::Quaternion(poses[i][4], poses[i][5], poses[i][6], poses[i][3]),
         KDL::Vector(poses[i][0], poses[i][1], poses[i][2]));
diff --git a/src/kdl_model.h b/src/kdl_model.h
index e8ca891..f0cf70a 100644
--- a/src/kdl_model.h
+++ b/src/kdl_model.h
@@ -29,11 +29,11 @@ class KDLModelTpl {
   DEFINE_TEMPLATE_EIGEN(DATATYPE)
   KDL::Tree tree;
   std::string tree_root_name;
-  bool verbose;
   std::vector<std::string> user_link_names;
   std::vector<std::string> user_joint_names;
   std::vector<int> joint_mapping_kdl_2_user;
   std::map<std::string, int> user_joint_idx_mapping;
+  bool verbose;
 
  public:
   KDLModelTpl(const std::string &urdf_filename,
diff --git a/src/ompl_planner.cpp b/src/ompl_planner.cpp
index ffb2f64..2f4e77d 100644
--- a/src/ompl_planner.cpp
+++ b/src/ompl_planner.cpp
@@ -207,7 +207,7 @@ Eigen::MatrixXd OMPLPlannerTpl<DATATYPE>::simplify_path(Eigen::MatrixXd &path) {
     print_warning("doing simplification in non-constrained space");
   }
   og::PathGeometric geo_path(si);
-  for (size_t i = 0; i < path.rows(); i++) {
+  for (size_t i = 0; i < static_cast<size_t>(path.rows()); i++) {
     ob::ScopedState<> state(cs);
     state = eigen2vector<DATATYPE, double>(path.row(i));
     geo_path.append(state.get());
@@ -229,7 +229,7 @@ std::shared_ptr<ob::GoalStates> OMPLPlannerTpl<DATATYPE>::make_goal_states(
   int tot_enum_states = 1, tot_goal_state = 0;
   for (size_t i = 0; i < dim; i++) tot_enum_states *= 3;
 
-  for (int ii = 0; ii < goal_states.size(); ii++) {
+  for (size_t ii = 0; ii < goal_states.size(); ii++) {
     for (int i = 0; i < tot_enum_states; i++) {
       std::vector<double> tmp_state;
       int tmp = i;
@@ -275,9 +275,9 @@ OMPLPlannerTpl<DATATYPE>::plan(
     const VectorX &start_state, const std::vector<VectorX> &goal_states,
     const std::string &planner_name, const double &time, const double &range,
     const bool verbose, const FixedJoints &fixed_joints, const bool no_simplification,
-    std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+    const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
         &constraint_function,
-    std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+    const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
         &constraint_jacobian,
     double constraint_tolerance) {
   if (fixed_joints.size() || last_fixed_joints.size()) {
@@ -308,7 +308,7 @@ OMPLPlannerTpl<DATATYPE>::plan(
 
   ASSERT(reduced_start_state.rows() == reduced_goal_states[0].rows(),
          "Length of start state and goal state should be equal");
-  ASSERT(reduced_start_state.rows() == dim,
+  ASSERT(static_cast<size_t>(reduced_start_state.rows()) == dim,
          "Length of start state and problem dimension should be equal");
   if (verbose == false) ompl::msg::noOutputHandler();
 
@@ -365,9 +365,10 @@ OMPLPlannerTpl<DATATYPE>::plan(
     for (size_t i = 0; i < len; i++) {
       auto res_i = state2eigen<DATATYPE>(path.getState(i), si.get(),
                                          state_space == p_constrained_space);
-      ASSERT(res_i.rows() == dim, "Result dimension is not correct!");
+      ASSERT(static_cast<size_t>(res_i.rows()) == dim,
+             "Result dimension is not correct!");
       res_i = add_fixed_joints(fixed_joints, res_i);
-      for (size_t j = 0; j < start_state.rows(); j++)
+      for (size_t j = 0; j < static_cast<size_t>(start_state.rows()); j++)
         ret(invalid_start + i, j) = res_i[j];
     }
     return std::make_pair(solved.asString(), ret);
diff --git a/src/ompl_planner.h b/src/ompl_planner.h
index b840ee4..114d4be 100644
--- a/src/ompl_planner.h
+++ b/src/ompl_planner.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <functional>
+
 #include <ompl/base/ConstrainedSpaceInformation.h>
 #include <ompl/base/Constraint.h>
 #include <ompl/base/SpaceInformation.h>
@@ -160,8 +162,10 @@ class GeneralConstraint : public ob::Constraint {
  public:
   GeneralConstraint(
       size_t dim,
-      std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)> &f,
-      std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)> &j)
+      const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+          &f,
+      const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+          &j)
       : ob::Constraint(dim, 1), f(f), j(j) {}
 
   void function(const Eigen::Ref<const Eigen::VectorXd> &x,
@@ -289,9 +293,9 @@ class OMPLPlannerTpl {
       const double &range = 0.0, const bool verbose = false,
       const FixedJoints &fixed_joints = FixedJoints(),
       const bool no_simplification = false,
-      std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+      const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
           &constraint_function = nullptr,
-      std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
+      const std::function<void(const Eigen::VectorXd &, Eigen::Ref<Eigen::VectorXd>)>
           &constraint_jacobian = nullptr,
       double constraint_tolerance = 1e-3);
 };
diff --git a/src/pinocchio_model.cpp b/src/pinocchio_model.cpp
index 0f18e75..1f06a5e 100644
--- a/src/pinocchio_model.cpp
+++ b/src/pinocchio_model.cpp
@@ -162,7 +162,7 @@ void PinocchioModelTpl<DATATYPE>::dfs_parse_tree(urdf::LinkConstSharedPtr link,
                                     " is not supported.");
     }
 
-    FrameIndex frame_id = visitor.getBodyId(link->name);
+    visitor.getBodyId(link->name);
     visitor << "Adding Body" << '\n'
             << '\"' << link_name << "\" connected to \"" << parent_link_name
             << "\" through joint \"" << joint_name << "\"\n"
@@ -231,7 +231,7 @@ Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>
 PinocchioModelTpl<DATATYPE>::qposUser2Pinocchio(const VectorX &q_user) {
   VectorX q_pinocchio(model.nq);
   size_t count = 0;
-  for (size_t i = 0; i < joint_index_user2pinocchio.size(); i++) {
+  for (size_t i = 0; i < static_cast<size_t>(joint_index_user2pinocchio.size()); i++) {
     auto start_idx = model.idx_qs[joint_index_user2pinocchio[i]];
     switch (nqs[i]) {
       case 0:  // FIX
@@ -252,7 +252,7 @@ PinocchioModelTpl<DATATYPE>::qposUser2Pinocchio(const VectorX &q_user) {
     }
     count += nvs[i];
   }
-  ASSERT(count == q_user.size(), "Qpos user2pinocchio failed");
+  ASSERT(count == static_cast<size_t>(q_user.size()), "Qpos user2pinocchio failed");
   return q_pinocchio;
 }
 
@@ -261,17 +261,17 @@ Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>
 PinocchioModelTpl<DATATYPE>::qposPinocchio2User(const VectorX &q_pinocchio) {
   VectorX q_user(model.nv);
   uint32_t count = 0;
-  for (size_t i = 0; i < joint_index_user2pinocchio.size(); i++) {
+  for (size_t i = 0; i < static_cast<size_t>(joint_index_user2pinocchio.size()); i++) {
     auto start_idx = model.idx_qs[joint_index_user2pinocchio[i]];
     switch (nqs[i]) {
       case 0:  // FIX
         break;
       case 1:  // REVOLUTE OR PRISMATIC
-        ASSERT(count < model.nv, "posP2S out of bound");
+        ASSERT(count < static_cast<size_t>(model.nv), "posP2S out of bound");
         q_user[count] = q_pinocchio[start_idx];
         break;
       case 2:  // CONTINUOUS
-        ASSERT(count < model.nv, "posS2P out of bound");
+        ASSERT(count < static_cast<size_t>(model.nv), "posS2P out of bound");
         q_user[count] = std::atan2(q_pinocchio[start_idx + 1], q_pinocchio[start_idx]);
         break;
       default:
@@ -304,7 +304,7 @@ void PinocchioModelTpl<DATATYPE>::setJointOrder(const std::vector<std::string> &
   size_t len_v = 0, len_q = 0;
   for (size_t i = 0; i < names.size(); i++) {
     auto pinocchio_idx = model.getJointId(names[i]);
-    if (pinocchio_idx == model.njoints)
+    if (pinocchio_idx == static_cast<size_t>(model.njoints))
       throw std::invalid_argument(
           names[i] + " is either an invalid or unsupported joint in your URDF");
     vidx[i] = len_v;
@@ -318,7 +318,7 @@ void PinocchioModelTpl<DATATYPE>::setJointOrder(const std::vector<std::string> &
     for (int j = 0; j < model.nvs[pinocchio_idx]; j++)
       v_index_user2pinocchio[len_v++] = model.idx_vs[pinocchio_idx] + j;
   }
-  ASSERT(len_v == model.nv, "setJointOrder failed");
+  ASSERT(len_v == static_cast<size_t>(model.nv), "setJointOrder failed");
   v_map_user2pinocchio =
       Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic>(v_index_user2pinocchio);
 }
@@ -329,7 +329,7 @@ void PinocchioModelTpl<DATATYPE>::setLinkOrder(const std::vector<std::string> &n
   link_index_user2pinocchio = VectorXI(names.size());
   for (size_t i = 0; i < names.size(); i++) {
     auto pinocchio_idx = model.getFrameId(names[i], pinocchio::BODY);
-    if (pinocchio_idx == model.nframes)
+    if (pinocchio_idx == static_cast<size_t>(model.nframes))
       throw std::invalid_argument(names[i] + " is a invalid names in setLinkOrder");
     link_index_user2pinocchio[i] = pinocchio_idx;
   }
@@ -401,7 +401,8 @@ void PinocchioModelTpl<DATATYPE>::computeForwardKinematics(const VectorX &qpos)
 template <typename DATATYPE>
 Eigen::Matrix<DATATYPE, 7, 1> PinocchioModelTpl<DATATYPE>::getLinkPose(
     const size_t &index) {
-  ASSERT(index < link_index_user2pinocchio.size(), "The link index is out of bound!");
+  ASSERT(index < static_cast<size_t>(link_index_user2pinocchio.size()),
+         "The link index is out of bound!");
   auto frame = link_index_user2pinocchio[index];
   auto parent_joint = model.frames[frame].parent;
   auto link2joint = model.frames[frame].placement;
@@ -438,7 +439,8 @@ Eigen::Matrix<DATATYPE, 7, 1> PinocchioModelTpl<DATATYPE>::getLinkPose(
 template <typename DATATYPE>
 Eigen::Matrix<DATATYPE, 7, 1> PinocchioModelTpl<DATATYPE>::getJointPose(
     const size_t &index) {
-  ASSERT(index < joint_index_user2pinocchio.size(), "The link index is out of bound!");
+  ASSERT(index < static_cast<size_t>(joint_index_user2pinocchio.size()),
+         "The link index is out of bound!");
   auto frame = joint_index_user2pinocchio[index];
 
   /* std::cout << index << " " << user_joint_names[index] << " "<< frame << " " <<
@@ -464,7 +466,8 @@ void PinocchioModelTpl<DATATYPE>::computeFullJacobian(const VectorX &qpos) {
 template <typename DATATYPE>
 Eigen::Matrix<DATATYPE, 6, Eigen::Dynamic> PinocchioModelTpl<DATATYPE>::getLinkJacobian(
     const size_t &index, const bool &local) {
-  ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
+  ASSERT(index < static_cast<size_t>(link_index_user2pinocchio.size()),
+         "link index out of bound");
   auto frameId = link_index_user2pinocchio[index];
   auto jointId = model.frames[frameId].parent;
 
@@ -488,7 +491,8 @@ Eigen::Matrix<DATATYPE, 6, Eigen::Dynamic>
 PinocchioModelTpl<DATATYPE>::computeSingleLinkJacobian(const VectorX &qpos,
                                                        const size_t &index,
                                                        bool local) {
-  ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
+  ASSERT(index < static_cast<size_t>(link_index_user2pinocchio.size()),
+         "link index out of bound");
   auto frameId = link_index_user2pinocchio[index];
   auto jointId = model.frames[frameId].parent;
   auto link2joint = model.frames[frameId].placement;
@@ -513,7 +517,8 @@ PinocchioModelTpl<DATATYPE>::computeIKCLIK(const size_t &index, const Vector7 &p
                                            const std::vector<bool> &mask,
                                            const double &eps, const int &maxIter,
                                            const double &dt, const double &damp) {
-  ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
+  ASSERT(index < static_cast<size_t>(link_index_user2pinocchio.size()),
+         "link index out of bound");
   auto frameId = link_index_user2pinocchio[index];
   auto jointId = model.frames[frameId].parent;
 
@@ -572,7 +577,8 @@ PinocchioModelTpl<DATATYPE>::computeIKCLIKJL(const size_t &index, const Vector7
                                              const VectorX &q_min, const VectorX &q_max,
                                              const double &eps, const int &maxIter,
                                              const double &dt, const double &damp) {
-  ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
+  ASSERT(index < static_cast<size_t>(link_index_user2pinocchio.size()),
+         "link index out of bound");
   auto frameId = link_index_user2pinocchio[index];
   auto jointId = model.frames[frameId].parent;
 
diff --git a/src/planning_world.cpp b/src/planning_world.cpp
index 66430a6..4de5626 100644
--- a/src/planning_world.cpp
+++ b/src/planning_world.cpp
@@ -23,8 +23,8 @@ PlanningWorldTpl<DATATYPE>::PlanningWorldTpl(
       articulation_names(articulation_names),
       move_articulation_id(move_articulation_id),
       has_point_cloud(false),
-      use_point_cloud(false),
       has_attach(false),
+      use_point_cloud(false),
       use_attach(false) {
   ASSERT(articulations.size() == articulation_names.size(),
          "articulations and articulation_names should have the same size");
@@ -44,9 +44,8 @@ template <typename DATATYPE>
 void PlanningWorldTpl<DATATYPE>::updatePointCloud(const Matrixx3 &vertices,
                                                   const double &radius) {
   std::shared_ptr<octomap::OcTree> p_octree = std::make_shared<octomap::OcTree>(radius);
-  for (size_t i = 0; i < vertices.rows(); i++) {
-    p_octree->updateNode(vertices(i, 0), vertices(i, 1), vertices(i, 2), true);
-  }
+  for (const auto &row : vertices.rowwise())
+    p_octree->updateNode(octomap::point3d(row(0), row(1), row(2)), true);
   point_cloud = std::make_shared<CollisionObject>(
       std::make_shared<fcl::OcTree<DATATYPE>>(p_octree), Transform3::Identity());
   has_point_cloud = true;
@@ -96,17 +95,18 @@ void PlanningWorldTpl<DATATYPE>::setQposAll(const VectorX &state) {
     auto n = articulations[i]->getQposDim();
     auto segment =
         state.segment(total_dim, total_dim + n);  //[total_dim, total_dim + n)
-    ASSERT(segment.size() == n,
+    ASSERT(static_cast<size_t>(segment.size()) == n,
            "Bug with size " + std::to_string(segment.size()) + " " + std::to_string(n));
     setQpos(i, segment);
     total_dim += n;
   }
-  ASSERT(total_dim == state.size(), "State dimension is not correct");
+  ASSERT(total_dim == static_cast<size_t>(state.size()),
+         "State dimension is not correct");
 }
 
 template <typename DATATYPE>
 std::vector<WorldCollisionResultTpl<DATATYPE>> PlanningWorldTpl<DATATYPE>::selfCollide(
-    int index, const CollisionRequest &request) {
+    size_t index, const CollisionRequest &request) {
   std::vector<WorldCollisionResult> ret;
   auto fcl_model = articulations[index]->getFCLModel();
   auto results = fcl_model.collideFull(request);
@@ -130,7 +130,7 @@ std::vector<WorldCollisionResultTpl<DATATYPE>> PlanningWorldTpl<DATATYPE>::selfC
 
 template <typename DATATYPE>
 std::vector<WorldCollisionResultTpl<DATATYPE>>
-PlanningWorldTpl<DATATYPE>::collideWithOthers(int index,
+PlanningWorldTpl<DATATYPE>::collideWithOthers(size_t index,
                                               const CollisionRequest &request) {
   std::vector<WorldCollisionResult> ret;
   auto pinocchio_model = articulations[index]->getPinocchioModel();
@@ -246,7 +246,7 @@ PlanningWorldTpl<DATATYPE>::collideWithOthers(int index,
 
 template <typename DATATYPE>
 std::vector<WorldCollisionResultTpl<DATATYPE>> PlanningWorldTpl<DATATYPE>::collideFull(
-    int index, const CollisionRequest &request) {
+    size_t index, const CollisionRequest &request) {
   std::vector<WorldCollisionResult> ret1 = selfCollide(index, request);
   std::vector<WorldCollisionResult> ret2 = collideWithOthers(index, request);
   ret1.insert(ret1.end(), ret2.begin(), ret2.end());
diff --git a/src/planning_world.h b/src/planning_world.h
index 6eb276a..270d9b4 100644
--- a/src/planning_world.h
+++ b/src/planning_world.h
@@ -52,8 +52,8 @@ class PlanningWorldTpl {
   using WorldCollisionResult = WorldCollisionResultTpl<DATATYPE>;
   using WorldCollisionResult_ptr = WorldCollisionResultTpl_ptr<DATATYPE>;
 
-  std::vector<std::string> articulation_names;
   std::vector<ArticulatedModel_ptr> articulations;
+  std::vector<std::string> articulation_names;
   // std::vector<bool> articulation_flags;
   std::unordered_map<std::string, CollisionObject_ptr> normal_object_map;
   int move_articulation_id, attach_link_id;
@@ -273,7 +273,7 @@ class PlanningWorldTpl {
    * @return: List of WorldCollisionResult objects
    */
   std::vector<WorldCollisionResult> selfCollide(
-      int index, const CollisionRequest &request = CollisionRequest());
+      size_t index, const CollisionRequest &request = CollisionRequest());
   /**
    * Check collision between the articulated model and other objects.
    *
@@ -282,7 +282,7 @@ class PlanningWorldTpl {
    * @return: List of WorldCollisionResult objects
    */
   std::vector<WorldCollisionResult> collideWithOthers(
-      int index, const CollisionRequest &request = CollisionRequest());
+      size_t index, const CollisionRequest &request = CollisionRequest());
 
   /**
    * Check collision between the articulated model and all objects.
@@ -292,7 +292,7 @@ class PlanningWorldTpl {
    * @return: List of WorldCollisionResult objects
    */
   std::vector<WorldCollisionResult> collideFull(
-      int index, const CollisionRequest &request = CollisionRequest());
+      size_t index, const CollisionRequest &request = CollisionRequest());
 };
 
 template <typename T>
diff --git a/src/urdf_utils.cpp b/src/urdf_utils.cpp
index a801646..1cc9626 100644
--- a/src/urdf_utils.cpp
+++ b/src/urdf_utils.cpp
@@ -192,8 +192,8 @@ std::shared_ptr<fcl::BVHModel<fcl::OBBRSS<DATATYPE>>> load_mesh_as_BVH(
   std::vector<fcl::Vector3<DATATYPE>> vertices;
   std::vector<fcl::Triangle> triangles;
 
-  int nbVertices = dfs_build_mesh<DATATYPE>(loader.scene, loader.scene->mRootNode,
-                                            scale, 0, vertices, triangles);
+  dfs_build_mesh<DATATYPE>(loader.scene, loader.scene->mRootNode, scale, 0, vertices,
+                           triangles);
   // std::cout << "Num of vertex " << nbVertices << " " << vertices.size() << " " <<
   // triangles.size() << std::endl;
   using Model = fcl::BVHModel<fcl::OBBRSS<DATATYPE>>;
@@ -217,8 +217,8 @@ std::shared_ptr<fcl::Convex<DATATYPE>> load_mesh_as_Convex(
           int num_faces, const std::shared_ptr<const std::vector<int>>& faces,
   bool throw_if_invalid = false);
   */
-  int nbVertices = dfs_build_mesh<DATATYPE>(loader.scene, loader.scene->mRootNode,
-                                            scale, 0, vertices, triangles);
+  dfs_build_mesh<DATATYPE>(loader.scene, loader.scene->mRootNode, scale, 0, vertices,
+                           triangles);
 
   auto faces = std::make_shared<std::vector<int>>();
   for (size_t i = 0; i < triangles.size(); i++) {

From 882f138f8cfbeaff6e0e6014ccaa07e92956435a Mon Sep 17 00:00:00 2001
From: Kolin Guo <ruguo@ucsd.edu>
Date: Sun, 21 Jan 2024 17:09:27 -0800
Subject: [PATCH 3/6] Separate header files to include/mplib; Rename
 macros_utils.hpp to .h

---
 CMakeLists.txt                                | 31 ++++++++---------
 dev/mkdoc.sh                                  |  2 +-
 {src => include/mplib}/articulated_model.h    |  2 +-
 {src => include/mplib}/color_printing.h       |  0
 {src => include/mplib}/fcl_model.h            |  2 +-
 {src => include/mplib}/kdl_model.h            |  2 +-
 .../mplib/macros_utils.h                      |  0
 {src => include/mplib}/ompl_planner.h         |  0
 {src => include/mplib}/pinocchio_model.h      |  2 +-
 {src => include/mplib}/planning_world.h       |  2 +-
 {src => include/mplib}/urdf_utils.h           |  0
 python/docstring/macros_utils.h               | 33 +++++++++++++++++++
 python/pybind_articulation.hpp                |  2 +-
 python/pybind_fcl.hpp                         |  6 ++--
 python/pybind_kdl.hpp                         |  4 +--
 python/pybind_ompl.hpp                        |  2 +-
 python/pybind_pinocchio.hpp                   |  4 +--
 python/pybind_planning_world.hpp              |  4 +--
 src/articulated_model.cpp                     |  4 +--
 src/fcl_model.cpp                             |  4 +--
 src/kdl_model.cpp                             |  2 +-
 src/ompl_planner.cpp                          |  4 +--
 src/pinocchio_model.cpp                       |  4 +--
 src/planning_world.cpp                        |  8 ++---
 src/urdf_utils.cpp                            |  2 +-
 tests/test_articulated_model.cpp              |  3 +-
 26 files changed, 80 insertions(+), 49 deletions(-)
 rename {src => include/mplib}/articulated_model.h (99%)
 rename {src => include/mplib}/color_printing.h (100%)
 rename {src => include/mplib}/fcl_model.h (99%)
 rename {src => include/mplib}/kdl_model.h (98%)
 rename src/macros_utils.hpp => include/mplib/macros_utils.h (100%)
 rename {src => include/mplib}/ompl_planner.h (100%)
 rename {src => include/mplib}/pinocchio_model.h (99%)
 rename {src => include/mplib}/planning_world.h (99%)
 rename {src => include/mplib}/urdf_utils.h (100%)
 create mode 100644 python/docstring/macros_utils.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6b76d86..fd40c86 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,23 +9,23 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
 # set -fuse-ld=lld if lld is found
 find_program(LLD_FOUND ld.lld)
 if(LLD_FOUND)
-    message(STATUS "Using lld")
-    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld")
+  message(STATUS "Using lld")
+  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld")
 endif(LLD_FOUND)
 
 # add ccache as compiler launcher
 find_program(CCACHE_FOUND ccache)
 if(CCACHE_FOUND)
-    message(STATUS "Using ccache")
-    set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+  message(STATUS "Using ccache")
+  set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
 endif(CCACHE_FOUND)
 
 # Pinocchio uses its own FindCppAD, but does not provide it.
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
 
-set(Boost_NO_WARN_NEW_VERSIONS 1)  # silence Boost CMake warnings
 find_package(Eigen3 3.4.0 REQUIRED)
-find_package(Boost COMPONENTS system filesystem REQUIRED)
+set(Boost_NO_WARN_NEW_VERSIONS 1)  # silence Boost CMake warnings
+find_package(Boost REQUIRED COMPONENTS system filesystem)
 find_package(ompl REQUIRED)
 find_package(fcl REQUIRED)
 find_package(pinocchio REQUIRED)
@@ -33,16 +33,14 @@ find_package(assimp REQUIRED)
 find_package(orocos_kdl REQUIRED)
 find_package(urdfdom REQUIRED)
 
-include_directories(${OMPL_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS})
-include_directories("src")
-
 # store libries in a variable
 set(LIBS ompl fcl assimp orocos-kdl Boost::system Boost::filesystem urdfdom_model urdfdom_world)
-
-# pymp
-file(GLOB_RECURSE PROJECT_SRC "src/*.h" "src/*.cpp" "src/*.hpp")
-add_library(mp STATIC ${PROJECT_SRC})
+# mp
+file(GLOB_RECURSE MPLIB_SRC "src/*.cpp")
+add_library(mp STATIC ${MPLIB_SRC})
 target_link_libraries(mp PRIVATE ${LIBS})
+target_include_directories(mp PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+target_include_directories(mp PUBLIC ${OMPL_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS})
 set_target_properties(mp PROPERTIES POSITION_INDEPENDENT_CODE TRUE)
 
 # pybind11_mkdoc
@@ -51,7 +49,7 @@ add_custom_target(
   COMMAND bash "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh"
                "-I$<JOIN:$<TARGET_PROPERTY:mp,INCLUDE_DIRECTORIES>,;-I>"
   BYPRODUCTS "${CMAKE_CURRENT_SOURCE_DIR}/python/docstring/*.h"
-  DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh" "${CMAKE_CURRENT_SOURCE_DIR}/src/*.h"
+  DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dev/mkdoc.sh" "${CMAKE_CURRENT_SOURCE_DIR}/include/mplib/*.h"
   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
   COMMAND_EXPAND_LISTS
   VERBATIM
@@ -59,12 +57,11 @@ add_custom_target(
 
 # Pybind11
 add_subdirectory("third_party/pybind11")
-include_directories("python")
 pybind11_add_module(pymp python/pybind.cpp)
-target_link_libraries(pymp PRIVATE mp)
+target_link_libraries(pymp PRIVATE mp Eigen3::Eigen)
 add_dependencies(pymp pybind11_mkdoc)
 
 # compile test_articulated_model and run the test
 add_executable(test_articulated_model tests/test_articulated_model.cpp)
-target_link_libraries(test_articulated_model PRIVATE mp)
+target_link_libraries(test_articulated_model PRIVATE mp Eigen3::Eigen)
 add_test(NAME test_articulated_model COMMAND test_articulated_model)
diff --git a/dev/mkdoc.sh b/dev/mkdoc.sh
index 4ac11a8..412e675 100755
--- a/dev/mkdoc.sh
+++ b/dev/mkdoc.sh
@@ -7,7 +7,7 @@
 # Additional flags that clang understands can be passed in as well
 CLANG_FLAGS="-std=c++17 ${@}"
 PY_SCRIPT_PATH="dev/mkdoc.py"
-CPP_INCLUDE_DIR="src"
+CPP_INCLUDE_DIR="include/mplib"
 OUTPUT_DOCSTRING_DIR="python/docstring"
 
 ############################################################
diff --git a/src/articulated_model.h b/include/mplib/articulated_model.h
similarity index 99%
rename from src/articulated_model.h
rename to include/mplib/articulated_model.h
index 4da1274..23e7283 100644
--- a/src/articulated_model.h
+++ b/include/mplib/articulated_model.h
@@ -2,7 +2,7 @@
 
 #include "color_printing.h"
 #include "fcl_model.h"
-#include "macros_utils.hpp"
+#include "macros_utils.h"
 #include "pinocchio_model.h"
 
 /**
diff --git a/src/color_printing.h b/include/mplib/color_printing.h
similarity index 100%
rename from src/color_printing.h
rename to include/mplib/color_printing.h
diff --git a/src/fcl_model.h b/include/mplib/fcl_model.h
similarity index 99%
rename from src/fcl_model.h
rename to include/mplib/fcl_model.h
index 9dbb9d6..2a6ac0b 100644
--- a/src/fcl_model.h
+++ b/include/mplib/fcl_model.h
@@ -13,7 +13,7 @@
 #include "fcl/narrowphase/collision_request.h"
 #include "fcl/narrowphase/collision_result.h"
 #include "fcl/narrowphase/gjk_solver_type.h"
-#include "macros_utils.hpp"
+#include "macros_utils.h"
 
 /**
  * FCL collision model of an articulation
diff --git a/src/kdl_model.h b/include/mplib/kdl_model.h
similarity index 98%
rename from src/kdl_model.h
rename to include/mplib/kdl_model.h
index f0cf70a..5593545 100644
--- a/src/kdl_model.h
+++ b/include/mplib/kdl_model.h
@@ -15,7 +15,7 @@
 #include <kdl/utilities/svd_eigen_HH.hpp>
 #include <urdf_parser/urdf_parser.h>
 
-#include "macros_utils.hpp"
+#include "macros_utils.h"
 #include "urdf_utils.h"
 
 /**
diff --git a/src/macros_utils.hpp b/include/mplib/macros_utils.h
similarity index 100%
rename from src/macros_utils.hpp
rename to include/mplib/macros_utils.h
diff --git a/src/ompl_planner.h b/include/mplib/ompl_planner.h
similarity index 100%
rename from src/ompl_planner.h
rename to include/mplib/ompl_planner.h
diff --git a/src/pinocchio_model.h b/include/mplib/pinocchio_model.h
similarity index 99%
rename from src/pinocchio_model.h
rename to include/mplib/pinocchio_model.h
index 46841df..8b1ac07 100644
--- a/src/pinocchio_model.h
+++ b/include/mplib/pinocchio_model.h
@@ -10,7 +10,7 @@
 #include <urdf_world/types.h>
 
 #include "color_printing.h"
-#include "macros_utils.hpp"
+#include "macros_utils.h"
 
 /**
  * Pinocchio model of an articulation
diff --git a/src/planning_world.h b/include/mplib/planning_world.h
similarity index 99%
rename from src/planning_world.h
rename to include/mplib/planning_world.h
index 270d9b4..9dca61f 100644
--- a/src/planning_world.h
+++ b/include/mplib/planning_world.h
@@ -6,7 +6,7 @@
 #include "articulated_model.h"
 #include "color_printing.h"
 #include "fcl_model.h"
-#include "macros_utils.hpp"
+#include "macros_utils.h"
 
 /// Result of the collision checking.
 template <typename DATATYPE>
diff --git a/src/urdf_utils.h b/include/mplib/urdf_utils.h
similarity index 100%
rename from src/urdf_utils.h
rename to include/mplib/urdf_utils.h
diff --git a/python/docstring/macros_utils.h b/python/docstring/macros_utils.h
new file mode 100644
index 0000000..7e71b62
--- /dev/null
+++ b/python/docstring/macros_utils.h
@@ -0,0 +1,33 @@
+#pragma once
+
+/*
+  This file contains docstrings for use in the Python bindings.
+  Do not edit! They were automatically extracted by mkdoc.py.
+ */
+
+#define __EXPAND(x)                                      x
+#define __COUNT(_1, _2, _3, _4, _5, _6, _7, COUNT, ...)  COUNT
+#define __VA_SIZE(...)                                   __EXPAND(__COUNT(__VA_ARGS__, 7, 6, 5, 4, 3, 2, 1, 0))
+#define __CAT1(a, b)                                     a ## b
+#define __CAT2(a, b)                                     __CAT1(a, b)
+#define __DOC1(n1)                                       __doc_##n1
+#define __DOC2(n1, n2)                                   __doc_##n1##_##n2
+#define __DOC3(n1, n2, n3)                               __doc_##n1##_##n2##_##n3
+#define __DOC4(n1, n2, n3, n4)                           __doc_##n1##_##n2##_##n3##_##n4
+#define __DOC5(n1, n2, n3, n4, n5)                       __doc_##n1##_##n2##_##n3##_##n4##_##n5
+#define __DOC6(n1, n2, n3, n4, n5, n6)                   __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6
+#define __DOC7(n1, n2, n3, n4, n5, n6, n7)               __doc_##n1##_##n2##_##n3##_##n4##_##n5##_##n6##_##n7
+#define DOC(...)                                         __EXPAND(__EXPAND(__CAT2(__DOC, __VA_SIZE(__VA_ARGS__)))(__VA_ARGS__))
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-variable"
+#endif
+
+/* ----- Begin of custom docstring section ----- */
+
+/* ----- End of custom docstring section ----- */
+
+#if defined(__GNUG__)
+#pragma GCC diagnostic pop
+#endif
diff --git a/python/pybind_articulation.hpp b/python/pybind_articulation.hpp
index f0bb97f..64720eb 100644
--- a/python/pybind_articulation.hpp
+++ b/python/pybind_articulation.hpp
@@ -8,8 +8,8 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/articulated_model.h"
 #include "docstring/articulated_model.h"
+#include "mplib/articulated_model.h"
 
 namespace py = pybind11;
 
diff --git a/python/pybind_fcl.hpp b/python/pybind_fcl.hpp
index f6ccb4a..6bdb183 100644
--- a/python/pybind_fcl.hpp
+++ b/python/pybind_fcl.hpp
@@ -8,9 +8,6 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/fcl_model.h"
-#include "../src/macros_utils.hpp"
-#include "../src/urdf_utils.h"
 #include "docstring/fcl_model.h"
 #include "docstring/urdf_utils.h"
 #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h"
@@ -29,6 +26,9 @@
 #include "fcl/narrowphase/distance_request.h"
 #include "fcl/narrowphase/distance_result.h"
 #include "fcl/narrowphase/gjk_solver_type.h"
+#include "mplib/fcl_model.h"
+#include "mplib/macros_utils.h"
+#include "mplib/urdf_utils.h"
 
 using DATATYPE = double;
 
diff --git a/python/pybind_kdl.hpp b/python/pybind_kdl.hpp
index e387fc9..87cdf0b 100644
--- a/python/pybind_kdl.hpp
+++ b/python/pybind_kdl.hpp
@@ -8,9 +8,9 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/kdl_model.h"
-#include "../src/macros_utils.hpp"
 #include "docstring/kdl_model.h"
+#include "mplib/kdl_model.h"
+#include "mplib/macros_utils.h"
 
 namespace py = pybind11;
 
diff --git a/python/pybind_ompl.hpp b/python/pybind_ompl.hpp
index 0f6cf3a..c55670a 100644
--- a/python/pybind_ompl.hpp
+++ b/python/pybind_ompl.hpp
@@ -9,7 +9,7 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/ompl_planner.h"
+#include "mplib/ompl_planner.h"
 #include "docstring/ompl_planner.h"
 
 namespace py = pybind11;
diff --git a/python/pybind_pinocchio.hpp b/python/pybind_pinocchio.hpp
index 08312ca..f3d924e 100644
--- a/python/pybind_pinocchio.hpp
+++ b/python/pybind_pinocchio.hpp
@@ -8,9 +8,9 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/macros_utils.hpp"
-#include "../src/pinocchio_model.h"
 #include "docstring/pinocchio_model.h"
+#include "mplib/macros_utils.h"
+#include "mplib/pinocchio_model.h"
 
 namespace py = pybind11;
 
diff --git a/python/pybind_planning_world.hpp b/python/pybind_planning_world.hpp
index 25f83f9..74e1855 100644
--- a/python/pybind_planning_world.hpp
+++ b/python/pybind_planning_world.hpp
@@ -8,10 +8,10 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
-#include "../src/planning_world.h"
 #include "docstring/planning_world.h"
 #include "fcl/narrowphase/collision_request.h"
-#include "macros_utils.hpp"
+#include "mplib/macros_utils.h"
+#include "mplib/planning_world.h"
 
 namespace py = pybind11;
 
diff --git a/src/articulated_model.cpp b/src/articulated_model.cpp
index d35fe38..99679c7 100644
--- a/src/articulated_model.cpp
+++ b/src/articulated_model.cpp
@@ -1,4 +1,4 @@
-#include "articulated_model.h"
+#include "mplib/articulated_model.h"
 
 #include <algorithm>
 #include <cmath>
@@ -14,12 +14,12 @@
 #include <pinocchio/algorithm/joint-configuration.hpp>
 #include <pinocchio/algorithm/rnea.hpp>
 
+#include "mplib/urdf_utils.h"
 #include "pinocchio/multibody/joint/fwd.hpp"
 #include "pinocchio/parsers/urdf.hpp"
 #include "pinocchio/parsers/urdf/geometry.hxx"
 #include "pinocchio/parsers/urdf/model.hxx"
 #include "pinocchio/parsers/utils.hpp"
-#include "urdf_utils.h"
 
 #define DEFINE_TEMPLATE_AM(DATATYPE) template class ArticulatedModelTpl<DATATYPE>;
 
diff --git a/src/fcl_model.cpp b/src/fcl_model.cpp
index 0bbdfb9..a3ad9f3 100644
--- a/src/fcl_model.cpp
+++ b/src/fcl_model.cpp
@@ -1,4 +1,4 @@
-#include "fcl_model.h"
+#include "mplib/fcl_model.h"
 
 #include <algorithm>
 
@@ -6,7 +6,7 @@
 #include <boost/property_tree/ptree.hpp>
 #include <boost/property_tree/xml_parser.hpp>
 
-#include "urdf_utils.h"
+#include "mplib/urdf_utils.h"
 
 #define DEFINE_TEMPLATE_FM(DATATYPE) template class FCLModelTpl<DATATYPE>;
 
diff --git a/src/kdl_model.cpp b/src/kdl_model.cpp
index c81fe70..0b96a0b 100644
--- a/src/kdl_model.cpp
+++ b/src/kdl_model.cpp
@@ -1,4 +1,4 @@
-#include "kdl_model.h"
+#include "mplib/kdl_model.h"
 
 #define DEFINE_TEMPLATE_FM(DATATYPE) template class KDLModelTpl<DATATYPE>;
 
diff --git a/src/ompl_planner.cpp b/src/ompl_planner.cpp
index 2f4e77d..f220f96 100644
--- a/src/ompl_planner.cpp
+++ b/src/ompl_planner.cpp
@@ -1,6 +1,6 @@
-#include "ompl_planner.h"
+#include "mplib/ompl_planner.h"
 
-#include "pinocchio_model.h"
+#include "mplib/pinocchio_model.h"
 
 #define DEFINE_TEMPLATE_OMPL(DATATYPE)         \
   template class ValidityCheckerTpl<DATATYPE>; \
diff --git a/src/pinocchio_model.cpp b/src/pinocchio_model.cpp
index 1f06a5e..1a2d6b8 100644
--- a/src/pinocchio_model.cpp
+++ b/src/pinocchio_model.cpp
@@ -1,8 +1,8 @@
-#include "pinocchio_model.h"
+#include "mplib/pinocchio_model.h"
 
 #include <urdf_parser/urdf_parser.h>
 
-#include "urdf_utils.h"
+#include "mplib/urdf_utils.h"
 // #include <boost/property_tree/xml_parser.hpp>
 // #include <boost/property_tree/ptree.hpp>
 
diff --git a/src/planning_world.cpp b/src/planning_world.cpp
index 4de5626..0f5be3f 100644
--- a/src/planning_world.cpp
+++ b/src/planning_world.cpp
@@ -1,9 +1,9 @@
-#include "planning_world.h"
+#include "mplib/planning_world.h"
 
 #include "fcl/geometry/octree/octree.h"
-#include "fcl_model.h"
-#include "macros_utils.hpp"
-#include "urdf_utils.h"
+#include "mplib/fcl_model.h"
+#include "mplib/macros_utils.h"
+#include "mplib/urdf_utils.h"
 
 #define DEFINE_TEMPLATE_PW(DATATYPE)         \
   template class PlanningWorldTpl<DATATYPE>; \
diff --git a/src/urdf_utils.cpp b/src/urdf_utils.cpp
index 1cc9626..90f3007 100644
--- a/src/urdf_utils.cpp
+++ b/src/urdf_utils.cpp
@@ -1,4 +1,4 @@
-#include "urdf_utils.h"
+#include "mplib/urdf_utils.h"
 
 #include <kdl/frames_io.hpp>
 
diff --git a/tests/test_articulated_model.cpp b/tests/test_articulated_model.cpp
index 7eeb0a1..f795535 100644
--- a/tests/test_articulated_model.cpp
+++ b/tests/test_articulated_model.cpp
@@ -1,5 +1,6 @@
 #include <Eigen/Core>
-#include <articulated_model.h>
+
+#include "mplib/articulated_model.h"
 
 using ArticulatedModel = ArticulatedModelTpl<double>;
 

From 31662e87646f1f2497f6602b9fbc8b9c3564bbdf Mon Sep 17 00:00:00 2001
From: Xinsong Lin <zhongjiezhex12@me.com>
Date: Sun, 21 Jan 2024 19:23:00 -0800
Subject: [PATCH 4/6] fix import

---
 mplib/__init__.py                  | 3 ++-
 mplib/examples/detect_collision.py | 4 ++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/mplib/__init__.py b/mplib/__init__.py
index 7728ddc..7e413bb 100644
--- a/mplib/__init__.py
+++ b/mplib/__init__.py
@@ -4,7 +4,8 @@
 
 from importlib.metadata import version
 
-from .planner import Planner
+from mplib.planner import Planner
+from mplib.pymp import *
 
 # from .pymp import set_global_seed
 
diff --git a/mplib/examples/detect_collision.py b/mplib/examples/detect_collision.py
index 2bf78ed..6e9d488 100644
--- a/mplib/examples/detect_collision.py
+++ b/mplib/examples/detect_collision.py
@@ -40,9 +40,9 @@ def demo(self):
            This will cause the planner to timeout
         6. Remove the floor and check for env-collision returns no collision
         """
-        floor = mplib.planner.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
+        floor = mplib.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
         # create a collision object for the floor, with a 10cm offset in the z direction
-        floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(
+        floor_fcl_collision_object = mplib.fcl.CollisionObject(
             floor, [0, 0, -0.1], [1, 0, 0, 0]
         )
         # update the planning world with the floor collision object

From 58a02164417f2db23e85bae1049bebb043f0b784 Mon Sep 17 00:00:00 2001
From: Kolin Guo <ruguo@ucsd.edu>
Date: Sun, 21 Jan 2024 20:37:08 -0800
Subject: [PATCH 5/6] Unclear wildcard imports should be avoided

---
 mplib/__init__.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mplib/__init__.py b/mplib/__init__.py
index 7e413bb..463098a 100644
--- a/mplib/__init__.py
+++ b/mplib/__init__.py
@@ -5,7 +5,7 @@
 from importlib.metadata import version
 
 from mplib.planner import Planner
-from mplib.pymp import *
+from mplib.pymp import articulation, fcl, kdl, ompl, pinocchio, planning_world
 
 # from .pymp import set_global_seed
 

From fb054f40deb3ac634f3c22e93290d766a04171e8 Mon Sep 17 00:00:00 2001
From: Kolin Guo <ruguo@ucsd.edu>
Date: Sun, 21 Jan 2024 20:40:39 -0800
Subject: [PATCH 6/6] Remove Eigen3::Eigen in target_link_libraries for pymp
 and tests

---
 CMakeLists.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd40c86..22b8f17 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,10 +58,10 @@ add_custom_target(
 # Pybind11
 add_subdirectory("third_party/pybind11")
 pybind11_add_module(pymp python/pybind.cpp)
-target_link_libraries(pymp PRIVATE mp Eigen3::Eigen)
+target_link_libraries(pymp PRIVATE mp)
 add_dependencies(pymp pybind11_mkdoc)
 
 # compile test_articulated_model and run the test
 add_executable(test_articulated_model tests/test_articulated_model.cpp)
-target_link_libraries(test_articulated_model PRIVATE mp Eigen3::Eigen)
+target_link_libraries(test_articulated_model PRIVATE mp)
 add_test(NAME test_articulated_model COMMAND test_articulated_model)