From af69f94086b489e2bb6b71818daa1c7c3c827e67 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Sun, 14 Jan 2024 22:38:18 -0800 Subject: [PATCH] Update stubs and docs; Add generate_stub_and_doc.sh * Use latest pybind11-stubgen with --numpy-array-use-type-var * Remove previous stubgen.py/stubgen.sh * Remove generate_pdoc.sh * Update dev/README.md * Remove legacy update_bashrc.sh --- dev/README.md | 25 +- dev/generate_pdoc.sh | 8 - dev/generate_stub_and_doc.sh | 92 + dev/stubgen.py | 1117 ----- dev/stubgen.sh | 9 - dev/update_bashrc.sh | 12 - docs/mplib.html | 25 +- docs/mplib/examples/collision_avoidance.html | 562 +-- docs/mplib/examples/constrained_planning.html | 682 +-- docs/mplib/examples/demo.html | 421 +- docs/mplib/examples/demo_setup.html | 1129 +++-- docs/mplib/examples/detect_collision.html | 451 +- docs/mplib/examples/moving_robot.html | 542 +- docs/mplib/examples/two_stage_motion.html | 947 ++-- docs/mplib/planner.html | 4383 +++++++++-------- docs/search.js | 2 +- mplib/__init__.pyi | 11 + mplib/planner.pyi | 261 + mplib/pymp/__init__.pyi | 30 +- .../__init__.pyi => articulation.pyi} | 58 +- mplib/pymp/fcl.pyi | 445 ++ mplib/pymp/fcl/__init__.pyi | 525 -- mplib/pymp/kdl.pyi | 18 + mplib/pymp/kdl/__init__.pyi | 18 - mplib/pymp/{ompl/__init__.pyi => ompl.pyi} | 57 +- .../{pinocchio/__init__.pyi => pinocchio.pyi} | 70 +- .../__init__.pyi => planning_world.pyi} | 101 +- 27 files changed, 5805 insertions(+), 6196 deletions(-) delete mode 100644 dev/generate_pdoc.sh create mode 100644 dev/generate_stub_and_doc.sh delete mode 100644 dev/stubgen.py delete mode 100644 dev/stubgen.sh delete mode 100644 dev/update_bashrc.sh create mode 100644 mplib/__init__.pyi create mode 100644 mplib/planner.pyi rename mplib/pymp/{articulation/__init__.pyi => articulation.pyi} (72%) create mode 100644 mplib/pymp/fcl.pyi delete mode 100644 mplib/pymp/fcl/__init__.pyi create mode 100644 mplib/pymp/kdl.pyi delete mode 100644 mplib/pymp/kdl/__init__.pyi rename mplib/pymp/{ompl/__init__.pyi => ompl.pyi} (52%) rename mplib/pymp/{pinocchio/__init__.pyi => pinocchio.pyi} (69%) rename mplib/pymp/{planning_world/__init__.pyi => planning_world.pyi} (77%) diff --git a/dev/README.md b/dev/README.md index ea1e46ac..b389cb57 100644 --- a/dev/README.md +++ b/dev/README.md @@ -69,17 +69,20 @@ Depending on your python version, you will get a file called `pymp.cpython-310-x To install the entire package along with python glue code, do `python3.[version] -m pip install .` inside the root directory of the project. -## Documentation Generation - -### Stub Generation - -Stubs are useful for type checking and IDE autocompletion. To generate stubs, you **first need to have mplib compiled and installed**. Then, do `python3.[version] -m pip install pybind11_stubgen` and then run `bash dev/generate_stubs.sh`. This will generate stubs for the entire project in the `stubs/` directory. Note that you might need to change the version of the python inside the script. The default is 3.11. - -### Website Generation - -We use `pdoc` to generate the documentation. First install a version of mplib referring to the section above. - -Then, you will need to install `pdoc` with `python3.[version] -m pip install pdoc`. The reason the version is important is because `pdoc` will actually do analysis on the files, so it will scrape through the mplib installed in the site package. Then, run `bash dev/generate_pdoc.sh` from the root of MPlib. This will generate the documentation in the `docs/` directory. Note that you might need to change the version of the python inside the script. The default is 3.11. +## Stubs & Documentation Generation + +To generate stubs and documentations, run `./dev/generate_stub_and_doc.sh`. +By default it uses `python3.10` in docker image [`kolinguo/mplib-build`](https://hub.docker.com/r/kolinguo/mplib-build). + +The script does the following: +* Build a python wheel using [`cibuildwheel`](https://cibuildwheel.readthedocs.io/en/stable/#how-it-works). +* In a docker container, install the python wheel and +use [`pybind11-stubgen`](https://github.com/sizmailov/pybind11-stubgen) +to generate stubs. +Copy the generated stubs into [`mplib`](../mplib/). +* In a docker container, install the python wheel and +use [`pdoc`](https://pdoc.dev/docs/pdoc.html) to generate documentations. +Copy the generated docs into [`docs`](../docs/). ## GitHub Action CI/CD Currently, a GitHub action is setup to build / release / publish python wheels. diff --git a/dev/generate_pdoc.sh b/dev/generate_pdoc.sh deleted file mode 100644 index 076281c2..00000000 --- a/dev/generate_pdoc.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env bash -# python3.[version] -m pip install pdoc -# run from the root of the project - -current_dir=$(pwd) -cd ~ # go out of the current directory to avoid picking up local mplib (need global) -python3.11 -m pdoc -o $current_dir/docs mplib -cd $current_dir diff --git a/dev/generate_stub_and_doc.sh b/dev/generate_stub_and_doc.sh new file mode 100644 index 00000000..30cf6d1a --- /dev/null +++ b/dev/generate_stub_and_doc.sh @@ -0,0 +1,92 @@ +#!/bin/bash + +# Python version to build wheels and generate stubs / docs +PY_VERSION=310 +# Docker image name to install wheel and generate stubs / docs +IMGNAME="kolinguo/mplib-build:latest" + +############################################################ +# Section 0: Bash Error Handling # +############################################################ +set -eEu -o pipefail +trap 'catch' ERR # Trap all errors (status != 0) and call catch() +catch() { + local err="$?" + local err_command="$BASH_COMMAND" + set +xv # disable trace printing + + echo -e "\n\e[1;31mCaught error in ${BASH_SOURCE[1]}:${BASH_LINENO[0]} ('${err_command}' exited with status ${err})\e[0m" >&2 + echo "Traceback (most recent call last, command might not be complete):" >&2 + for ((i = 0; i < ${#FUNCNAME[@]} - 1; i++)); do + local funcname="${FUNCNAME[$i]}" + [ "$i" -eq "0" ] && funcname=$err_command + echo -e " ($i) ${BASH_SOURCE[$i+1]}:${BASH_LINENO[$i]}\t'${funcname}'" >&2 + done + exit "$err" +} + +############################################################ +# Section 1: Build python wheels # +############################################################ +# Move to the repo folder, so later commands can use relative paths +SCRIPT_PATH=$(readlink -f "$0") +REPO_DIR=$(dirname "$(dirname "$SCRIPT_PATH")") +cd "$REPO_DIR" +REPO_NAME=$(basename "$REPO_DIR") # default WORKDIR inside container + +echo_info() { + echo -e "\n\e[1;36m$1 ...\e[0m" +} + +echo_info "Removing previous wheels under 'wheelhouse/'" +rm -rfv wheelhouse/*.whl + +echo_info "Building wheels with 'dev/build_wheels.sh'" +dev/build_wheels.sh --py "$PY_VERSION" + +############################################################ +# Section 2: Build stubs # +############################################################ +BUILD_STUB_CMD="\ + export PATH=\"/opt/python/cp${PY_VERSION}-cp${PY_VERSION}/bin:\${PATH}\" \ + && python3 -m pip install pybind11-stubgen \ + && python3 -m pip install wheelhouse/mplib*.whl \ + && pybind11-stubgen --numpy-array-use-type-var mplib +" +# TODO: add ruff + +echo_info "Building stubs in docker '${IMGNAME}'" +docker run -it --rm \ + -v "$REPO_DIR":/${REPO_NAME} \ + "$IMGNAME" \ + bash -c "$BUILD_STUB_CMD" + +echo_info "Removing previous stubs under 'mplib/'" +find mplib -name "*.pyi" -exec rm -v {} \; +echo_info "Moving generated stubs into 'mplib/'" +mv -v stubs/mplib/* mplib +echo_info "Removing 'stubs/'" +rm -rfv stubs/ + +############################################################ +# Section 3: Build docs # +############################################################ +# TODO: switch to other tools to generate docs, README.md should not be included in wheels +# TODO: do we must install sapien to generate doc? +BUILD_DOC_CMD="\ + export PATH=\"/opt/python/cp${PY_VERSION}-cp${PY_VERSION}/bin:\${PATH}\" \ + && python3 -m pip install pdoc \ + && python3 -m pip install sapien==3.0.0.dev0 \ + && python3 -m pip install wheelhouse/mplib*.whl \ + && cp -v README.md /opt/python/cp${PY_VERSION}-cp${PY_VERSION}/lib/python*/site-packages/mplib \ + && pdoc -o docs mplib +" + +echo_info "Removing previous docs under 'mplib/docs/'" +rm -rfv mplib/docs/ + +echo_info "Building docs in docker '${IMGNAME}'" +docker run -it --rm \ + -v "$REPO_DIR":/${REPO_NAME} \ + "$IMGNAME" \ + bash -c "$BUILD_DOC_CMD" diff --git a/dev/stubgen.py b/dev/stubgen.py deleted file mode 100644 index e413ed2a..00000000 --- a/dev/stubgen.py +++ /dev/null @@ -1,1117 +0,0 @@ -import ast -import importlib -import inspect -import itertools -import logging -import os -import re -import sys -import warnings -from argparse import ArgumentParser -from functools import cmp_to_key -from typing import ( - Any, - Callable, - Dict, - Iterable, - Iterator, - List, - Mapping, - Optional, - Set, - Tuple, -) - -logger = logging.getLogger(__name__) - -_visited_objects = [] - -# A list of function docstring pre-processing hooks -function_docstring_preprocessing_hooks: List[Callable[[str], str]] = [] - - -class DirectoryWalkerGuard(object): - def __init__(self, dirname): - self.dirname = dirname - - def __enter__(self): - if not os.path.exists(self.dirname): - os.mkdir(self.dirname) - - assert os.path.isdir(self.dirname) - - os.chdir(self.dirname) - - def __exit__(self, exc_type, exc_val, exc_tb): - os.chdir(os.path.pardir) - - -_default_pybind11_repr_re = re.compile( - r"(<(?P\w+(\.\w+)*) object at 0x[0-9a-fA-F]+>)|" - r"(<(?P\w+(.\w+)*): \d+>)" -) - - -def replace_default_pybind11_repr(line): - default_reprs = [] - - def replacement(m): - if m.group("class"): - default_reprs.append(m.group(0)) - return "..." - return m.group("enum") - - return default_reprs, _default_pybind11_repr_re.sub(replacement, line) - - -class FunctionSignature(object): - # When True don't raise an error when invalid signatures/defaultargs are - # encountered (yes, global variables, blame me) - ignore_invalid_signature = False - ignore_invalid_defaultarg = False - - signature_downgrade = True - - # Number of invalid default values found so far - n_invalid_default_values = 0 - - # Number of invalid signatures found so far - n_invalid_signatures = 0 - - @classmethod - def n_fatal_errors(cls): - return ( - 0 if cls.ignore_invalid_defaultarg else cls.n_invalid_default_values - ) + (0 if cls.ignore_invalid_signature else cls.n_invalid_signatures) - - def __init__(self, name, args="*args, **kwargs", rtype="None", validate=True): - self.name = name - self.args = args - self.rtype = rtype - - if validate: - invalid_defaults, self.args = replace_default_pybind11_repr(self.args) - if invalid_defaults: - FunctionSignature.n_invalid_default_values += 1 - lvl = ( - logging.WARNING - if FunctionSignature.ignore_invalid_defaultarg - else logging.ERROR - ) - logger.log( - lvl, "Default argument value(s) replaced with ellipses (...):" - ) - for invalid_default in invalid_defaults: - logger.log(lvl, " {}".format(invalid_default)) - - function_def_str = "def {sig.name}({sig.args}) -> {sig.rtype}: ...".format( - sig=self - ) - try: - ast.parse(function_def_str) - except SyntaxError as e: - FunctionSignature.n_invalid_signatures += 1 - if FunctionSignature.signature_downgrade: - self.name = name - self.args = "*args, **kwargs" - self.rtype = "typing.Any" - lvl = ( - logging.WARNING - if FunctionSignature.ignore_invalid_signature - else logging.ERROR - ) - logger.log( - lvl, - "Generated stubs signature is degraded to `(*args, **kwargs) ->" - " typing.Any` for", - ) - else: - lvl = logging.WARNING - logger.warning("Ignoring invalid signature:") - logger.log(lvl, function_def_str) - logger.log(lvl, " " * (e.offset - 1) + "^-- Invalid syntax") - - def __eq__(self, other): - return isinstance(other, FunctionSignature) and ( - self.name, - self.args, - self.rtype, - ) == (other.name, other.args, other.rtype) - - def __hash__(self): - return hash((self.name, self.args, self.rtype)) - - def split_arguments(self): - if len(self.args.strip()) == 0: - return [] - - prev_stop = 0 - brackets = 0 - splitted_args = [] - - for i, c in enumerate(self.args): - if c == "[": - brackets += 1 - elif c == "]": - brackets -= 1 - assert brackets >= 0 - elif c == "," and brackets == 0: - splitted_args.append(self.args[prev_stop:i]) - prev_stop = i + 1 - - splitted_args.append(self.args[prev_stop:]) - assert brackets == 0 - return splitted_args - - @staticmethod - def argument_type(arg): - return arg.split(":")[-1].strip() - - def get_all_involved_types(self): - types = [] - for t in [self.rtype] + self.split_arguments(): - types.extend([ - m[0] - for m in re.findall( - r"([a-zA-Z_][a-zA-Z0-9_]*(\.[a-zA-Z_][a-zA-Z0-9_]*)*)", - self.argument_type(t), - ) - ]) - return types - - -class PropertySignature(object): - NONE = 0 - READ_ONLY = 1 - WRITE_ONLY = 2 - READ_WRITE = READ_ONLY | WRITE_ONLY - - def __init__(self, rtype, setter_args, access_type): - self.rtype = rtype - self.setter_args = setter_args - self.access_type = access_type - - @property - def setter_arg_type(self): - return FunctionSignature.argument_type( - FunctionSignature("name", self.setter_args).split_arguments()[1] - ) - - -# If true numpy.ndarray[int32[3,3]] will be reduced to numpy.ndarray -BARE_NUPMY_NDARRAY = False - - -def replace_numpy_array(match_obj): - if BARE_NUPMY_NDARRAY: - return "numpy.ndarray" - numpy_type = match_obj.group("type") - # pybind always append size of data type - if numpy_type in [ - "int8", - "int16", - "int32", - "int64", - "float16", - "float32", - "float64", - "complex32", - "complex64", - "longcomplex", - ]: - numpy_type = "numpy." + numpy_type - - shape = match_obj.group("shape") - if shape: - shape = ", _Shape[{}]".format(shape) - else: - shape = "" - result = r"numpy.ndarray[{type}{shape}]".format(type=numpy_type, shape=shape) - return result - - -def replace_typing_types(match): - # pybind used to have iterator/iterable in place of Iterator/Iterable - return "typing." + match.group("type").capitalize() - - -class StubsGenerator(object): - INDENT = " " * 4 - - GLOBAL_CLASSNAME_REPLACEMENTS = { - re.compile( - r"numpy.ndarray\[(?P[^\[\]]+)(\[(?P[^\[\]]+)\])?(?P[^][]*)\]" - ): replace_numpy_array, - re.compile( - r"(?Callable|Dict|[Ii]terator|[Ii]terable|List|Optional|Set|Tuple|Union')(?!\w)" - ): replace_typing_types, - } - - def parse(self): - raise NotImplementedError - - def to_lines(self): # type: () -> List[str] - raise NotImplementedError - - @staticmethod - def _indent(line): # type: (str) -> str - return StubsGenerator.INDENT + line - - @staticmethod - def indent(lines): # type: (str) -> str - lines = lines.split("\n") - lines = [StubsGenerator._indent(l) if l else l for l in lines] - return "\n".join(lines) - - @staticmethod - def fully_qualified_name(klass): - module_name = klass.__module__ if hasattr(klass, "__module__") else None - class_name = getattr(klass, "__qualname__", klass.__name__) - - if module_name == "builtins": - return class_name - else: - return "{module}.{klass}".format(module=module_name, klass=class_name) - - @staticmethod - def apply_classname_replacements(s): # type: (str) -> Any - for k, v in StubsGenerator.GLOBAL_CLASSNAME_REPLACEMENTS.items(): - s = k.sub(v, s) - return s - - @staticmethod - def function_signatures_from_docstring( - name, func, module_name - ): # type: (Any, str) -> List[FunctionSignature] - try: - no_parentheses = r"[^()]*" - parentheses_one_fold = r"({nopar}(\({nopar}\))?)*".format( - nopar=no_parentheses - ) - parentheses_two_fold = r"({nopar}(\({par1}\))?)*".format( - par1=parentheses_one_fold, nopar=no_parentheses - ) - parentheses_three_fold = r"({nopar}(\({par2}\))?)*".format( - par2=parentheses_two_fold, nopar=no_parentheses - ) - signature_regex = ( - r"(\s*(?P\d+).)" - r"?\s*{name}\s*\((?P{balanced_parentheses})\)" - r"\s*->\s*" - r"(?P[^\(\)]+)\s*".format( - name=name, balanced_parentheses=parentheses_three_fold - ) - ) - docstring = func.__doc__ - - for hook in function_docstring_preprocessing_hooks: - docstring = hook(docstring) - - signatures = [] - for line in docstring.split("\n"): - m = re.match(signature_regex, line) - if m: - args = m.group("args") - rtype = m.group("rtype") - signatures.append(FunctionSignature(name, args, rtype)) - - # strip module name if provided - if module_name: - for sig in signatures: - regex = r"{}\.(\w+)".format(module_name.replace(".", r"\.")) - sig.args = re.sub(regex, r"\g<1>", sig.args) - sig.rtype = re.sub(regex, r"\g<1>", sig.rtype) - - for sig in signatures: - sig.args = StubsGenerator.apply_classname_replacements(sig.args) - sig.rtype = StubsGenerator.apply_classname_replacements(sig.rtype) - - return sorted(list(set(signatures)), key=lambda fs: fs.args) - except AttributeError: - return [] - - @staticmethod - def property_signature_from_docstring( - prop, module_name - ): # type: (Any, str)-> PropertySignature - - getter_rtype = "None" - setter_args = "None" - access_type = PropertySignature.NONE - - strip_module_name = module_name is not None - - if hasattr(prop, "fget") and prop.fget is not None: - access_type |= PropertySignature.READ_ONLY - if hasattr(prop.fget, "__doc__") and prop.fget.__doc__ is not None: - for line in prop.fget.__doc__.split("\n"): - if strip_module_name: - line = line.replace(module_name + ".", "") - m = re.match( - r"\s*(\w*)\((?P[^()]*)\)\s*->\s*(?P[^()]+)\s*", - line, - ) - if m: - getter_rtype = m.group("rtype") - break - - if hasattr(prop, "fset") and prop.fset is not None: - access_type |= PropertySignature.WRITE_ONLY - if hasattr(prop.fset, "__doc__") and prop.fset.__doc__ is not None: - for line in prop.fset.__doc__.split("\n"): - if strip_module_name: - line = line.replace(module_name + ".", "") - m = re.match( - r"\s*(\w*)\((?P[^()]*)\)\s*->\s*(?P[^()]+)\s*", - line, - ) - if m: - args = m.group("args") - # replace first argument with self - setter_args = ",".join(["self"] + args.split(",")[1:]) - break - getter_rtype = StubsGenerator.apply_classname_replacements(getter_rtype) - setter_args = StubsGenerator.apply_classname_replacements(setter_args) - return PropertySignature(getter_rtype, setter_args, access_type) - - @staticmethod - def remove_signatures(docstring): # type: (str) ->str - - if docstring is None: - return "" - - for hook in function_docstring_preprocessing_hooks: - docstring = hook(docstring) - - signature_regex = ( - r"(\s*(?P\d+).\s*)" - r"?{name}\s*\((?P.*)\)\s*(->\s*(?P[^\(\)]+)\s*)?".format( - name=r"\w+" - ) - ) - - lines = docstring.split("\n\n") - lines = filter(lambda line: line != "Overloaded function.", lines) - - return "\n\n".join( - filter(lambda line: not re.match(signature_regex, line), lines) - ) - - @staticmethod - def sanitize_docstring(docstring): # type: (str) ->str - docstring = StubsGenerator.remove_signatures(docstring) - docstring = docstring.rstrip("\n") - - if docstring and re.match(r"^\s*$", docstring): - docstring = "" - - return docstring - - @staticmethod - def format_docstring(docstring): - return StubsGenerator.indent('"""\n{}\n"""'.format(docstring.strip("\n"))) - - -class AttributeStubsGenerator(StubsGenerator): - def __init__(self, name, attribute): # type: (str, Any)-> None - self.name = name - self.attr = attribute - - def parse(self): - if self in _visited_objects: - return - _visited_objects.append(self) - - def is_safe_to_use_repr(self, value): - if value is None or isinstance(value, (int, str)): - return True - if isinstance(value, (float, complex)): - try: - eval(repr(value)) - return True - except (SyntaxError, NameError): - return False - if isinstance(value, (list, tuple, set)): - for x in value: - if not self.is_safe_to_use_repr(x): - return False - return True - if isinstance(value, dict): - for k, v in value.items(): - if not self.is_safe_to_use_repr(k) or not self.is_safe_to_use_repr(v): - return False - return True - return False - - def to_lines(self): # type: () -> List[str] - if self.is_safe_to_use_repr(self.attr): - return ["{name} = {repr}".format(name=self.name, repr=repr(self.attr))] - - # special case for modules - # https://github.com/sizmailov/pybind11-stubgen/issues/43 - if type(self.attr) is type(os) and hasattr(self.attr, "__name__"): - return ["{name} = {repr}".format(name=self.name, repr=self.attr.__name__)] - - value_lines = repr(self.attr).split("\n") - if len(value_lines) == 1: - value = value_lines[0] - # remove random address from - value = re.sub(r" at 0x[0-9a-fA-F]+>", ">", value) - typename = self.fully_qualified_name(type(self.attr)) - if value == "<{typename} object>".format(typename=typename): - value_comment = "" - else: - value_comment = " # value = {value}".format(value=value) - return [ - "{name}: {typename}{value_comment}".format( - name=self.name, typename=typename, value_comment=value_comment - ) - ] - else: - return ( - [ - "{name}: {typename} # value = ".format( - name=self.name, typename=str(type(self.attr)) - ) - ] - + ['"""'] - + [l.replace('"""', r"\"\"\"") for l in value_lines] - + ['"""'] - ) - - def get_involved_modules_names(self): # type: () -> Set[str] - if type(self.attr) is type(os): - return {self.attr.__name__} - return {self.attr.__class__.__module__} - - -class FreeFunctionStubsGenerator(StubsGenerator): - def __init__(self, name, free_function, module_name): - self.name = name - self.member = free_function - self.module_name = module_name - self.signatures = [] # type: List[FunctionSignature] - - def parse(self): - self.signatures = self.function_signatures_from_docstring( - self.name, self.member, self.module_name - ) - - def to_lines(self): # type: () -> List[str] - result = [] - docstring = self.sanitize_docstring(self.member.__doc__) - if not docstring and not ( - self.name.startswith("__") and self.name.endswith("__") - ): - logger.debug( - "Docstring is empty for '%s'" % self.fully_qualified_name(self.member) - ) - for sig in self.signatures: - if len(self.signatures) > 1: - result.append("@typing.overload") - result.append( - "def {name}({args}) -> {rtype}:".format( - name=sig.name, args=sig.args, rtype=sig.rtype - ) - ) - if docstring: - result.append(self.format_docstring(docstring)) - docstring = None # don't print docstring for other overloads - else: - result.append(self.indent("pass")) - - return result - - def get_involved_modules_names(self): # type: () -> Set[str] - involved_modules_names = set() - for s in self.signatures: # type: FunctionSignature - for t in s.get_all_involved_types(): # type: str - try: - i = t.rindex(".") - involved_modules_names.add(t[:i]) - except ValueError: - pass - return involved_modules_names - - -class ClassMemberStubsGenerator(FreeFunctionStubsGenerator): - def __init__(self, name, free_function, module_name): - super(ClassMemberStubsGenerator, self).__init__( - name, free_function, module_name - ) - - def to_lines(self): # type: () -> List[str] - result = [] - docstring = self.sanitize_docstring(self.member.__doc__) - if not docstring and not ( - self.name.startswith("__") and self.name.endswith("__") - ): - logger.debug( - "Docstring is empty for '%s'" % self.fully_qualified_name(self.member) - ) - for sig in self.signatures: - args = sig.args - if not args.strip().startswith("self"): - result.append("@staticmethod") - else: - # remove type of self - args = ",".join(["self"] + sig.split_arguments()[1:]) - if len(self.signatures) > 1: - result.append("@typing.overload") - - result.append( - "def {name}({args}) -> {rtype}: {ellipsis}".format( - name=sig.name, - args=args, - rtype=sig.rtype, - ellipsis="" if docstring else "...", - ) - ) - if docstring: - result.append(self.format_docstring(docstring)) - docstring = None # don't print docstring for other overloads - return result - - -class PropertyStubsGenerator(StubsGenerator): - def __init__(self, name, prop, module_name): - self.name = name - self.prop = prop - self.module_name = module_name - self.signature = None # type: PropertySignature - - def parse(self): - self.signature = self.property_signature_from_docstring( - self.prop, self.module_name - ) - - def to_lines(self): # type: () -> List[str] - - docstring = self.sanitize_docstring(self.prop.__doc__) - docstring_prop = "\n\n".join( - [docstring, ":type: {rtype}".format(rtype=self.signature.rtype)] - ) - - result = [ - "@property", - "def {field_name}(self) -> {rtype}:".format( - field_name=self.name, rtype=self.signature.rtype - ), - self.format_docstring(docstring_prop), - ] - - if self.signature.setter_args != "None": - result.append("@{field_name}.setter".format(field_name=self.name)) - result.append( - "def {field_name}({args}) -> None:".format( - field_name=self.name, args=self.signature.setter_args - ) - ) - if docstring: - result.append(self.format_docstring(docstring)) - else: - result.append(self.indent("pass")) - - return result - - -class ClassStubsGenerator(StubsGenerator): - ATTRIBUTES_BLACKLIST = ( - "__class__", - "__module__", - "__qualname__", - "__dict__", - "__weakref__", - "__annotations__", - ) - PYBIND11_ATTRIBUTES_BLACKLIST = ("__entries",) - METHODS_BLACKLIST = ("__dir__", "__sizeof__") - BASE_CLASS_BLACKLIST = ("pybind11_object", "object") - CLASS_NAME_BLACKLIST = ("pybind11_type",) - - def __init__( - self, - klass, - attributes_blacklist=ATTRIBUTES_BLACKLIST, - pybind11_attributes_blacklist=PYBIND11_ATTRIBUTES_BLACKLIST, - base_class_blacklist=BASE_CLASS_BLACKLIST, - methods_blacklist=METHODS_BLACKLIST, - class_name_blacklist=CLASS_NAME_BLACKLIST, - ): - self.klass = klass - assert inspect.isclass(klass) - - self.doc_string = None # type: Optional[str] - - self.classes = [] # type: List[ClassStubsGenerator] - self.fields = [] # type: List[AttributeStubsGenerator] - self.properties = [] # type: List[PropertyStubsGenerator] - self.methods = [] # type: List[ClassMemberStubsGenerator] - - self.base_classes = [] - self.involved_modules_names = set() # Set[str] - - self.attributes_blacklist = attributes_blacklist - self.pybind11_attributes_blacklist = pybind11_attributes_blacklist - self.base_class_blacklist = base_class_blacklist - self.methods_blacklist = methods_blacklist - self.class_name_blacklist = class_name_blacklist - - def get_involved_modules_names(self): - return self.involved_modules_names - - def parse(self): - if self.klass in _visited_objects: - return - _visited_objects.append(self.klass) - - bases = inspect.getmro(self.klass)[1:] - - def is_base_member(name, member): - for base in bases: - if hasattr(base, name) and getattr(base, name) is member: - return True - return False - - is_pybind11 = any(base.__name__ == "pybind11_object" for base in bases) - - for name, member in inspect.getmembers(self.klass): - # check if attribute is in __dict__ (fast path) before slower search in base classes - if name not in self.klass.__dict__ and is_base_member(name, member): - continue - if name.startswith("__pybind11_module"): - continue - if inspect.isroutine(member): - self.methods.append( - ClassMemberStubsGenerator(name, member, self.klass.__module__) - ) - elif name != "__class__" and inspect.isclass(member): - if member.__name__ not in self.class_name_blacklist: - self.classes.append(ClassStubsGenerator(member)) - elif isinstance(member, property): - self.properties.append( - PropertyStubsGenerator(name, member, self.klass.__module__) - ) - elif name == "__doc__": - self.doc_string = member - elif not ( - name in self.attributes_blacklist - or (is_pybind11 and name in self.pybind11_attributes_blacklist) - ): - self.fields.append(AttributeStubsGenerator(name, member)) - # logger.warning("Unknown member %s type : `%s` " % (name, str(type(member)))) - - for x in itertools.chain( - self.classes, self.methods, self.properties, self.fields - ): - x.parse() - - for B in bases: - if ( - B.__name__ != self.klass.__name__ - and B.__name__ not in self.base_class_blacklist - ): - self.base_classes.append(B) - self.involved_modules_names.add(B.__module__) - - for f in self.methods: # type: ClassMemberStubsGenerator - self.involved_modules_names |= f.get_involved_modules_names() - - for attr in self.fields: - self.involved_modules_names |= attr.get_involved_modules_names() - - def to_lines(self): # type: () -> List[str] - def strip_current_module_name(obj, module_name): - regex = r"{}\.(\w+)".format(module_name.replace(".", r"\.")) - return re.sub(regex, r"\g<1>", obj) - - base_classes_list = [ - strip_current_module_name( - self.fully_qualified_name(b), self.klass.__module__ - ) - for b in self.base_classes - ] - result = [ - "class {class_name}({base_classes_list}):{doc_string}".format( - class_name=self.klass.__name__, - base_classes_list=", ".join(base_classes_list), - doc_string=( - "\n" + self.format_docstring(self.doc_string) - if self.doc_string - else "" - ), - ), - ] - for cl in self.classes: - result.extend(map(self.indent, cl.to_lines())) - - for f in self.methods: - if f.name not in self.methods_blacklist: - result.extend(map(self.indent, f.to_lines())) - - for p in self.properties: - result.extend(map(self.indent, p.to_lines())) - - for p in self.fields: - result.extend(map(self.indent, p.to_lines())) - - result.append(self.indent("pass")) - return result - - -class ModuleStubsGenerator(StubsGenerator): - CLASS_NAME_BLACKLIST = ClassStubsGenerator.CLASS_NAME_BLACKLIST - ATTRIBUTES_BLACKLIST = ( - "__file__", - "__loader__", - "__name__", - "__package__", - "__spec__", - "__path__", - "__cached__", - "__builtins__", - ) - - def __init__( - self, - module_or_module_name, - attributes_blacklist=ATTRIBUTES_BLACKLIST, - class_name_blacklist=CLASS_NAME_BLACKLIST, - ): - if isinstance(module_or_module_name, str): - self.module = importlib.import_module(module_or_module_name) - else: - self.module = module_or_module_name - assert inspect.ismodule(self.module) - - self.doc_string = None # type: Optional[str] - self.classes = [] # type: List[ClassStubsGenerator] - self.free_functions = [] # type: List[FreeFunctionStubsGenerator] - self.submodules = [] # type: List[ModuleStubsGenerator] - self.imported_modules = [] # type: List[str] - self.imported_classes = {} # type: Dict[str, type] - self.attributes = [] # type: List[AttributeStubsGenerator] - self.stub_suffix = "" - self.write_setup_py = False - - self.attributes_blacklist = attributes_blacklist - self.class_name_blacklist = class_name_blacklist - - def parse(self): - if self.module in _visited_objects: - return - _visited_objects.append(self.module) - logger.debug("Parsing '%s' module" % self.module.__name__) - for name, member in inspect.getmembers(self.module): - if inspect.ismodule(member): - m = ModuleStubsGenerator(member) - if m.module.__name__.split(".")[:-1] == self.module.__name__.split("."): - self.submodules.append(m) - else: - self.imported_modules += [m.module.__name__] - logger.debug( - "Skip '%s' module while parsing '%s' " - % (m.module.__name__, self.module.__name__) - ) - elif inspect.isbuiltin(member) or inspect.isfunction(member): - self.free_functions.append( - FreeFunctionStubsGenerator(name, member, self.module.__name__) - ) - elif inspect.isclass(member): - if member.__module__ == self.module.__name__: - if member.__name__ not in self.class_name_blacklist: - self.classes.append(ClassStubsGenerator(member)) - else: - self.imported_classes[name] = member - elif name == "__doc__": - self.doc_string = member - elif name not in self.attributes_blacklist: - self.attributes.append(AttributeStubsGenerator(name, member)) - - for x in itertools.chain( - self.submodules, self.classes, self.free_functions, self.attributes - ): - x.parse() - - def class_ordering( - a, b - ): # type: (ClassStubsGenerator, ClassStubsGenerator) -> int - if a.klass is b.klass: - return 0 - if issubclass(a.klass, b.klass): - return -1 - if issubclass(b.klass, a.klass): - return 1 - return 0 - - # reorder classes so base classes would be printed before derived - # print([ k.klass.__name__ for k in self.classes ]) - for i in range(len(self.classes)): - for j in range(i + 1, len(self.classes)): - if class_ordering(self.classes[i], self.classes[j]) < 0: - t = self.classes[i] - self.classes[i] = self.classes[j] - self.classes[j] = t - # print( [ k.klass.__name__ for k in self.classes ] ) - - def get_involved_modules_names(self): - result = set(self.imported_modules) - - for attr in self.attributes: - result |= attr.get_involved_modules_names() - - for C in self.classes: # type: ClassStubsGenerator - result |= C.get_involved_modules_names() - - for f in self.free_functions: # type: FreeFunctionStubsGenerator - result |= f.get_involved_modules_names() - - return set(result) - {"builtins", "typing", self.module.__name__} - - def to_lines(self): # type: () -> List[str] - - result = [] - - if self.doc_string: - result += ['"""' + self.doc_string.replace('"""', r"\"\"\"") + '"""'] - - result += ["import {}".format(self.module.__name__)] - - # import everything from typing - result += ["import typing"] - - for name, class_ in self.imported_classes.items(): - class_name = getattr(class_, "__qualname__", class_.__name__) - if name == class_name: - suffix = "" - else: - suffix = " as {}".format(name) - result += [ - "from {} import {}{}".format(class_.__module__, class_name, suffix) - ] - - # import used packages - used_modules = sorted(self.get_involved_modules_names()) - if used_modules: - # result.append("if TYPE_CHECKING:") - # result.extend(map(self.indent, map(lambda m: "import {}".format(m), used_modules))) - result.extend(map(lambda mod: "import {}".format(mod), used_modules)) - - if "numpy" in used_modules and not BARE_NUPMY_NDARRAY: - result += ["_Shape = typing.Tuple[int, ...]"] - - # add space between imports and rest of module - result += [""] - - globals_ = {} - exec("from {} import *".format(self.module.__name__), globals_) - all_ = set(globals_.keys()) - {"__builtins__"} - # ---------------------------------------------------------------------------- # - # NOTE(jigu): Add the following codes to deal with extension submodules - # I use a simple way to distinguish extension from pure python module - # More detail analysis can be found at https://stackoverflow.com/questions/20339053/in-python-how-can-one-tell-if-a-module-comes-from-a-c-extension. - from importlib.machinery import EXTENSION_SUFFIXES - - if ( - hasattr(self.module, "__file__") - and os.path.splitext(self.module.__file__)[-1] in EXTENSION_SUFFIXES - ): - result.extend(["from . import {}".format(k) for k in sorted(all_)]) - result += [""] - # ---------------------------------------------------------------------------- # - result.append( - "__all__ = [\n " - + ",\n ".join(map(lambda s: '"%s"' % s, sorted(all_))) - + "\n]\n\n" - ) - - for x in itertools.chain(self.classes, self.free_functions, self.attributes): - result.extend(x.to_lines()) - result.append("") # Newline at EOF - return result - - @property - def short_name(self): - return self.module.__name__.split(".")[-1] - - def write(self): - if not os.path.exists(self.short_name + self.stub_suffix): - logger.debug("mkdir `%s`" % (self.short_name + self.stub_suffix)) - os.mkdir(self.short_name + self.stub_suffix) - - with DirectoryWalkerGuard(self.short_name + self.stub_suffix): - with open("__init__.pyi", "w") as init_pyi: - init_pyi.write("\n".join(self.to_lines())) - for m in self.submodules: - m.write() - - if self.write_setup_py: - with open("setup.py", "w") as setuppy: - setuppy.write("""from setuptools import setup -import os - - -def find_stubs(package): - stubs = [] - for root, dirs, files in os.walk(package): - for file in files: - path = os.path.join(root, file).replace(package + os.sep, '', 1) - stubs.append(path) - return dict(package=stubs) - - -setup( - name='{package_name}-stubs', - maintainer="{package_name} Developers", - maintainer_email="example@python.org", - description="PEP 561 type stubs for {package_name}", - version='1.0', - packages=['{package_name}-stubs'], - # PEP 561 requires these - install_requires=['{package_name}'], - package_data=find_stubs('{package_name}-stubs'), -)""".format(package_name=self.short_name)) - - -def recursive_mkdir_walker(subdirs, callback): # type: (List[str], Callable) -> None - if len(subdirs) == 0: - callback() - else: - if not os.path.exists(subdirs[0]): - os.mkdir(subdirs[0]) - with DirectoryWalkerGuard(subdirs[0]): - recursive_mkdir_walker(subdirs[1:], callback) - - -def main(args=None): - parser = ArgumentParser( - prog="pybind11-stubgen", description="Generates stubs for specified modules" - ) - parser.add_argument( - "-o", - "--output-dir", - help="the root directory for output stubs", - default="./stubs", - ) - parser.add_argument( - "--root-module-suffix", - type=str, - default="-stubs", - dest="root_module_suffix", - help="optional suffix to disambiguate from the original package", - ) - parser.add_argument( - "--root_module_suffix", - type=str, - default=None, - dest="root_module_suffix_deprecated", - help="Deprecated. Use `--root-module-suffix`", - ) - parser.add_argument("--no-setup-py", action="store_true") - parser.add_argument( - "--non-stop", action="store_true", help="Deprecated. Use `--ignore-invalid=all`" - ) - parser.add_argument( - "--ignore-invalid", - nargs="+", - choices=["signature", "defaultarg", "all"], - default=[], - help="Ignore invalid specified python expressions in docstrings", - ) - parser.add_argument( - "--skip-signature-downgrade", - action="store_true", - help="Do not downgrade invalid function signatures to func(*args, **kwargs)", - ) - parser.add_argument( - "--bare-numpy-ndarray", - action="store_true", - default=False, - help=( - "Render `numpy.ndarray` without (non-standardized) bracket-enclosed type" - " and shape info" - ), - ) - parser.add_argument( - "module_names", nargs="+", metavar="MODULE_NAME", type=str, help="modules names" - ) - parser.add_argument("--log-level", default="INFO", help="Set output log level") - - sys_args = parser.parse_args(args or sys.argv[1:]) - - if sys_args.non_stop: - sys_args.ignore_invalid = ["all"] - warnings.warn( - "`--non-stop` is deprecated in favor of `--ignore-invalid=all`", - FutureWarning, - ) - - if sys_args.bare_numpy_ndarray: - global BARE_NUPMY_NDARRAY - BARE_NUPMY_NDARRAY = True - - if "all" in sys_args.ignore_invalid: - FunctionSignature.ignore_invalid_signature = True - FunctionSignature.ignore_invalid_defaultarg = True - else: - if "signature" in sys_args.ignore_invalid: - FunctionSignature.ignore_invalid_signature = True - if "defaultarg" in sys_args.ignore_invalid: - FunctionSignature.ignore_invalid_defaultarg = True - - if sys_args.skip_signature_downgrade: - FunctionSignature.signature_downgrade = False - - if sys_args.root_module_suffix_deprecated is not None: - sys_args.root_module_suffix = sys_args.root_module_suffix_deprecated - warnings.warn( - "`--root_module_suffix` is deprecated in favor of `--root-module-suffix`", - FutureWarning, - ) - - stderr_handler = logging.StreamHandler(sys.stderr) - handlers = [stderr_handler] - - logging.basicConfig( - level=logging.getLevelName(sys_args.log_level), - format="[%(asctime)s] {%(filename)s:%(lineno)d} %(levelname)s - %(message)s", - handlers=handlers, - ) - - output_path = sys_args.output_dir - - if not os.path.exists(output_path): - os.mkdir(output_path) - - with DirectoryWalkerGuard(output_path): - for _module_name in sys_args.module_names: - _module = ModuleStubsGenerator(_module_name) - _module.parse() - if FunctionSignature.n_fatal_errors() == 0: - _module.stub_suffix = sys_args.root_module_suffix - _module.write_setup_py = not sys_args.no_setup_py - recursive_mkdir_walker( - _module_name.split(".")[:-1], lambda: _module.write() - ) - - if FunctionSignature.n_invalid_signatures > 0: - logger.info("Useful link: Avoiding C++ types in docstrings:") - logger.info( - " https://pybind11.readthedocs.io/en/latest/advanced/misc.html" - "#avoiding-cpp-types-in-docstrings" - ) - - if FunctionSignature.n_invalid_default_values > 0: - logger.info("Useful link: Default argument representation:") - logger.info( - " " - " https://pybind11.readthedocs.io/en/latest/advanced/functions.html" - "#default-arguments-revisited" - ) - - if FunctionSignature.n_fatal_errors() > 0: - exit(1) - - -if __name__ == "__main__": - main() diff --git a/dev/stubgen.sh b/dev/stubgen.sh deleted file mode 100644 index bdc1bac3..00000000 --- a/dev/stubgen.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env bash -# python3.[version] pip install pybind11-stubgen - -if [ -d stubs ]; then - rm -rf stubs -fi - -python3.11 dev/stubgen.py mplib --no-setup-py --ignore-invalid all -rm -rf mplib/pymp && cp -r stubs/mplib-stubs/pymp mplib/pymp diff --git a/dev/update_bashrc.sh b/dev/update_bashrc.sh deleted file mode 100644 index 4c63472c..00000000 --- a/dev/update_bashrc.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env bash - -PY36_BIN=/opt/python/cp36-cp36m/bin -PY36_INCLUDE=/opt/python/cp36-cp36m/include/python3.6m -PY38_BIN=/opt/python/cp38-cp38/bin -PY38_INCLUDE=/opt/python/cp38-cp38/include/python3.8 - -PY_BIN=$PY36_BIN -PY_INCLUDE=$PY36_INCLUDE - -echo "export PATH=${PY_BIN}:\$PATH" >> ~/.bashrc -echo "export CPLUS_INCLUDE_PATH=${PY_INCLUDE}:\$CPLUS_INCLUDE_PATH" >> ~/.bashrc \ No newline at end of file diff --git a/docs/mplib.html b/docs/mplib.html index 71e82578..cee8715e 100644 --- a/docs/mplib.html +++ b/docs/mplib.html @@ -38,6 +38,10 @@

Submodules

  • pymp
    • +

    API Documentation

    +
      +
    + @@ -56,6 +60,12 @@

    MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up. With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.

    +

    documentation

    + +

    + +

    +

    Installation

    Pre-built pip packages support Ubuntu 18.04+ with Python 3.6+.

    @@ -72,11 +82,16 @@

    Usage

    - 
    1"""
-2.. include:: ./README.md
-3"""
-4
-5from mplib.planner import Planner
+                        
     1"""
+ 2.. include:: ./README.md
+ 3"""
+ 4
+ 5from importlib.metadata import version
+ 6
+ 7from .planner import Planner
+ 8# from .pymp import set_global_seed
+ 9
+10__version__ = version("mplib")
 
    
 
 
diff --git a/docs/mplib/examples/collision_avoidance.html b/docs/mplib/examples/collision_avoidance.html
index 602845ef..f4ddb6df 100644
--- a/docs/mplib/examples/collision_avoidance.html
+++ b/docs/mplib/examples/collision_avoidance.html
@@ -79,106 +79,113 @@ 
    
                         
 
                         
      1import sapien.core as sapien
-  2from .demo_setup import DemoSetup
-  3
-  4class PlanningDemo(DemoSetup):
-  5    """ The shows the planner's ability to generate a collision free path with the straight path causes collisions """
-  6    def __init__(self):
-  7        """
-  8        Same setup as demo.py except the boxes are of difference sizes and different use
-  9        Red box is the target we want to grab
- 10        Blue box is the obstacle we want to avoid
- 11        green box is the landing pad on which we want to place the red box
- 12        """
- 13        super().__init__()
- 14        self.setup_scene()
- 15        self.load_robot()
- 16        self.setup_planner()
- 17
- 18        # Set initial joint positions
- 19        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
- 20        self.robot.set_qpos(init_qpos)
- 21
- 22        # table top
- 23        builder = self.scene.create_actor_builder()
- 24        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
- 25        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
- 26        self.table = builder.build_kinematic(name='table')
- 27        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
- 28
- 29        # red box is the target we want to grab
- 30        builder = self.scene.create_actor_builder()
- 31        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
- 32        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
- 33        self.red_cube = builder.build(name='red_cube')
- 34        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
- 35
- 36        # green box is the landing pad on which we want to place the red box
- 37        builder = self.scene.create_actor_builder()
- 38        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
- 39        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
- 40        self.green_cube = builder.build(name='green_cube')
- 41        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
- 42
- 43        # blue box is the obstacle we want to avoid
- 44        builder = self.scene.create_actor_builder()
- 45        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
- 46        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
- 47        self.blue_cube = builder.build(name='blue_cube')
- 48        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
- 49
- 50    def add_point_cloud(self):
- 51        """ we tell the planner about the obstacle through a point cloud """
- 52        import trimesh
- 53        box = trimesh.creation.box([0.1, 0.4, 0.2])
- 54        points, _ = trimesh.sample.sample_surface(box, 1000)
- 55        points += [0.55, 0, 0.1]
- 56        self.planner.update_point_cloud(points, radius=0.02)
- 57        return 
- 58
- 59    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
- 60        """
- 61        We pick up the red box while avoiding the blue box and place it back down on top of the green box
- 62        """
- 63        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
- 64        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
- 65        
- 66        # tell the planner where the obstacle is
- 67        if use_point_cloud:
- 68            self.add_point_cloud()
- 69        
- 70        # move to the pickup pose
- 71        pickup_pose[2] += 0.2
- 72        # no need to check collision against attached object since nothing picked up yet
- 73        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
- 74        self.open_gripper()
- 75        pickup_pose[2] -= 0.12
- 76        # no attach since nothing picked up yet
- 77        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
- 78        self.close_gripper()
- 79
- 80        if use_attach:
- 81            self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
- 82
- 83        # move to the delivery pose
- 84        pickup_pose[2] += 0.12
- 85        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) 
- 86        delivery_pose[2] += 0.2
- 87        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
- 88        delivery_pose[2] -= 0.12
- 89        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
- 90        self.open_gripper()
- 91        delivery_pose[2] += 0.12
- 92        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) 
- 93
- 94if __name__ == '__main__':
- 95    """
- 96    As you change some of the parameters, you will see different behaviors
- 97    In particular, when point cloud is not used, the robot will attemt to go through the blue box
- 98    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
- 99    """
-100    demo = PlanningDemo()
-101    demo.demo(False, True, True)
+  2
+  3from .demo_setup import DemoSetup
+  4
+  5
+  6class PlanningDemo(DemoSetup):
+  7    """The shows the planner's ability to generate a collision free path with the straight path causes collisions"""
+  8
+  9    def __init__(self):
+ 10        """
+ 11        Same setup as demo.py except the boxes are of difference sizes and different use
+ 12        Red box is the target we want to grab
+ 13        Blue box is the obstacle we want to avoid
+ 14        green box is the landing pad on which we want to place the red box
+ 15        """
+ 16        super().__init__()
+ 17        self.setup_scene()
+ 18        self.load_robot()
+ 19        self.setup_planner()
+ 20
+ 21        # Set initial joint positions
+ 22        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+ 23        self.robot.set_qpos(init_qpos)
+ 24
+ 25        # table top
+ 26        builder = self.scene.create_actor_builder()
+ 27        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+ 28        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+ 29        self.table = builder.build_kinematic(name="table")
+ 30        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+ 31
+ 32        # red box is the target we want to grab
+ 33        builder = self.scene.create_actor_builder()
+ 34        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+ 35        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+ 36        self.red_cube = builder.build(name="red_cube")
+ 37        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+ 38
+ 39        # green box is the landing pad on which we want to place the red box
+ 40        builder = self.scene.create_actor_builder()
+ 41        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+ 42        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+ 43        self.green_cube = builder.build(name="green_cube")
+ 44        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+ 45
+ 46        # blue box is the obstacle we want to avoid
+ 47        builder = self.scene.create_actor_builder()
+ 48        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+ 49        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+ 50        self.blue_cube = builder.build(name="blue_cube")
+ 51        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+ 52
+ 53    def add_point_cloud(self):
+ 54        """we tell the planner about the obstacle through a point cloud"""
+ 55        import trimesh
+ 56
+ 57        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 58        points, _ = trimesh.sample.sample_surface(box, 1000)
+ 59        points += [0.55, 0, 0.1]
+ 60        self.planner.update_point_cloud(points, radius=0.02)
+ 61        return
+ 62
+ 63    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
+ 64        """
+ 65        We pick up the red box while avoiding the blue box and place it back down on top of the green box
+ 66        """
+ 67        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+ 68        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+ 69
+ 70        # tell the planner where the obstacle is
+ 71        if use_point_cloud:
+ 72            self.add_point_cloud()
+ 73
+ 74        # move to the pickup pose
+ 75        pickup_pose[2] += 0.2
+ 76        # no need to check collision against attached object since nothing picked up yet
+ 77        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+ 78        self.open_gripper()
+ 79        pickup_pose[2] -= 0.12
+ 80        # no attach since nothing picked up yet
+ 81        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+ 82        self.close_gripper()
+ 83
+ 84        if use_attach:
+ 85            self.planner.update_attached_box(
+ 86                [0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]
+ 87            )
+ 88
+ 89        # move to the delivery pose
+ 90        pickup_pose[2] += 0.12
+ 91        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach)
+ 92        delivery_pose[2] += 0.2
+ 93        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+ 94        delivery_pose[2] -= 0.12
+ 95        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+ 96        self.open_gripper()
+ 97        delivery_pose[2] += 0.12
+ 98        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False)
+ 99
+100
+101if __name__ == "__main__":
+102    """
+103    As you change some of the parameters, you will see different behaviors
+104    In particular, when point cloud is not used, the robot will attemt to go through the blue box
+105    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
+106    """
+107    demo = PlanningDemo()
+108    demo.demo(False, True, True)
 
    
 
 
@@ -194,95 +201,99 @@
    - 
     5class PlanningDemo(DemoSetup):
- 6    """ The shows the planner's ability to generate a collision free path with the straight path causes collisions """
- 7    def __init__(self):
- 8        """
- 9        Same setup as demo.py except the boxes are of difference sizes and different use
-10        Red box is the target we want to grab
-11        Blue box is the obstacle we want to avoid
-12        green box is the landing pad on which we want to place the red box
-13        """
-14        super().__init__()
-15        self.setup_scene()
-16        self.load_robot()
-17        self.setup_planner()
-18
-19        # Set initial joint positions
-20        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-21        self.robot.set_qpos(init_qpos)
-22
-23        # table top
-24        builder = self.scene.create_actor_builder()
-25        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-26        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-27        self.table = builder.build_kinematic(name='table')
-28        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-29
-30        # red box is the target we want to grab
-31        builder = self.scene.create_actor_builder()
-32        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-33        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-34        self.red_cube = builder.build(name='red_cube')
-35        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
-36
-37        # green box is the landing pad on which we want to place the red box
-38        builder = self.scene.create_actor_builder()
-39        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
-40        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
-41        self.green_cube = builder.build(name='green_cube')
-42        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
-43
-44        # blue box is the obstacle we want to avoid
-45        builder = self.scene.create_actor_builder()
-46        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
-47        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
-48        self.blue_cube = builder.build(name='blue_cube')
-49        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
-50
-51    def add_point_cloud(self):
-52        """ we tell the planner about the obstacle through a point cloud """
-53        import trimesh
-54        box = trimesh.creation.box([0.1, 0.4, 0.2])
-55        points, _ = trimesh.sample.sample_surface(box, 1000)
-56        points += [0.55, 0, 0.1]
-57        self.planner.update_point_cloud(points, radius=0.02)
-58        return 
-59
-60    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
-61        """
-62        We pick up the red box while avoiding the blue box and place it back down on top of the green box
-63        """
-64        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-65        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-66        
-67        # tell the planner where the obstacle is
-68        if use_point_cloud:
-69            self.add_point_cloud()
-70        
-71        # move to the pickup pose
-72        pickup_pose[2] += 0.2
-73        # no need to check collision against attached object since nothing picked up yet
-74        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
-75        self.open_gripper()
-76        pickup_pose[2] -= 0.12
-77        # no attach since nothing picked up yet
-78        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
-79        self.close_gripper()
-80
-81        if use_attach:
-82            self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
-83
-84        # move to the delivery pose
-85        pickup_pose[2] += 0.12
-86        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) 
-87        delivery_pose[2] += 0.2
-88        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
-89        delivery_pose[2] -= 0.12
-90        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
-91        self.open_gripper()
-92        delivery_pose[2] += 0.12
-93        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) 
+            
     7class PlanningDemo(DemoSetup):
+ 8    """The shows the planner's ability to generate a collision free path with the straight path causes collisions"""
+ 9
+10    def __init__(self):
+11        """
+12        Same setup as demo.py except the boxes are of difference sizes and different use
+13        Red box is the target we want to grab
+14        Blue box is the obstacle we want to avoid
+15        green box is the landing pad on which we want to place the red box
+16        """
+17        super().__init__()
+18        self.setup_scene()
+19        self.load_robot()
+20        self.setup_planner()
+21
+22        # Set initial joint positions
+23        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+24        self.robot.set_qpos(init_qpos)
+25
+26        # table top
+27        builder = self.scene.create_actor_builder()
+28        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+29        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+30        self.table = builder.build_kinematic(name="table")
+31        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+32
+33        # red box is the target we want to grab
+34        builder = self.scene.create_actor_builder()
+35        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+36        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+37        self.red_cube = builder.build(name="red_cube")
+38        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+39
+40        # green box is the landing pad on which we want to place the red box
+41        builder = self.scene.create_actor_builder()
+42        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+43        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+44        self.green_cube = builder.build(name="green_cube")
+45        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+46
+47        # blue box is the obstacle we want to avoid
+48        builder = self.scene.create_actor_builder()
+49        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+50        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+51        self.blue_cube = builder.build(name="blue_cube")
+52        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+53
+54    def add_point_cloud(self):
+55        """we tell the planner about the obstacle through a point cloud"""
+56        import trimesh
+57
+58        box = trimesh.creation.box([0.1, 0.4, 0.2])
+59        points, _ = trimesh.sample.sample_surface(box, 1000)
+60        points += [0.55, 0, 0.1]
+61        self.planner.update_point_cloud(points, radius=0.02)
+62        return
+63
+64    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
+65        """
+66        We pick up the red box while avoiding the blue box and place it back down on top of the green box
+67        """
+68        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+69        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+70
+71        # tell the planner where the obstacle is
+72        if use_point_cloud:
+73            self.add_point_cloud()
+74
+75        # move to the pickup pose
+76        pickup_pose[2] += 0.2
+77        # no need to check collision against attached object since nothing picked up yet
+78        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+79        self.open_gripper()
+80        pickup_pose[2] -= 0.12
+81        # no attach since nothing picked up yet
+82        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+83        self.close_gripper()
+84
+85        if use_attach:
+86            self.planner.update_attached_box(
+87                [0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]
+88            )
+89
+90        # move to the delivery pose
+91        pickup_pose[2] += 0.12
+92        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach)
+93        delivery_pose[2] += 0.2
+94        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+95        delivery_pose[2] -= 0.12
+96        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+97        self.open_gripper()
+98        delivery_pose[2] += 0.12
+99        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False)
 
    
 
 
@@ -300,49 +311,49 @@
    - 
     7    def __init__(self):
- 8        """
- 9        Same setup as demo.py except the boxes are of difference sizes and different use
-10        Red box is the target we want to grab
-11        Blue box is the obstacle we want to avoid
-12        green box is the landing pad on which we want to place the red box
-13        """
-14        super().__init__()
-15        self.setup_scene()
-16        self.load_robot()
-17        self.setup_planner()
-18
-19        # Set initial joint positions
-20        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-21        self.robot.set_qpos(init_qpos)
-22
-23        # table top
-24        builder = self.scene.create_actor_builder()
-25        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-26        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-27        self.table = builder.build_kinematic(name='table')
-28        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-29
-30        # red box is the target we want to grab
-31        builder = self.scene.create_actor_builder()
-32        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-33        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-34        self.red_cube = builder.build(name='red_cube')
-35        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
-36
-37        # green box is the landing pad on which we want to place the red box
-38        builder = self.scene.create_actor_builder()
-39        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
-40        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
-41        self.green_cube = builder.build(name='green_cube')
-42        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
-43
-44        # blue box is the obstacle we want to avoid
-45        builder = self.scene.create_actor_builder()
-46        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
-47        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
-48        self.blue_cube = builder.build(name='blue_cube')
-49        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+            
    10    def __init__(self):
+11        """
+12        Same setup as demo.py except the boxes are of difference sizes and different use
+13        Red box is the target we want to grab
+14        Blue box is the obstacle we want to avoid
+15        green box is the landing pad on which we want to place the red box
+16        """
+17        super().__init__()
+18        self.setup_scene()
+19        self.load_robot()
+20        self.setup_planner()
+21
+22        # Set initial joint positions
+23        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+24        self.robot.set_qpos(init_qpos)
+25
+26        # table top
+27        builder = self.scene.create_actor_builder()
+28        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+29        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+30        self.table = builder.build_kinematic(name="table")
+31        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+32
+33        # red box is the target we want to grab
+34        builder = self.scene.create_actor_builder()
+35        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+36        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+37        self.red_cube = builder.build(name="red_cube")
+38        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+39
+40        # green box is the landing pad on which we want to place the red box
+41        builder = self.scene.create_actor_builder()
+42        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+43        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+44        self.green_cube = builder.build(name="green_cube")
+45        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+46
+47        # blue box is the obstacle we want to avoid
+48        builder = self.scene.create_actor_builder()
+49        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+50        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+51        self.blue_cube = builder.build(name="blue_cube")
+52        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
 
    
 
 
@@ -409,14 +420,15 @@
    - 
    51    def add_point_cloud(self):
-52        """ we tell the planner about the obstacle through a point cloud """
-53        import trimesh
-54        box = trimesh.creation.box([0.1, 0.4, 0.2])
-55        points, _ = trimesh.sample.sample_surface(box, 1000)
-56        points += [0.55, 0, 0.1]
-57        self.planner.update_point_cloud(points, radius=0.02)
-58        return 
+            
    54    def add_point_cloud(self):
+55        """we tell the planner about the obstacle through a point cloud"""
+56        import trimesh
+57
+58        box = trimesh.creation.box([0.1, 0.4, 0.2])
+59        points, _ = trimesh.sample.sample_surface(box, 1000)
+60        points += [0.55, 0, 0.1]
+61        self.planner.update_point_cloud(points, radius=0.02)
+62        return
 
    
 
 
@@ -436,40 +448,42 @@
    - 
    60    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
-61        """
-62        We pick up the red box while avoiding the blue box and place it back down on top of the green box
-63        """
-64        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-65        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-66        
-67        # tell the planner where the obstacle is
-68        if use_point_cloud:
-69            self.add_point_cloud()
-70        
-71        # move to the pickup pose
-72        pickup_pose[2] += 0.2
-73        # no need to check collision against attached object since nothing picked up yet
-74        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
-75        self.open_gripper()
-76        pickup_pose[2] -= 0.12
-77        # no attach since nothing picked up yet
-78        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False) 
-79        self.close_gripper()
-80
-81        if use_attach:
-82            self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
-83
-84        # move to the delivery pose
-85        pickup_pose[2] += 0.12
-86        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach) 
-87        delivery_pose[2] += 0.2
-88        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
-89        delivery_pose[2] -= 0.12
-90        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach) 
-91        self.open_gripper()
-92        delivery_pose[2] += 0.12
-93        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False) 
+            
    64    def demo(self, with_screw=True, use_point_cloud=True, use_attach=True):
+65        """
+66        We pick up the red box while avoiding the blue box and place it back down on top of the green box
+67        """
+68        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+69        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+70
+71        # tell the planner where the obstacle is
+72        if use_point_cloud:
+73            self.add_point_cloud()
+74
+75        # move to the pickup pose
+76        pickup_pose[2] += 0.2
+77        # no need to check collision against attached object since nothing picked up yet
+78        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+79        self.open_gripper()
+80        pickup_pose[2] -= 0.12
+81        # no attach since nothing picked up yet
+82        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach=False)
+83        self.close_gripper()
+84
+85        if use_attach:
+86            self.planner.update_attached_box(
+87                [0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0]
+88            )
+89
+90        # move to the delivery pose
+91        pickup_pose[2] += 0.12
+92        self.move_to_pose(pickup_pose, with_screw, use_point_cloud, use_attach)
+93        delivery_pose[2] += 0.2
+94        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+95        delivery_pose[2] -= 0.12
+96        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach)
+97        self.open_gripper()
+98        delivery_pose[2] += 0.12
+99        self.move_to_pose(delivery_pose, with_screw, use_point_cloud, use_attach=False)
 
    
 
 
diff --git a/docs/mplib/examples/constrained_planning.html b/docs/mplib/examples/constrained_planning.html
index 648a87b0..3fea7cf6 100644
--- a/docs/mplib/examples/constrained_planning.html
+++ b/docs/mplib/examples/constrained_planning.html
@@ -79,117 +79,140 @@ 
    
   2
   3import numpy as np
   4import transforms3d
-  5from .demo_setup import DemoSetup
-  6
-  7class ConstrainedPlanningDemo(DemoSetup):
-  8  """
-  9  This demo shows the planner's ability to plan with constraints.
- 10  For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
- 11  """
- 12  def __init__(self):
- 13    """ set up the scene and load the robot """
- 14    super().__init__()
- 15    self.setup_scene()
- 16    self.load_robot()
- 17    self.setup_planner()
- 18
- 19  def add_point_cloud(self):
- 20    """ add some random obstacles to make the planning more challenging """
- 21    import trimesh
- 22    box = trimesh.creation.box([0.1, 0.4, 0.2])
- 23    points, _ = trimesh.sample.sample_surface(box, 1000)
- 24    all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0)
- 25    self.planner.update_point_cloud(all_pts, radius=0.02)
- 26    return
- 27
- 28  def get_eef_z(self):
- 29    """ helper function for constraint """
- 30    ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
- 31    ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
- 32    mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
- 33    return mat[:,2]
- 34
- 35  def make_f(self):
- 36    """
- 37    create a constraint function that takes in a qpos and outputs a scalar
- 38    A valid constraint function should evaluates to 0 when the constraint is satisfied
- 39    See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
- 40    """
- 41    def f(x, out):
- 42      self.planner.robot.set_qpos(x)
- 43      out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966  # maintain 15 degrees w.r.t. -z axis
- 44    return f
- 45
- 46  def make_j(self):
- 47    """
- 48    create the jacobian of the constraint function w.r.t. qpos
- 49    This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
- 50    """
- 51    def j(x, out):
- 52      full_qpos = self.planner.pad_qpos(x)
- 53      jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1)
- 54      rot_jac = jac[3:, self.planner.move_group_joint_indices]
- 55      for i in range(len(self.planner.move_group_joint_indices)):
- 56        out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1]))
- 57    return j
- 58
- 59  def demo(self):
- 60    """
- 61    We first plan with constraints to three poses, then plan without constraints to the same poses
- 62    While not always the case, sometimes without constraints, the end effector will tilt almost upside down
- 63    """
- 64    # this starting pose has the end effector tilted roughly 15 degrees
- 65    starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
- 66    self.robot.set_qpos(starting_qpos)
- 67    self.planner.robot.set_qpos(starting_qpos[:7])
- 68    # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
- 69    poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
- 70             [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
- 71             [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]]
- 72    
- 73    # add some point cloud to make the planning more challenging so we can see the effect of no constraint
- 74    self.add_point_cloud()
- 75
- 76    # with constraint
- 77    print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis")
- 78    for pose in poses:
- 79      result = self.planner.plan_qpos_to_pose(
- 80        pose,
- 81        self.robot.get_qpos(),
- 82        time_step=1/250,
- 83        use_point_cloud=True,
- 84        use_attach=False,
- 85        planner_name="RRTConnect",
- 86        constraint_function=self.make_f(),
- 87        constraint_jacobian=self.make_j(),
- 88        constraint_tolerance=0.05,
- 89      )
- 90      if result['status'] != "Success":
- 91        print(result['status'])
- 92        return -1
- 93      self.follow_path(result)
- 94
- 95    # without constraint
- 96    print("without constraint. certain movements can sometimes tilt the end effector almost upside down")
- 97    for pose in poses:
- 98      result = self.planner.plan_qpos_to_pose(
- 99        pose,
-100        self.robot.get_qpos(),
-101        time_step=1/250,
-102        use_point_cloud=True,
-103        use_attach=False,
-104        planner_name="RRTConnect",
-105        no_simplification=True
-106      )
-107      if result['status'] != "Success":
-108        print(result['status'])
-109        return -1
-110      self.follow_path(result)
-111
-112
-113if __name__ == '__main__':
-114  demo = ConstrainedPlanningDemo()
-115  demo.demo()
+  5
+  6from .demo_setup import DemoSetup
+  7
+  8
+  9class ConstrainedPlanningDemo(DemoSetup):
+ 10    """
+ 11    This demo shows the planner's ability to plan with constraints.
+ 12    For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
+ 13    """
+ 14
+ 15    def __init__(self):
+ 16        """set up the scene and load the robot"""
+ 17        super().__init__()
+ 18        self.setup_scene()
+ 19        self.load_robot()
+ 20        self.setup_planner()
+ 21
+ 22    def add_point_cloud(self):
+ 23        """add some random obstacles to make the planning more challenging"""
+ 24        import trimesh
+ 25
+ 26        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 27        points, _ = trimesh.sample.sample_surface(box, 1000)
+ 28        all_pts = np.concatenate(
+ 29            [points + [-0.65, -0.1, 0.4], points + [0.55, 0, 0.1]], axis=0
+ 30        )
+ 31        self.planner.update_point_cloud(all_pts, radius=0.02)
+ 32        return
+ 33
+ 34    def get_eef_z(self):
+ 35        """helper function for constraint"""
+ 36        ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
+ 37        ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
+ 38        mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
+ 39        return mat[:, 2]
+ 40
+ 41    def make_f(self):
+ 42        """
+ 43        create a constraint function that takes in a qpos and outputs a scalar
+ 44        A valid constraint function should evaluates to 0 when the constraint is satisfied
+ 45        See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
+ 46        """
+ 47
+ 48        def f(x, out):
+ 49            self.planner.robot.set_qpos(x)
+ 50            out[0] = (
+ 51                self.get_eef_z().dot(np.array([0, 0, -1])) - 0.966
+ 52            )  # maintain 15 degrees w.r.t. -z axis
+ 53
+ 54        return f
+ 55
+ 56    def make_j(self):
+ 57        """
+ 58        create the jacobian of the constraint function w.r.t. qpos
+ 59        This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
+ 60        """
+ 61
+ 62        def j(x, out):
+ 63            full_qpos = self.planner.pad_qpos(x)
+ 64            jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(
+ 65                full_qpos, len(self.planner.move_group_joint_indices) - 1
+ 66            )
+ 67            rot_jac = jac[3:, self.planner.move_group_joint_indices]
+ 68            for i in range(len(self.planner.move_group_joint_indices)):
+ 69                out[i] = np.cross(rot_jac[:, i], self.get_eef_z()).dot(
+ 70                    np.array([0, 0, -1])
+ 71                )
+ 72
+ 73        return j
+ 74
+ 75    def demo(self):
+ 76        """
+ 77        We first plan with constraints to three poses, then plan without constraints to the same poses
+ 78        While not always the case, sometimes without constraints, the end effector will tilt almost upside down
+ 79        """
+ 80        # this starting pose has the end effector tilted roughly 15 degrees
+ 81        starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
+ 82        self.robot.set_qpos(starting_qpos)
+ 83        self.planner.robot.set_qpos(starting_qpos[:7])
+ 84        # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
+ 85        poses = [
+ 86            [-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
+ 87            [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
+ 88            [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0],
+ 89        ]
+ 90
+ 91        # add some point cloud to make the planning more challenging so we can see the effect of no constraint
+ 92        self.add_point_cloud()
+ 93
+ 94        # with constraint
+ 95        print(
+ 96            "with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis"
+ 97        )
+ 98        for pose in poses:
+ 99            result = self.planner.plan_qpos_to_pose(
+100                pose,
+101                self.robot.get_qpos(),
+102                time_step=1 / 250,
+103                use_point_cloud=True,
+104                use_attach=False,
+105                planner_name="RRTConnect",
+106                constraint_function=self.make_f(),
+107                constraint_jacobian=self.make_j(),
+108                constraint_tolerance=0.05,
+109            )
+110            if result["status"] != "Success":
+111                print(result["status"])
+112                return -1
+113            self.follow_path(result)
+114
+115        # without constraint
+116        print(
+117            "without constraint. certain movements can sometimes tilt the end effector"
+118            " almost upside down"
+119        )
+120        for pose in poses:
+121            result = self.planner.plan_qpos_to_pose(
+122                pose,
+123                self.robot.get_qpos(),
+124                time_step=1 / 250,
+125                use_point_cloud=True,
+126                use_attach=False,
+127                planner_name="RRTConnect",
+128                no_simplification=True,
+129            )
+130            if result["status"] != "Success":
+131                print(result["status"])
+132                return -1
+133            self.follow_path(result)
+134
+135
+136if __name__ == "__main__":
+137    demo = ConstrainedPlanningDemo()
+138    demo.demo()
    @@ -205,110 +228,131 @@

    - 
      8class ConstrainedPlanningDemo(DemoSetup):
-  9  """
- 10  This demo shows the planner's ability to plan with constraints.
- 11  For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
- 12  """
- 13  def __init__(self):
- 14    """ set up the scene and load the robot """
- 15    super().__init__()
- 16    self.setup_scene()
- 17    self.load_robot()
- 18    self.setup_planner()
- 19
- 20  def add_point_cloud(self):
- 21    """ add some random obstacles to make the planning more challenging """
- 22    import trimesh
- 23    box = trimesh.creation.box([0.1, 0.4, 0.2])
- 24    points, _ = trimesh.sample.sample_surface(box, 1000)
- 25    all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0)
- 26    self.planner.update_point_cloud(all_pts, radius=0.02)
- 27    return
- 28
- 29  def get_eef_z(self):
- 30    """ helper function for constraint """
- 31    ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
- 32    ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
- 33    mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
- 34    return mat[:,2]
- 35
- 36  def make_f(self):
- 37    """
- 38    create a constraint function that takes in a qpos and outputs a scalar
- 39    A valid constraint function should evaluates to 0 when the constraint is satisfied
- 40    See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
- 41    """
- 42    def f(x, out):
- 43      self.planner.robot.set_qpos(x)
- 44      out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966  # maintain 15 degrees w.r.t. -z axis
- 45    return f
- 46
- 47  def make_j(self):
- 48    """
- 49    create the jacobian of the constraint function w.r.t. qpos
- 50    This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
- 51    """
- 52    def j(x, out):
- 53      full_qpos = self.planner.pad_qpos(x)
- 54      jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1)
- 55      rot_jac = jac[3:, self.planner.move_group_joint_indices]
- 56      for i in range(len(self.planner.move_group_joint_indices)):
- 57        out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1]))
- 58    return j
- 59
- 60  def demo(self):
- 61    """
- 62    We first plan with constraints to three poses, then plan without constraints to the same poses
- 63    While not always the case, sometimes without constraints, the end effector will tilt almost upside down
- 64    """
- 65    # this starting pose has the end effector tilted roughly 15 degrees
- 66    starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
- 67    self.robot.set_qpos(starting_qpos)
- 68    self.planner.robot.set_qpos(starting_qpos[:7])
- 69    # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
- 70    poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
- 71             [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
- 72             [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]]
- 73    
- 74    # add some point cloud to make the planning more challenging so we can see the effect of no constraint
- 75    self.add_point_cloud()
- 76
- 77    # with constraint
- 78    print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis")
- 79    for pose in poses:
- 80      result = self.planner.plan_qpos_to_pose(
- 81        pose,
- 82        self.robot.get_qpos(),
- 83        time_step=1/250,
- 84        use_point_cloud=True,
- 85        use_attach=False,
- 86        planner_name="RRTConnect",
- 87        constraint_function=self.make_f(),
- 88        constraint_jacobian=self.make_j(),
- 89        constraint_tolerance=0.05,
- 90      )
- 91      if result['status'] != "Success":
- 92        print(result['status'])
- 93        return -1
- 94      self.follow_path(result)
- 95
- 96    # without constraint
- 97    print("without constraint. certain movements can sometimes tilt the end effector almost upside down")
- 98    for pose in poses:
- 99      result = self.planner.plan_qpos_to_pose(
-100        pose,
-101        self.robot.get_qpos(),
-102        time_step=1/250,
-103        use_point_cloud=True,
-104        use_attach=False,
-105        planner_name="RRTConnect",
-106        no_simplification=True
-107      )
-108      if result['status'] != "Success":
-109        print(result['status'])
-110        return -1
-111      self.follow_path(result)
+            
     10class ConstrainedPlanningDemo(DemoSetup):
+ 11    """
+ 12    This demo shows the planner's ability to plan with constraints.
+ 13    For this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
+ 14    """
+ 15
+ 16    def __init__(self):
+ 17        """set up the scene and load the robot"""
+ 18        super().__init__()
+ 19        self.setup_scene()
+ 20        self.load_robot()
+ 21        self.setup_planner()
+ 22
+ 23    def add_point_cloud(self):
+ 24        """add some random obstacles to make the planning more challenging"""
+ 25        import trimesh
+ 26
+ 27        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 28        points, _ = trimesh.sample.sample_surface(box, 1000)
+ 29        all_pts = np.concatenate(
+ 30            [points + [-0.65, -0.1, 0.4], points + [0.55, 0, 0.1]], axis=0
+ 31        )
+ 32        self.planner.update_point_cloud(all_pts, radius=0.02)
+ 33        return
+ 34
+ 35    def get_eef_z(self):
+ 36        """helper function for constraint"""
+ 37        ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
+ 38        ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
+ 39        mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
+ 40        return mat[:, 2]
+ 41
+ 42    def make_f(self):
+ 43        """
+ 44        create a constraint function that takes in a qpos and outputs a scalar
+ 45        A valid constraint function should evaluates to 0 when the constraint is satisfied
+ 46        See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
+ 47        """
+ 48
+ 49        def f(x, out):
+ 50            self.planner.robot.set_qpos(x)
+ 51            out[0] = (
+ 52                self.get_eef_z().dot(np.array([0, 0, -1])) - 0.966
+ 53            )  # maintain 15 degrees w.r.t. -z axis
+ 54
+ 55        return f
+ 56
+ 57    def make_j(self):
+ 58        """
+ 59        create the jacobian of the constraint function w.r.t. qpos
+ 60        This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
+ 61        """
+ 62
+ 63        def j(x, out):
+ 64            full_qpos = self.planner.pad_qpos(x)
+ 65            jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(
+ 66                full_qpos, len(self.planner.move_group_joint_indices) - 1
+ 67            )
+ 68            rot_jac = jac[3:, self.planner.move_group_joint_indices]
+ 69            for i in range(len(self.planner.move_group_joint_indices)):
+ 70                out[i] = np.cross(rot_jac[:, i], self.get_eef_z()).dot(
+ 71                    np.array([0, 0, -1])
+ 72                )
+ 73
+ 74        return j
+ 75
+ 76    def demo(self):
+ 77        """
+ 78        We first plan with constraints to three poses, then plan without constraints to the same poses
+ 79        While not always the case, sometimes without constraints, the end effector will tilt almost upside down
+ 80        """
+ 81        # this starting pose has the end effector tilted roughly 15 degrees
+ 82        starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
+ 83        self.robot.set_qpos(starting_qpos)
+ 84        self.planner.robot.set_qpos(starting_qpos[:7])
+ 85        # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
+ 86        poses = [
+ 87            [-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
+ 88            [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
+ 89            [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0],
+ 90        ]
+ 91
+ 92        # add some point cloud to make the planning more challenging so we can see the effect of no constraint
+ 93        self.add_point_cloud()
+ 94
+ 95        # with constraint
+ 96        print(
+ 97            "with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis"
+ 98        )
+ 99        for pose in poses:
+100            result = self.planner.plan_qpos_to_pose(
+101                pose,
+102                self.robot.get_qpos(),
+103                time_step=1 / 250,
+104                use_point_cloud=True,
+105                use_attach=False,
+106                planner_name="RRTConnect",
+107                constraint_function=self.make_f(),
+108                constraint_jacobian=self.make_j(),
+109                constraint_tolerance=0.05,
+110            )
+111            if result["status"] != "Success":
+112                print(result["status"])
+113                return -1
+114            self.follow_path(result)
+115
+116        # without constraint
+117        print(
+118            "without constraint. certain movements can sometimes tilt the end effector"
+119            " almost upside down"
+120        )
+121        for pose in poses:
+122            result = self.planner.plan_qpos_to_pose(
+123                pose,
+124                self.robot.get_qpos(),
+125                time_step=1 / 250,
+126                use_point_cloud=True,
+127                use_attach=False,
+128                planner_name="RRTConnect",
+129                no_simplification=True,
+130            )
+131            if result["status"] != "Success":
+132                print(result["status"])
+133                return -1
+134            self.follow_path(result)
 
    
 
 
@@ -327,12 +371,12 @@
    - 
    13  def __init__(self):
-14    """ set up the scene and load the robot """
-15    super().__init__()
-16    self.setup_scene()
-17    self.load_robot()
-18    self.setup_planner()
+            
    16    def __init__(self):
+17        """set up the scene and load the robot"""
+18        super().__init__()
+19        self.setup_scene()
+20        self.load_robot()
+21        self.setup_planner()
 
    
 
 
@@ -352,14 +396,17 @@
    - 
    20  def add_point_cloud(self):
-21    """ add some random obstacles to make the planning more challenging """
-22    import trimesh
-23    box = trimesh.creation.box([0.1, 0.4, 0.2])
-24    points, _ = trimesh.sample.sample_surface(box, 1000)
-25    all_pts = np.concatenate([points+[-0.65, -0.1, 0.4], points+[0.55, 0, 0.1]], axis=0)
-26    self.planner.update_point_cloud(all_pts, radius=0.02)
-27    return
+            
    23    def add_point_cloud(self):
+24        """add some random obstacles to make the planning more challenging"""
+25        import trimesh
+26
+27        box = trimesh.creation.box([0.1, 0.4, 0.2])
+28        points, _ = trimesh.sample.sample_surface(box, 1000)
+29        all_pts = np.concatenate(
+30            [points + [-0.65, -0.1, 0.4], points + [0.55, 0, 0.1]], axis=0
+31        )
+32        self.planner.update_point_cloud(all_pts, radius=0.02)
+33        return
 
    
 
 
@@ -379,12 +426,12 @@
    - 
    29  def get_eef_z(self):
-30    """ helper function for constraint """
-31    ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
-32    ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
-33    mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
-34    return mat[:,2]
+            
    35    def get_eef_z(self):
+36        """helper function for constraint"""
+37        ee_idx = self.planner.link_name_2_idx[self.planner.move_group]
+38        ee_pose = self.planner.robot.get_pinocchio_model().get_link_pose(ee_idx)
+39        mat = transforms3d.quaternions.quat2mat(ee_pose[3:])
+40        return mat[:, 2]
 
    
 
 
@@ -404,16 +451,20 @@
    - 
    36  def make_f(self):
-37    """
-38    create a constraint function that takes in a qpos and outputs a scalar
-39    A valid constraint function should evaluates to 0 when the constraint is satisfied
-40    See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
-41    """
-42    def f(x, out):
-43      self.planner.robot.set_qpos(x)
-44      out[0] = self.get_eef_z().dot(np.array([0,0,-1]))-0.966  # maintain 15 degrees w.r.t. -z axis
-45    return f
+            
    42    def make_f(self):
+43        """
+44        create a constraint function that takes in a qpos and outputs a scalar
+45        A valid constraint function should evaluates to 0 when the constraint is satisfied
+46        See [ompl constrained planning](https://ompl.kavrakilab.org/constrainedPlanning.html) for more details
+47        """
+48
+49        def f(x, out):
+50            self.planner.robot.set_qpos(x)
+51            out[0] = (
+52                self.get_eef_z().dot(np.array([0, 0, -1])) - 0.966
+53            )  # maintain 15 degrees w.r.t. -z axis
+54
+55        return f
 
    
 
 
@@ -435,18 +486,24 @@
    - 
    47  def make_j(self):
-48    """
-49    create the jacobian of the constraint function w.r.t. qpos
-50    This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
-51    """
-52    def j(x, out):
-53      full_qpos = self.planner.pad_qpos(x)
-54      jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(full_qpos, len(self.planner.move_group_joint_indices)-1)
-55      rot_jac = jac[3:, self.planner.move_group_joint_indices]
-56      for i in range(len(self.planner.move_group_joint_indices)):
-57        out[i] = np.cross(rot_jac[:,i], self.get_eef_z()).dot(np.array([0,0,-1]))
-58    return j
+            
    57    def make_j(self):
+58        """
+59        create the jacobian of the constraint function w.r.t. qpos
+60        This is needed because the planner uses the jacobian to project a random sample to the constraint manifold
+61        """
+62
+63        def j(x, out):
+64            full_qpos = self.planner.pad_qpos(x)
+65            jac = self.planner.robot.get_pinocchio_model().compute_single_link_jacobian(
+66                full_qpos, len(self.planner.move_group_joint_indices) - 1
+67            )
+68            rot_jac = jac[3:, self.planner.move_group_joint_indices]
+69            for i in range(len(self.planner.move_group_joint_indices)):
+70                out[i] = np.cross(rot_jac[:, i], self.get_eef_z()).dot(
+71                    np.array([0, 0, -1])
+72                )
+73
+74        return j
 
    
 
 
@@ -467,58 +524,65 @@
    - 
     60  def demo(self):
- 61    """
- 62    We first plan with constraints to three poses, then plan without constraints to the same poses
- 63    While not always the case, sometimes without constraints, the end effector will tilt almost upside down
- 64    """
- 65    # this starting pose has the end effector tilted roughly 15 degrees
- 66    starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
- 67    self.robot.set_qpos(starting_qpos)
- 68    self.planner.robot.set_qpos(starting_qpos[:7])
- 69    # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
- 70    poses = [[-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
- 71             [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
- 72             [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0]]
- 73    
- 74    # add some point cloud to make the planning more challenging so we can see the effect of no constraint
- 75    self.add_point_cloud()
- 76
- 77    # with constraint
- 78    print("with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis")
- 79    for pose in poses:
- 80      result = self.planner.plan_qpos_to_pose(
- 81        pose,
- 82        self.robot.get_qpos(),
- 83        time_step=1/250,
- 84        use_point_cloud=True,
- 85        use_attach=False,
- 86        planner_name="RRTConnect",
- 87        constraint_function=self.make_f(),
- 88        constraint_jacobian=self.make_j(),
- 89        constraint_tolerance=0.05,
- 90      )
- 91      if result['status'] != "Success":
- 92        print(result['status'])
- 93        return -1
- 94      self.follow_path(result)
- 95
- 96    # without constraint
- 97    print("without constraint. certain movements can sometimes tilt the end effector almost upside down")
- 98    for pose in poses:
- 99      result = self.planner.plan_qpos_to_pose(
-100        pose,
-101        self.robot.get_qpos(),
-102        time_step=1/250,
-103        use_point_cloud=True,
-104        use_attach=False,
-105        planner_name="RRTConnect",
-106        no_simplification=True
-107      )
-108      if result['status'] != "Success":
-109        print(result['status'])
-110        return -1
-111      self.follow_path(result)
+            
     76    def demo(self):
+ 77        """
+ 78        We first plan with constraints to three poses, then plan without constraints to the same poses
+ 79        While not always the case, sometimes without constraints, the end effector will tilt almost upside down
+ 80        """
+ 81        # this starting pose has the end effector tilted roughly 15 degrees
+ 82        starting_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.88, 0.78, 0, 0]
+ 83        self.robot.set_qpos(starting_qpos)
+ 84        self.planner.robot.set_qpos(starting_qpos[:7])
+ 85        # all these poses are constrain compatible (roughly 15 degrees w.r.t. -z axis)
+ 86        poses = [
+ 87            [-0.4, -0.3, 0.28, 0.0704682, -0.5356872, 0.8342834, 0.1097478],
+ 88            [0.6, 0.1, 0.44, 0.0704682, -0.5356872, -0.8342834, -0.1097478],
+ 89            [0, -0.3, 0.5, 0.1304237, -0.9914583, 0, 0],
+ 90        ]
+ 91
+ 92        # add some point cloud to make the planning more challenging so we can see the effect of no constraint
+ 93        self.add_point_cloud()
+ 94
+ 95        # with constraint
+ 96        print(
+ 97            "with constraint. all movements roughly maintain 15 degrees w.r.t. -z axis"
+ 98        )
+ 99        for pose in poses:
+100            result = self.planner.plan_qpos_to_pose(
+101                pose,
+102                self.robot.get_qpos(),
+103                time_step=1 / 250,
+104                use_point_cloud=True,
+105                use_attach=False,
+106                planner_name="RRTConnect",
+107                constraint_function=self.make_f(),
+108                constraint_jacobian=self.make_j(),
+109                constraint_tolerance=0.05,
+110            )
+111            if result["status"] != "Success":
+112                print(result["status"])
+113                return -1
+114            self.follow_path(result)
+115
+116        # without constraint
+117        print(
+118            "without constraint. certain movements can sometimes tilt the end effector"
+119            " almost upside down"
+120        )
+121        for pose in poses:
+122            result = self.planner.plan_qpos_to_pose(
+123                pose,
+124                self.robot.get_qpos(),
+125                time_step=1 / 250,
+126                use_point_cloud=True,
+127                use_attach=False,
+128                planner_name="RRTConnect",
+129                no_simplification=True,
+130            )
+131            if result["status"] != "Success":
+132                print(result["status"])
+133                return -1
+134            self.follow_path(result)
 
    
 
 
diff --git a/docs/mplib/examples/demo.html b/docs/mplib/examples/demo.html
index 12cf2bb0..0f40020e 100644
--- a/docs/mplib/examples/demo.html
+++ b/docs/mplib/examples/demo.html
@@ -76,82 +76,88 @@ 
    
                         
 
                         
     1import sapien.core as sapien
- 2from .demo_setup import DemoSetup
- 3
- 4class PlanningDemo(DemoSetup):
- 5    """
- 6    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
- 7    """
- 8    def __init__(self):
- 9        """
-10        Setting up the scene, the planner, and adding some objects to the scene
-11        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-12        """
-13        super().__init__()
-14        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
-15        self.setup_scene()
-16        self.load_robot()
-17        self.setup_planner()
-18
-19        # Set initial joint positions
-20        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
-21        self.robot.set_qpos(init_qpos)
-22
-23        # table top
-24        builder = self.scene.create_actor_builder()
-25        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-26        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-27        self.table = builder.build_kinematic(name='table')
-28        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-29
-30        # boxes
-31        builder = self.scene.create_actor_builder()
-32        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-33        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-34        self.red_cube = builder.build(name='red_cube')
-35        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
-36
-37        builder = self.scene.create_actor_builder()
-38        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-39        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-40        self.green_cube = builder.build(name='green_cube')
-41        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
-42
-43        builder = self.scene.create_actor_builder()
-44        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-45        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-46        self.blue_cube = builder.build(name='blue_cube')
-47        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
-48
-49    def demo(self):
-50        """
-51        Declare three poses for the robot to move to, each one corresponding to the position of a box
-52        Pick up the box, and set it down 0.1m to the right of its original position
-53        """
-54        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-55                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-56                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-57        for i in range(3):
-58            pose = poses[i]
-59            pose[2] += 0.2
-60            self.move_to_pose(pose)
-61            self.open_gripper()
-62            pose[2] -= 0.12
-63            self.move_to_pose(pose)
-64            self.close_gripper()
-65            pose[2] += 0.12
-66            self.move_to_pose(pose)
-67            pose[0] += 0.1
+ 2
+ 3from .demo_setup import DemoSetup
+ 4
+ 5
+ 6class PlanningDemo(DemoSetup):
+ 7    """
+ 8    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
+ 9    """
+10
+11    def __init__(self):
+12        """
+13        Setting up the scene, the planner, and adding some objects to the scene
+14        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+15        """
+16        super().__init__()
+17        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
+18        self.setup_scene()
+19        self.load_robot()
+20        self.setup_planner()
+21
+22        # Set initial joint positions
+23        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
+24        self.robot.set_qpos(init_qpos)
+25
+26        # table top
+27        builder = self.scene.create_actor_builder()
+28        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+29        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+30        self.table = builder.build_kinematic(name="table")
+31        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+32
+33        # boxes
+34        builder = self.scene.create_actor_builder()
+35        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+36        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+37        self.red_cube = builder.build(name="red_cube")
+38        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
+39
+40        builder = self.scene.create_actor_builder()
+41        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+42        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+43        self.green_cube = builder.build(name="green_cube")
+44        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
+45
+46        builder = self.scene.create_actor_builder()
+47        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+48        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+49        self.blue_cube = builder.build(name="blue_cube")
+50        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
+51
+52    def demo(self):
+53        """
+54        Declare three poses for the robot to move to, each one corresponding to the position of a box
+55        Pick up the box, and set it down 0.1m to the right of its original position
+56        """
+57        poses = [
+58            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+59            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+60            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+61        ]
+62        for i in range(3):
+63            pose = poses[i]
+64            pose[2] += 0.2
+65            self.move_to_pose(pose)
+66            self.open_gripper()
+67            pose[2] -= 0.12
 68            self.move_to_pose(pose)
-69            pose[2] -= 0.12
-70            self.move_to_pose(pose)
-71            self.open_gripper()
-72            pose[2] += 0.12
+69            self.close_gripper()
+70            pose[2] += 0.12
+71            self.move_to_pose(pose)
+72            pose[0] += 0.1
 73            self.move_to_pose(pose)
-74
-75if __name__ == '__main__':
-76    demo = PlanningDemo()
-77    demo.demo()
+74            pose[2] -= 0.12
+75            self.move_to_pose(pose)
+76            self.open_gripper()
+77            pose[2] += 0.12
+78            self.move_to_pose(pose)
+79
+80
+81if __name__ == "__main__":
+82    demo = PlanningDemo()
+83    demo.demo()
 
    
 
 
@@ -167,76 +173,79 @@
    - 
     5class PlanningDemo(DemoSetup):
- 6    """
- 7    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
- 8    """
- 9    def __init__(self):
-10        """
-11        Setting up the scene, the planner, and adding some objects to the scene
-12        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-13        """
-14        super().__init__()
-15        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
-16        self.setup_scene()
-17        self.load_robot()
-18        self.setup_planner()
-19
-20        # Set initial joint positions
-21        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
-22        self.robot.set_qpos(init_qpos)
-23
-24        # table top
-25        builder = self.scene.create_actor_builder()
-26        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-27        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-28        self.table = builder.build_kinematic(name='table')
-29        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-30
-31        # boxes
-32        builder = self.scene.create_actor_builder()
-33        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-34        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-35        self.red_cube = builder.build(name='red_cube')
-36        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
-37
-38        builder = self.scene.create_actor_builder()
-39        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-40        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-41        self.green_cube = builder.build(name='green_cube')
-42        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
-43
-44        builder = self.scene.create_actor_builder()
-45        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-46        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-47        self.blue_cube = builder.build(name='blue_cube')
-48        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
-49
-50    def demo(self):
-51        """
-52        Declare three poses for the robot to move to, each one corresponding to the position of a box
-53        Pick up the box, and set it down 0.1m to the right of its original position
-54        """
-55        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-56                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-57                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-58        for i in range(3):
-59            pose = poses[i]
-60            pose[2] += 0.2
-61            self.move_to_pose(pose)
-62            self.open_gripper()
-63            pose[2] -= 0.12
-64            self.move_to_pose(pose)
-65            self.close_gripper()
-66            pose[2] += 0.12
-67            self.move_to_pose(pose)
-68            pose[0] += 0.1
+            
     7class PlanningDemo(DemoSetup):
+ 8    """
+ 9    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
+10    """
+11
+12    def __init__(self):
+13        """
+14        Setting up the scene, the planner, and adding some objects to the scene
+15        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+16        """
+17        super().__init__()
+18        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
+19        self.setup_scene()
+20        self.load_robot()
+21        self.setup_planner()
+22
+23        # Set initial joint positions
+24        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
+25        self.robot.set_qpos(init_qpos)
+26
+27        # table top
+28        builder = self.scene.create_actor_builder()
+29        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+30        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+31        self.table = builder.build_kinematic(name="table")
+32        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+33
+34        # boxes
+35        builder = self.scene.create_actor_builder()
+36        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+37        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+38        self.red_cube = builder.build(name="red_cube")
+39        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
+40
+41        builder = self.scene.create_actor_builder()
+42        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+43        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+44        self.green_cube = builder.build(name="green_cube")
+45        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
+46
+47        builder = self.scene.create_actor_builder()
+48        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+49        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+50        self.blue_cube = builder.build(name="blue_cube")
+51        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
+52
+53    def demo(self):
+54        """
+55        Declare three poses for the robot to move to, each one corresponding to the position of a box
+56        Pick up the box, and set it down 0.1m to the right of its original position
+57        """
+58        poses = [
+59            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+60            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+61            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+62        ]
+63        for i in range(3):
+64            pose = poses[i]
+65            pose[2] += 0.2
+66            self.move_to_pose(pose)
+67            self.open_gripper()
+68            pose[2] -= 0.12
 69            self.move_to_pose(pose)
-70            pose[2] -= 0.12
-71            self.move_to_pose(pose)
-72            self.open_gripper()
-73            pose[2] += 0.12
+70            self.close_gripper()
+71            pose[2] += 0.12
+72            self.move_to_pose(pose)
+73            pose[0] += 0.1
 74            self.move_to_pose(pose)
+75            pose[2] -= 0.12
+76            self.move_to_pose(pose)
+77            self.open_gripper()
+78            pose[2] += 0.12
+79            self.move_to_pose(pose)
 
    
 
 
@@ -254,46 +263,46 @@
    - 
     9    def __init__(self):
-10        """
-11        Setting up the scene, the planner, and adding some objects to the scene
-12        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-13        """
-14        super().__init__()
-15        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
-16        self.setup_scene()
-17        self.load_robot()
-18        self.setup_planner()
-19
-20        # Set initial joint positions
-21        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
-22        self.robot.set_qpos(init_qpos)
-23
-24        # table top
-25        builder = self.scene.create_actor_builder()
-26        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-27        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-28        self.table = builder.build_kinematic(name='table')
-29        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-30
-31        # boxes
-32        builder = self.scene.create_actor_builder()
-33        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-34        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-35        self.red_cube = builder.build(name='red_cube')
-36        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
-37
-38        builder = self.scene.create_actor_builder()
-39        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-40        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-41        self.green_cube = builder.build(name='green_cube')
-42        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
-43
-44        builder = self.scene.create_actor_builder()
-45        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-46        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-47        self.blue_cube = builder.build(name='blue_cube')
-48        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
+            
    12    def __init__(self):
+13        """
+14        Setting up the scene, the planner, and adding some objects to the scene
+15        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+16        """
+17        super().__init__()
+18        # load the world, the robot, and then setup the planner. See demo_setup.py for more details
+19        self.setup_scene()
+20        self.load_robot()
+21        self.setup_planner()
+22
+23        # Set initial joint positions
+24        init_qpos = [0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79, 0, 0]
+25        self.robot.set_qpos(init_qpos)
+26
+27        # table top
+28        builder = self.scene.create_actor_builder()
+29        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+30        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+31        self.table = builder.build_kinematic(name="table")
+32        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+33
+34        # boxes
+35        builder = self.scene.create_actor_builder()
+36        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+37        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+38        self.red_cube = builder.build(name="red_cube")
+39        self.red_cube.set_pose(sapien.Pose([0.4, 0.3, 0.06]))
+40
+41        builder = self.scene.create_actor_builder()
+42        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+43        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+44        self.green_cube = builder.build(name="green_cube")
+45        self.green_cube.set_pose(sapien.Pose([0.2, -0.3, 0.04]))
+46
+47        builder = self.scene.create_actor_builder()
+48        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+49        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+50        self.blue_cube = builder.build(name="blue_cube")
+51        self.blue_cube.set_pose(sapien.Pose([0.6, 0.1, 0.07]))
 
    
 
 
@@ -358,31 +367,33 @@
    - 
    50    def demo(self):
-51        """
-52        Declare three poses for the robot to move to, each one corresponding to the position of a box
-53        Pick up the box, and set it down 0.1m to the right of its original position
-54        """
-55        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-56                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-57                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-58        for i in range(3):
-59            pose = poses[i]
-60            pose[2] += 0.2
-61            self.move_to_pose(pose)
-62            self.open_gripper()
-63            pose[2] -= 0.12
-64            self.move_to_pose(pose)
-65            self.close_gripper()
-66            pose[2] += 0.12
-67            self.move_to_pose(pose)
-68            pose[0] += 0.1
+            
    53    def demo(self):
+54        """
+55        Declare three poses for the robot to move to, each one corresponding to the position of a box
+56        Pick up the box, and set it down 0.1m to the right of its original position
+57        """
+58        poses = [
+59            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+60            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+61            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+62        ]
+63        for i in range(3):
+64            pose = poses[i]
+65            pose[2] += 0.2
+66            self.move_to_pose(pose)
+67            self.open_gripper()
+68            pose[2] -= 0.12
 69            self.move_to_pose(pose)
-70            pose[2] -= 0.12
-71            self.move_to_pose(pose)
-72            self.open_gripper()
-73            pose[2] += 0.12
+70            self.close_gripper()
+71            pose[2] += 0.12
+72            self.move_to_pose(pose)
+73            pose[0] += 0.1
 74            self.move_to_pose(pose)
+75            pose[2] -= 0.12
+76            self.move_to_pose(pose)
+77            self.open_gripper()
+78            pose[2] += 0.12
+79            self.move_to_pose(pose)
 
    
 
 
diff --git a/docs/mplib/examples/demo_setup.html b/docs/mplib/examples/demo_setup.html
index f7f60044..5d8c7b8b 100644
--- a/docs/mplib/examples/demo_setup.html
+++ b/docs/mplib/examples/demo_setup.html
@@ -94,190 +94,213 @@ 
    
   2
   3import sapien.core as sapien
   4from sapien.utils.viewer import Viewer
-  5import mplib
-  6
-  7class DemoSetup():
-  8  """
-  9  This class is the super class to abstract away some of the setups for the demos
- 10  You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer 
- 11  """
- 12  def __init__(self):
- 13    """ Nothing to do """
- 14    pass
- 15
- 16  def setup_scene(self, **kwargs):
- 17    """ This is the Sapien simulator setup and has nothing to do with mplib """
- 18    # declare sapien sim
- 19    self.engine = sapien.Engine()
- 20    # declare sapien renderer
- 21    self.renderer = sapien.SapienRenderer()
- 22    # give renderer to sapien sim
- 23    self.engine.set_renderer(self.renderer)
- 24
- 25    # declare sapien scene 
- 26    scene_config = sapien.SceneConfig()
- 27    self.scene = self.engine.create_scene(scene_config)
- 28    # set simulation timestep
- 29    self.scene.set_timestep(kwargs.get('timestep', 1 / 240))
- 30    # add ground to scene
- 31    self.scene.add_ground(kwargs.get('ground_height', 0))
- 32    # set default physical material
- 33    self.scene.default_physical_material = self.scene.create_physical_material(
- 34      kwargs.get('static_friction', 1),
- 35      kwargs.get('dynamic_friction', 1),
- 36      kwargs.get('restitution', 0)
- 37    )
- 38    # give some white ambient light of moderate intensity
- 39    self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5]))
- 40    # default enable shadow unless specified otherwise
- 41    shadow = kwargs.get('shadow', True)
- 42    # default spotlight angle and intensity
- 43    direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]])
- 44    for direction_light in direction_lights:
- 45      self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow)
- 46    # default point lights position and intensity
- 47    point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]])
- 48    for point_light in point_lights:
- 49      self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
- 50
- 51    # initialize viewer with camera position and orientation
- 52    self.viewer = Viewer(self.renderer)
- 53    self.viewer.set_scene(self.scene)
- 54    self.viewer.set_camera_xyz(
- 55      x=kwargs.get('camera_xyz_x', 1.2),
- 56      y=kwargs.get('camera_xyz_y', 0.25),
- 57      z=kwargs.get('camera_xyz_z', 0.4)
- 58    )
- 59    self.viewer.set_camera_rpy(
- 60      r=kwargs.get('camera_rpy_r', 0),
- 61      p=kwargs.get('camera_rpy_p', -0.4),
- 62      y=kwargs.get('camera_rpy_y', 2.7)
- 63    )
- 64
- 65  def load_robot(self, **kwargs):
- 66    """
- 67    This function loads a robot from a URDF file into the Sapien Scene created above.
- 68    Note that does mean that setup_scene() must be called before this function.
- 69    """
- 70    loader: sapien.URDFLoader = self.scene.create_urdf_loader()
- 71    loader.fix_root_link = True
- 72    self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf"))
- 73    self.robot.set_root_pose(
- 74      sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0]))
- 75    )
- 76    self.active_joints = self.robot.get_active_joints()
- 77    for joint in self.active_joints:
- 78      joint.set_drive_property(
- 79        stiffness=kwargs.get('joint_stiffness', 1000),
- 80        damping=kwargs.get('joint_damping', 200)
- 81      )
- 82
- 83  def setup_planner(self, **kwargs):
- 84    """
- 85    Create an mplib planner using the default robot
- 86    see planner.py for more details on the arguments
- 87    """
- 88    self.planner = mplib.Planner(
- 89      urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"),
- 90      srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"),
- 91      move_group=kwargs.get('move_group', 'panda_hand')
- 92    )
- 93
- 94  def follow_path(self, result):
- 95    """ Helper function to follow a path generated by the planner """
- 96    # number of waypoints in the path
- 97    n_step = result['position'].shape[0]
- 98    # this makes sure the robot stays neutrally boyant instead of sagging under gravity
- 99    for i in range(n_step):
-100      qf = self.robot.compute_passive_force(
-101        external=False,
-102        gravity=True, 
-103        coriolis_and_centrifugal=True)
-104      self.robot.set_qf(qf)
-105      # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
-106      for j in range(len(self.planner.move_group_joint_indices)):
-107        self.active_joints[j].set_drive_target(result['position'][i][j])
-108        self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j])
-109      # simulation step
-110      self.scene.step()
-111      # render every 4 simulation steps to make it faster
-112      if i % 4 == 0:
-113        self.scene.update_render()
-114        self.viewer.render()
-115
-116  def set_gripper(self, pos):
-117    """
-118    Helper function to activate gripper joints
-119    Args:
-120        pos: position of the gripper joint in real number
-121    """
-122    # The following two lines are particular to the panda robot
-123    for joint in self.active_joints[-2:]:
-124      joint.set_drive_target(pos)
-125    # 100 steps is plenty to reach the target position
-126    for i in range(100): 
-127      qf = self.robot.compute_passive_force(
-128        external=False,
-129        gravity=True, 
-130        coriolis_and_centrifugal=True)
-131      self.robot.set_qf(qf)
-132      self.scene.step()
-133      if i % 4 == 0:
-134        self.scene.update_render()
-135        self.viewer.render()
-136
-137  def open_gripper(self):
-138    self.set_gripper(0.4)
-139
-140  def close_gripper(self):
-141    self.set_gripper(0)
-142
-143  def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False):
-144    """
-145    Plan and follow a path to a pose using RRTConnect
-146
-147    Args:
-148        pose: [x, y, z, qx, qy, qz, qw]
-149        use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
-150        use_attach (optional): if to take the attach into consideration for collision checking.
-151    """
-152    # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
-153    result = self.planner.plan_qpos_to_pose(
-154      pose,
-155      self.robot.get_qpos(),
-156      time_step=1/250,
-157      use_point_cloud=use_point_cloud,
-158      use_attach=use_attach,
-159      planner_name="RRTConnect"
-160    )
-161    if result['status'] != "Success":
-162      print(result['status'])
-163      return -1
-164    # do nothing if the planning fails; follow the path if the planning succeeds
-165    self.follow_path(result)
-166    return 0
-167
-168  def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
-169    """
-170    Interpolative planning with screw motion.
-171    Will not avoid collision and will fail if the path contains collision.
-172    """
-173    result = self.planner.plan_screw(
-174      pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach)
-175    if result['status'] == "Success":
-176      self.follow_path(result)
-177      return 0
-178    else:
-179      # fall back to RRTConnect if the screw motion fails (say contains collision)
-180      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
-181
-182
-183  def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):
-184    """ API to multiplex between the two planning methods """
-185    if with_screw:
-186      return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
-187    else:
-188      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+  5
+  6import mplib
+  7
+  8
+  9class DemoSetup:
+ 10    """
+ 11    This class is the super class to abstract away some of the setups for the demos
+ 12    You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer
+ 13    """
+ 14
+ 15    def __init__(self):
+ 16        """Nothing to do"""
+ 17        pass
+ 18
+ 19    def setup_scene(self, **kwargs):
+ 20        """This is the Sapien simulator setup and has nothing to do with mplib"""
+ 21        # declare sapien sim
+ 22        self.engine = sapien.Engine()
+ 23        # declare sapien renderer
+ 24        self.renderer = sapien.SapienRenderer()
+ 25        # give renderer to sapien sim
+ 26        self.engine.set_renderer(self.renderer)
+ 27
+ 28        # declare sapien scene
+ 29        scene_config = sapien.SceneConfig()
+ 30        self.scene = self.engine.create_scene(scene_config)
+ 31        # set simulation timestep
+ 32        self.scene.set_timestep(kwargs.get("timestep", 1 / 240))
+ 33        # add ground to scene
+ 34        self.scene.add_ground(kwargs.get("ground_height", 0))
+ 35        # set default physical material
+ 36        self.scene.default_physical_material = self.scene.create_physical_material(
+ 37            kwargs.get("static_friction", 1),
+ 38            kwargs.get("dynamic_friction", 1),
+ 39            kwargs.get("restitution", 0),
+ 40        )
+ 41        # give some white ambient light of moderate intensity
+ 42        self.scene.set_ambient_light(kwargs.get("ambient_light", [0.5, 0.5, 0.5]))
+ 43        # default enable shadow unless specified otherwise
+ 44        shadow = kwargs.get("shadow", True)
+ 45        # default spotlight angle and intensity
+ 46        direction_lights = kwargs.get(
+ 47            "direction_lights", [[[0, 1, -1], [0.5, 0.5, 0.5]]]
+ 48        )
+ 49        for direction_light in direction_lights:
+ 50            self.scene.add_directional_light(
+ 51                direction_light[0], direction_light[1], shadow=shadow
+ 52            )
+ 53        # default point lights position and intensity
+ 54        point_lights = kwargs.get(
+ 55            "point_lights",
+ 56            [[[1, 2, 2], [1, 1, 1]], [[1, -2, 2], [1, 1, 1]], [[-1, 0, 1], [1, 1, 1]]],
+ 57        )
+ 58        for point_light in point_lights:
+ 59            self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
+ 60
+ 61        # initialize viewer with camera position and orientation
+ 62        self.viewer = Viewer(self.renderer)
+ 63        self.viewer.set_scene(self.scene)
+ 64        self.viewer.set_camera_xyz(
+ 65            x=kwargs.get("camera_xyz_x", 1.2),
+ 66            y=kwargs.get("camera_xyz_y", 0.25),
+ 67            z=kwargs.get("camera_xyz_z", 0.4),
+ 68        )
+ 69        self.viewer.set_camera_rpy(
+ 70            r=kwargs.get("camera_rpy_r", 0),
+ 71            p=kwargs.get("camera_rpy_p", -0.4),
+ 72            y=kwargs.get("camera_rpy_y", 2.7),
+ 73        )
+ 74
+ 75    def load_robot(self, **kwargs):
+ 76        """
+ 77        This function loads a robot from a URDF file into the Sapien Scene created above.
+ 78        Note that does mean that setup_scene() must be called before this function.
+ 79        """
+ 80        loader: sapien.URDFLoader = self.scene.create_urdf_loader()
+ 81        loader.fix_root_link = True
+ 82        self.robot: sapien.Articulation = loader.load(
+ 83            kwargs.get("urdf_path", "./data/panda/panda.urdf")
+ 84        )
+ 85        self.robot.set_root_pose(
+ 86            sapien.Pose(
+ 87                kwargs.get("robot_origin_xyz", [0, 0, 0]),
+ 88                kwargs.get("robot_origin_quat", [1, 0, 0, 0]),
+ 89            )
+ 90        )
+ 91        self.active_joints = self.robot.get_active_joints()
+ 92        for joint in self.active_joints:
+ 93            joint.set_drive_property(
+ 94                stiffness=kwargs.get("joint_stiffness", 1000),
+ 95                damping=kwargs.get("joint_damping", 200),
+ 96            )
+ 97
+ 98    def setup_planner(self, **kwargs):
+ 99        """
+100        Create an mplib planner using the default robot
+101        see planner.py for more details on the arguments
+102        """
+103        self.planner = mplib.Planner(
+104            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
+105            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
+106            move_group=kwargs.get("move_group", "panda_hand"),
+107        )
+108
+109    def follow_path(self, result):
+110        """Helper function to follow a path generated by the planner"""
+111        # number of waypoints in the path
+112        n_step = result["position"].shape[0]
+113        # this makes sure the robot stays neutrally boyant instead of sagging under gravity
+114        for i in range(n_step):
+115            qf = self.robot.compute_passive_force(
+116                external=False, gravity=True, coriolis_and_centrifugal=True
+117            )
+118            self.robot.set_qf(qf)
+119            # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
+120            for j in range(len(self.planner.move_group_joint_indices)):
+121                self.active_joints[j].set_drive_target(result["position"][i][j])
+122                self.active_joints[j].set_drive_velocity_target(
+123                    result["velocity"][i][j]
+124                )
+125            # simulation step
+126            self.scene.step()
+127            # render every 4 simulation steps to make it faster
+128            if i % 4 == 0:
+129                self.scene.update_render()
+130                self.viewer.render()
+131
+132    def set_gripper(self, pos):
+133        """
+134        Helper function to activate gripper joints
+135        Args:
+136            pos: position of the gripper joint in real number
+137        """
+138        # The following two lines are particular to the panda robot
+139        for joint in self.active_joints[-2:]:
+140            joint.set_drive_target(pos)
+141        # 100 steps is plenty to reach the target position
+142        for i in range(100):
+143            qf = self.robot.compute_passive_force(
+144                external=False, gravity=True, coriolis_and_centrifugal=True
+145            )
+146            self.robot.set_qf(qf)
+147            self.scene.step()
+148            if i % 4 == 0:
+149                self.scene.update_render()
+150                self.viewer.render()
+151
+152    def open_gripper(self):
+153        self.set_gripper(0.4)
+154
+155    def close_gripper(self):
+156        self.set_gripper(0)
+157
+158    def move_to_pose_with_RRTConnect(
+159        self, pose, use_point_cloud=False, use_attach=False
+160    ):
+161        """
+162        Plan and follow a path to a pose using RRTConnect
+163
+164        Args:
+165            pose: [x, y, z, qx, qy, qz, qw]
+166            use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
+167            use_attach (optional): if to take the attach into consideration for collision checking.
+168        """
+169        # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
+170        result = self.planner.plan_qpos_to_pose(
+171            pose,
+172            self.robot.get_qpos(),
+173            time_step=1 / 250,
+174            use_point_cloud=use_point_cloud,
+175            use_attach=use_attach,
+176            planner_name="RRTConnect",
+177        )
+178        if result["status"] != "Success":
+179            print(result["status"])
+180            return -1
+181        # do nothing if the planning fails; follow the path if the planning succeeds
+182        self.follow_path(result)
+183        return 0
+184
+185    def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
+186        """
+187        Interpolative planning with screw motion.
+188        Will not avoid collision and will fail if the path contains collision.
+189        """
+190        result = self.planner.plan_screw(
+191            pose,
+192            self.robot.get_qpos(),
+193            time_step=1 / 250,
+194            use_point_cloud=use_point_cloud,
+195            use_attach=use_attach,
+196        )
+197        if result["status"] == "Success":
+198            self.follow_path(result)
+199            return 0
+200        else:
+201            # fall back to RRTConnect if the screw motion fails (say contains collision)
+202            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+203
+204    def move_to_pose(
+205        self, pose, with_screw=True, use_point_cloud=False, use_attach=False
+206    ):
+207        """API to multiplex between the two planning methods"""
+208        if with_screw:
+209            return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
+210        else:
+211            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
    @@ -293,188 +316,209 @@

    - 
      8class DemoSetup():
-  9  """
- 10  This class is the super class to abstract away some of the setups for the demos
- 11  You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer 
- 12  """
- 13  def __init__(self):
- 14    """ Nothing to do """
- 15    pass
- 16
- 17  def setup_scene(self, **kwargs):
- 18    """ This is the Sapien simulator setup and has nothing to do with mplib """
- 19    # declare sapien sim
- 20    self.engine = sapien.Engine()
- 21    # declare sapien renderer
- 22    self.renderer = sapien.SapienRenderer()
- 23    # give renderer to sapien sim
- 24    self.engine.set_renderer(self.renderer)
- 25
- 26    # declare sapien scene 
- 27    scene_config = sapien.SceneConfig()
- 28    self.scene = self.engine.create_scene(scene_config)
- 29    # set simulation timestep
- 30    self.scene.set_timestep(kwargs.get('timestep', 1 / 240))
- 31    # add ground to scene
- 32    self.scene.add_ground(kwargs.get('ground_height', 0))
- 33    # set default physical material
- 34    self.scene.default_physical_material = self.scene.create_physical_material(
- 35      kwargs.get('static_friction', 1),
- 36      kwargs.get('dynamic_friction', 1),
- 37      kwargs.get('restitution', 0)
- 38    )
- 39    # give some white ambient light of moderate intensity
- 40    self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5]))
- 41    # default enable shadow unless specified otherwise
- 42    shadow = kwargs.get('shadow', True)
- 43    # default spotlight angle and intensity
- 44    direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]])
- 45    for direction_light in direction_lights:
- 46      self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow)
- 47    # default point lights position and intensity
- 48    point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]])
- 49    for point_light in point_lights:
- 50      self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
- 51
- 52    # initialize viewer with camera position and orientation
- 53    self.viewer = Viewer(self.renderer)
- 54    self.viewer.set_scene(self.scene)
- 55    self.viewer.set_camera_xyz(
- 56      x=kwargs.get('camera_xyz_x', 1.2),
- 57      y=kwargs.get('camera_xyz_y', 0.25),
- 58      z=kwargs.get('camera_xyz_z', 0.4)
- 59    )
- 60    self.viewer.set_camera_rpy(
- 61      r=kwargs.get('camera_rpy_r', 0),
- 62      p=kwargs.get('camera_rpy_p', -0.4),
- 63      y=kwargs.get('camera_rpy_y', 2.7)
- 64    )
- 65
- 66  def load_robot(self, **kwargs):
- 67    """
- 68    This function loads a robot from a URDF file into the Sapien Scene created above.
- 69    Note that does mean that setup_scene() must be called before this function.
- 70    """
- 71    loader: sapien.URDFLoader = self.scene.create_urdf_loader()
- 72    loader.fix_root_link = True
- 73    self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf"))
- 74    self.robot.set_root_pose(
- 75      sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0]))
- 76    )
- 77    self.active_joints = self.robot.get_active_joints()
- 78    for joint in self.active_joints:
- 79      joint.set_drive_property(
- 80        stiffness=kwargs.get('joint_stiffness', 1000),
- 81        damping=kwargs.get('joint_damping', 200)
- 82      )
- 83
- 84  def setup_planner(self, **kwargs):
- 85    """
- 86    Create an mplib planner using the default robot
- 87    see planner.py for more details on the arguments
- 88    """
- 89    self.planner = mplib.Planner(
- 90      urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"),
- 91      srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"),
- 92      move_group=kwargs.get('move_group', 'panda_hand')
- 93    )
- 94
- 95  def follow_path(self, result):
- 96    """ Helper function to follow a path generated by the planner """
- 97    # number of waypoints in the path
- 98    n_step = result['position'].shape[0]
- 99    # this makes sure the robot stays neutrally boyant instead of sagging under gravity
-100    for i in range(n_step):
-101      qf = self.robot.compute_passive_force(
-102        external=False,
-103        gravity=True, 
-104        coriolis_and_centrifugal=True)
-105      self.robot.set_qf(qf)
-106      # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
-107      for j in range(len(self.planner.move_group_joint_indices)):
-108        self.active_joints[j].set_drive_target(result['position'][i][j])
-109        self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j])
-110      # simulation step
-111      self.scene.step()
-112      # render every 4 simulation steps to make it faster
-113      if i % 4 == 0:
-114        self.scene.update_render()
-115        self.viewer.render()
-116
-117  def set_gripper(self, pos):
-118    """
-119    Helper function to activate gripper joints
-120    Args:
-121        pos: position of the gripper joint in real number
-122    """
-123    # The following two lines are particular to the panda robot
-124    for joint in self.active_joints[-2:]:
-125      joint.set_drive_target(pos)
-126    # 100 steps is plenty to reach the target position
-127    for i in range(100): 
-128      qf = self.robot.compute_passive_force(
-129        external=False,
-130        gravity=True, 
-131        coriolis_and_centrifugal=True)
-132      self.robot.set_qf(qf)
-133      self.scene.step()
-134      if i % 4 == 0:
-135        self.scene.update_render()
-136        self.viewer.render()
-137
-138  def open_gripper(self):
-139    self.set_gripper(0.4)
-140
-141  def close_gripper(self):
-142    self.set_gripper(0)
-143
-144  def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False):
-145    """
-146    Plan and follow a path to a pose using RRTConnect
-147
-148    Args:
-149        pose: [x, y, z, qx, qy, qz, qw]
-150        use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
-151        use_attach (optional): if to take the attach into consideration for collision checking.
-152    """
-153    # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
-154    result = self.planner.plan_qpos_to_pose(
-155      pose,
-156      self.robot.get_qpos(),
-157      time_step=1/250,
-158      use_point_cloud=use_point_cloud,
-159      use_attach=use_attach,
-160      planner_name="RRTConnect"
-161    )
-162    if result['status'] != "Success":
-163      print(result['status'])
-164      return -1
-165    # do nothing if the planning fails; follow the path if the planning succeeds
-166    self.follow_path(result)
-167    return 0
-168
-169  def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
-170    """
-171    Interpolative planning with screw motion.
-172    Will not avoid collision and will fail if the path contains collision.
-173    """
-174    result = self.planner.plan_screw(
-175      pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach)
-176    if result['status'] == "Success":
-177      self.follow_path(result)
-178      return 0
-179    else:
-180      # fall back to RRTConnect if the screw motion fails (say contains collision)
-181      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
-182
-183
-184  def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):
-185    """ API to multiplex between the two planning methods """
-186    if with_screw:
-187      return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
-188    else:
-189      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+            
     10class DemoSetup:
+ 11    """
+ 12    This class is the super class to abstract away some of the setups for the demos
+ 13    You will need to install Sapien via `pip install sapien` for this to work if you want to use the viewer
+ 14    """
+ 15
+ 16    def __init__(self):
+ 17        """Nothing to do"""
+ 18        pass
+ 19
+ 20    def setup_scene(self, **kwargs):
+ 21        """This is the Sapien simulator setup and has nothing to do with mplib"""
+ 22        # declare sapien sim
+ 23        self.engine = sapien.Engine()
+ 24        # declare sapien renderer
+ 25        self.renderer = sapien.SapienRenderer()
+ 26        # give renderer to sapien sim
+ 27        self.engine.set_renderer(self.renderer)
+ 28
+ 29        # declare sapien scene
+ 30        scene_config = sapien.SceneConfig()
+ 31        self.scene = self.engine.create_scene(scene_config)
+ 32        # set simulation timestep
+ 33        self.scene.set_timestep(kwargs.get("timestep", 1 / 240))
+ 34        # add ground to scene
+ 35        self.scene.add_ground(kwargs.get("ground_height", 0))
+ 36        # set default physical material
+ 37        self.scene.default_physical_material = self.scene.create_physical_material(
+ 38            kwargs.get("static_friction", 1),
+ 39            kwargs.get("dynamic_friction", 1),
+ 40            kwargs.get("restitution", 0),
+ 41        )
+ 42        # give some white ambient light of moderate intensity
+ 43        self.scene.set_ambient_light(kwargs.get("ambient_light", [0.5, 0.5, 0.5]))
+ 44        # default enable shadow unless specified otherwise
+ 45        shadow = kwargs.get("shadow", True)
+ 46        # default spotlight angle and intensity
+ 47        direction_lights = kwargs.get(
+ 48            "direction_lights", [[[0, 1, -1], [0.5, 0.5, 0.5]]]
+ 49        )
+ 50        for direction_light in direction_lights:
+ 51            self.scene.add_directional_light(
+ 52                direction_light[0], direction_light[1], shadow=shadow
+ 53            )
+ 54        # default point lights position and intensity
+ 55        point_lights = kwargs.get(
+ 56            "point_lights",
+ 57            [[[1, 2, 2], [1, 1, 1]], [[1, -2, 2], [1, 1, 1]], [[-1, 0, 1], [1, 1, 1]]],
+ 58        )
+ 59        for point_light in point_lights:
+ 60            self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
+ 61
+ 62        # initialize viewer with camera position and orientation
+ 63        self.viewer = Viewer(self.renderer)
+ 64        self.viewer.set_scene(self.scene)
+ 65        self.viewer.set_camera_xyz(
+ 66            x=kwargs.get("camera_xyz_x", 1.2),
+ 67            y=kwargs.get("camera_xyz_y", 0.25),
+ 68            z=kwargs.get("camera_xyz_z", 0.4),
+ 69        )
+ 70        self.viewer.set_camera_rpy(
+ 71            r=kwargs.get("camera_rpy_r", 0),
+ 72            p=kwargs.get("camera_rpy_p", -0.4),
+ 73            y=kwargs.get("camera_rpy_y", 2.7),
+ 74        )
+ 75
+ 76    def load_robot(self, **kwargs):
+ 77        """
+ 78        This function loads a robot from a URDF file into the Sapien Scene created above.
+ 79        Note that does mean that setup_scene() must be called before this function.
+ 80        """
+ 81        loader: sapien.URDFLoader = self.scene.create_urdf_loader()
+ 82        loader.fix_root_link = True
+ 83        self.robot: sapien.Articulation = loader.load(
+ 84            kwargs.get("urdf_path", "./data/panda/panda.urdf")
+ 85        )
+ 86        self.robot.set_root_pose(
+ 87            sapien.Pose(
+ 88                kwargs.get("robot_origin_xyz", [0, 0, 0]),
+ 89                kwargs.get("robot_origin_quat", [1, 0, 0, 0]),
+ 90            )
+ 91        )
+ 92        self.active_joints = self.robot.get_active_joints()
+ 93        for joint in self.active_joints:
+ 94            joint.set_drive_property(
+ 95                stiffness=kwargs.get("joint_stiffness", 1000),
+ 96                damping=kwargs.get("joint_damping", 200),
+ 97            )
+ 98
+ 99    def setup_planner(self, **kwargs):
+100        """
+101        Create an mplib planner using the default robot
+102        see planner.py for more details on the arguments
+103        """
+104        self.planner = mplib.Planner(
+105            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
+106            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
+107            move_group=kwargs.get("move_group", "panda_hand"),
+108        )
+109
+110    def follow_path(self, result):
+111        """Helper function to follow a path generated by the planner"""
+112        # number of waypoints in the path
+113        n_step = result["position"].shape[0]
+114        # this makes sure the robot stays neutrally boyant instead of sagging under gravity
+115        for i in range(n_step):
+116            qf = self.robot.compute_passive_force(
+117                external=False, gravity=True, coriolis_and_centrifugal=True
+118            )
+119            self.robot.set_qf(qf)
+120            # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
+121            for j in range(len(self.planner.move_group_joint_indices)):
+122                self.active_joints[j].set_drive_target(result["position"][i][j])
+123                self.active_joints[j].set_drive_velocity_target(
+124                    result["velocity"][i][j]
+125                )
+126            # simulation step
+127            self.scene.step()
+128            # render every 4 simulation steps to make it faster
+129            if i % 4 == 0:
+130                self.scene.update_render()
+131                self.viewer.render()
+132
+133    def set_gripper(self, pos):
+134        """
+135        Helper function to activate gripper joints
+136        Args:
+137            pos: position of the gripper joint in real number
+138        """
+139        # The following two lines are particular to the panda robot
+140        for joint in self.active_joints[-2:]:
+141            joint.set_drive_target(pos)
+142        # 100 steps is plenty to reach the target position
+143        for i in range(100):
+144            qf = self.robot.compute_passive_force(
+145                external=False, gravity=True, coriolis_and_centrifugal=True
+146            )
+147            self.robot.set_qf(qf)
+148            self.scene.step()
+149            if i % 4 == 0:
+150                self.scene.update_render()
+151                self.viewer.render()
+152
+153    def open_gripper(self):
+154        self.set_gripper(0.4)
+155
+156    def close_gripper(self):
+157        self.set_gripper(0)
+158
+159    def move_to_pose_with_RRTConnect(
+160        self, pose, use_point_cloud=False, use_attach=False
+161    ):
+162        """
+163        Plan and follow a path to a pose using RRTConnect
+164
+165        Args:
+166            pose: [x, y, z, qx, qy, qz, qw]
+167            use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
+168            use_attach (optional): if to take the attach into consideration for collision checking.
+169        """
+170        # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
+171        result = self.planner.plan_qpos_to_pose(
+172            pose,
+173            self.robot.get_qpos(),
+174            time_step=1 / 250,
+175            use_point_cloud=use_point_cloud,
+176            use_attach=use_attach,
+177            planner_name="RRTConnect",
+178        )
+179        if result["status"] != "Success":
+180            print(result["status"])
+181            return -1
+182        # do nothing if the planning fails; follow the path if the planning succeeds
+183        self.follow_path(result)
+184        return 0
+185
+186    def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
+187        """
+188        Interpolative planning with screw motion.
+189        Will not avoid collision and will fail if the path contains collision.
+190        """
+191        result = self.planner.plan_screw(
+192            pose,
+193            self.robot.get_qpos(),
+194            time_step=1 / 250,
+195            use_point_cloud=use_point_cloud,
+196            use_attach=use_attach,
+197        )
+198        if result["status"] == "Success":
+199            self.follow_path(result)
+200            return 0
+201        else:
+202            # fall back to RRTConnect if the screw motion fails (say contains collision)
+203            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+204
+205    def move_to_pose(
+206        self, pose, with_screw=True, use_point_cloud=False, use_attach=False
+207    ):
+208        """API to multiplex between the two planning methods"""
+209        if with_screw:
+210            return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
+211        else:
+212            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
 
    
 
 
@@ -493,9 +537,9 @@
    - 
    13  def __init__(self):
-14    """ Nothing to do """
-15    pass
+            
    16    def __init__(self):
+17        """Nothing to do"""
+18        pass
 
    
 
 
@@ -515,54 +559,61 @@
    - 
    17  def setup_scene(self, **kwargs):
-18    """ This is the Sapien simulator setup and has nothing to do with mplib """
-19    # declare sapien sim
-20    self.engine = sapien.Engine()
-21    # declare sapien renderer
-22    self.renderer = sapien.SapienRenderer()
-23    # give renderer to sapien sim
-24    self.engine.set_renderer(self.renderer)
-25
-26    # declare sapien scene 
-27    scene_config = sapien.SceneConfig()
-28    self.scene = self.engine.create_scene(scene_config)
-29    # set simulation timestep
-30    self.scene.set_timestep(kwargs.get('timestep', 1 / 240))
-31    # add ground to scene
-32    self.scene.add_ground(kwargs.get('ground_height', 0))
-33    # set default physical material
-34    self.scene.default_physical_material = self.scene.create_physical_material(
-35      kwargs.get('static_friction', 1),
-36      kwargs.get('dynamic_friction', 1),
-37      kwargs.get('restitution', 0)
-38    )
-39    # give some white ambient light of moderate intensity
-40    self.scene.set_ambient_light(kwargs.get('ambient_light', [0.5, 0.5, 0.5]))
-41    # default enable shadow unless specified otherwise
-42    shadow = kwargs.get('shadow', True)
-43    # default spotlight angle and intensity
-44    direction_lights = kwargs.get('direction_lights', [[[0, 1, -1], [0.5, 0.5, 0.5]]])
-45    for direction_light in direction_lights:
-46      self.scene.add_directional_light(direction_light[0], direction_light[1], shadow=shadow)
-47    # default point lights position and intensity
-48    point_lights = kwargs.get('point_lights', [[[1,2,2], [1,1,1]], [[1,-2,2],[1,1,1]], [[-1,0,1],[1,1,1]]])
-49    for point_light in point_lights:
-50      self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
-51
-52    # initialize viewer with camera position and orientation
-53    self.viewer = Viewer(self.renderer)
-54    self.viewer.set_scene(self.scene)
-55    self.viewer.set_camera_xyz(
-56      x=kwargs.get('camera_xyz_x', 1.2),
-57      y=kwargs.get('camera_xyz_y', 0.25),
-58      z=kwargs.get('camera_xyz_z', 0.4)
-59    )
-60    self.viewer.set_camera_rpy(
-61      r=kwargs.get('camera_rpy_r', 0),
-62      p=kwargs.get('camera_rpy_p', -0.4),
-63      y=kwargs.get('camera_rpy_y', 2.7)
-64    )
+            
    20    def setup_scene(self, **kwargs):
+21        """This is the Sapien simulator setup and has nothing to do with mplib"""
+22        # declare sapien sim
+23        self.engine = sapien.Engine()
+24        # declare sapien renderer
+25        self.renderer = sapien.SapienRenderer()
+26        # give renderer to sapien sim
+27        self.engine.set_renderer(self.renderer)
+28
+29        # declare sapien scene
+30        scene_config = sapien.SceneConfig()
+31        self.scene = self.engine.create_scene(scene_config)
+32        # set simulation timestep
+33        self.scene.set_timestep(kwargs.get("timestep", 1 / 240))
+34        # add ground to scene
+35        self.scene.add_ground(kwargs.get("ground_height", 0))
+36        # set default physical material
+37        self.scene.default_physical_material = self.scene.create_physical_material(
+38            kwargs.get("static_friction", 1),
+39            kwargs.get("dynamic_friction", 1),
+40            kwargs.get("restitution", 0),
+41        )
+42        # give some white ambient light of moderate intensity
+43        self.scene.set_ambient_light(kwargs.get("ambient_light", [0.5, 0.5, 0.5]))
+44        # default enable shadow unless specified otherwise
+45        shadow = kwargs.get("shadow", True)
+46        # default spotlight angle and intensity
+47        direction_lights = kwargs.get(
+48            "direction_lights", [[[0, 1, -1], [0.5, 0.5, 0.5]]]
+49        )
+50        for direction_light in direction_lights:
+51            self.scene.add_directional_light(
+52                direction_light[0], direction_light[1], shadow=shadow
+53            )
+54        # default point lights position and intensity
+55        point_lights = kwargs.get(
+56            "point_lights",
+57            [[[1, 2, 2], [1, 1, 1]], [[1, -2, 2], [1, 1, 1]], [[-1, 0, 1], [1, 1, 1]]],
+58        )
+59        for point_light in point_lights:
+60            self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)
+61
+62        # initialize viewer with camera position and orientation
+63        self.viewer = Viewer(self.renderer)
+64        self.viewer.set_scene(self.scene)
+65        self.viewer.set_camera_xyz(
+66            x=kwargs.get("camera_xyz_x", 1.2),
+67            y=kwargs.get("camera_xyz_y", 0.25),
+68            z=kwargs.get("camera_xyz_z", 0.4),
+69        )
+70        self.viewer.set_camera_rpy(
+71            r=kwargs.get("camera_rpy_r", 0),
+72            p=kwargs.get("camera_rpy_p", -0.4),
+73            y=kwargs.get("camera_rpy_y", 2.7),
+74        )
 
    
 
 
@@ -582,23 +633,28 @@
    - 
    66  def load_robot(self, **kwargs):
-67    """
-68    This function loads a robot from a URDF file into the Sapien Scene created above.
-69    Note that does mean that setup_scene() must be called before this function.
-70    """
-71    loader: sapien.URDFLoader = self.scene.create_urdf_loader()
-72    loader.fix_root_link = True
-73    self.robot: sapien.Articulation = loader.load(kwargs.get('urdf_path', "./data/panda/panda.urdf"))
-74    self.robot.set_root_pose(
-75      sapien.Pose(kwargs.get("robot_origin_xyz", [0,0,0]), kwargs.get("robot_origin_quat", [1,0,0,0]))
-76    )
-77    self.active_joints = self.robot.get_active_joints()
-78    for joint in self.active_joints:
-79      joint.set_drive_property(
-80        stiffness=kwargs.get('joint_stiffness', 1000),
-81        damping=kwargs.get('joint_damping', 200)
-82      )
+            
    76    def load_robot(self, **kwargs):
+77        """
+78        This function loads a robot from a URDF file into the Sapien Scene created above.
+79        Note that does mean that setup_scene() must be called before this function.
+80        """
+81        loader: sapien.URDFLoader = self.scene.create_urdf_loader()
+82        loader.fix_root_link = True
+83        self.robot: sapien.Articulation = loader.load(
+84            kwargs.get("urdf_path", "./data/panda/panda.urdf")
+85        )
+86        self.robot.set_root_pose(
+87            sapien.Pose(
+88                kwargs.get("robot_origin_xyz", [0, 0, 0]),
+89                kwargs.get("robot_origin_quat", [1, 0, 0, 0]),
+90            )
+91        )
+92        self.active_joints = self.robot.get_active_joints()
+93        for joint in self.active_joints:
+94            joint.set_drive_property(
+95                stiffness=kwargs.get("joint_stiffness", 1000),
+96                damping=kwargs.get("joint_damping", 200),
+97            )
 
    
 
 
@@ -619,16 +675,16 @@
    - 
    84  def setup_planner(self, **kwargs):
-85    """
-86    Create an mplib planner using the default robot
-87    see planner.py for more details on the arguments
-88    """
-89    self.planner = mplib.Planner(
-90      urdf=kwargs.get('urdf_path', "./data/panda/panda.urdf"),
-91      srdf=kwargs.get('srdf_path', "./data/panda/panda.srdf"),
-92      move_group=kwargs.get('move_group', 'panda_hand')
-93    )
+            
     99    def setup_planner(self, **kwargs):
+100        """
+101        Create an mplib planner using the default robot
+102        see planner.py for more details on the arguments
+103        """
+104        self.planner = mplib.Planner(
+105            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
+106            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
+107            move_group=kwargs.get("move_group", "panda_hand"),
+108        )
 
    
 
 
@@ -649,27 +705,28 @@
    - 
     95  def follow_path(self, result):
- 96    """ Helper function to follow a path generated by the planner """
- 97    # number of waypoints in the path
- 98    n_step = result['position'].shape[0]
- 99    # this makes sure the robot stays neutrally boyant instead of sagging under gravity
-100    for i in range(n_step):
-101      qf = self.robot.compute_passive_force(
-102        external=False,
-103        gravity=True, 
-104        coriolis_and_centrifugal=True)
-105      self.robot.set_qf(qf)
-106      # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
-107      for j in range(len(self.planner.move_group_joint_indices)):
-108        self.active_joints[j].set_drive_target(result['position'][i][j])
-109        self.active_joints[j].set_drive_velocity_target(result['velocity'][i][j])
-110      # simulation step
-111      self.scene.step()
-112      # render every 4 simulation steps to make it faster
-113      if i % 4 == 0:
-114        self.scene.update_render()
-115        self.viewer.render()
+            
    110    def follow_path(self, result):
+111        """Helper function to follow a path generated by the planner"""
+112        # number of waypoints in the path
+113        n_step = result["position"].shape[0]
+114        # this makes sure the robot stays neutrally boyant instead of sagging under gravity
+115        for i in range(n_step):
+116            qf = self.robot.compute_passive_force(
+117                external=False, gravity=True, coriolis_and_centrifugal=True
+118            )
+119            self.robot.set_qf(qf)
+120            # set the joint positions and velocities for move group joints only. The others are not the responsibility of the planner
+121            for j in range(len(self.planner.move_group_joint_indices)):
+122                self.active_joints[j].set_drive_target(result["position"][i][j])
+123                self.active_joints[j].set_drive_velocity_target(
+124                    result["velocity"][i][j]
+125                )
+126            # simulation step
+127            self.scene.step()
+128            # render every 4 simulation steps to make it faster
+129            if i % 4 == 0:
+130                self.scene.update_render()
+131                self.viewer.render()
 
    
 
 
@@ -689,26 +746,25 @@
    - 
    117  def set_gripper(self, pos):
-118    """
-119    Helper function to activate gripper joints
-120    Args:
-121        pos: position of the gripper joint in real number
-122    """
-123    # The following two lines are particular to the panda robot
-124    for joint in self.active_joints[-2:]:
-125      joint.set_drive_target(pos)
-126    # 100 steps is plenty to reach the target position
-127    for i in range(100): 
-128      qf = self.robot.compute_passive_force(
-129        external=False,
-130        gravity=True, 
-131        coriolis_and_centrifugal=True)
-132      self.robot.set_qf(qf)
-133      self.scene.step()
-134      if i % 4 == 0:
-135        self.scene.update_render()
-136        self.viewer.render()
+            
    133    def set_gripper(self, pos):
+134        """
+135        Helper function to activate gripper joints
+136        Args:
+137            pos: position of the gripper joint in real number
+138        """
+139        # The following two lines are particular to the panda robot
+140        for joint in self.active_joints[-2:]:
+141            joint.set_drive_target(pos)
+142        # 100 steps is plenty to reach the target position
+143        for i in range(100):
+144            qf = self.robot.compute_passive_force(
+145                external=False, gravity=True, coriolis_and_centrifugal=True
+146            )
+147            self.robot.set_qf(qf)
+148            self.scene.step()
+149            if i % 4 == 0:
+150                self.scene.update_render()
+151                self.viewer.render()
 
    
 
 
@@ -730,8 +786,8 @@
    - 
    138  def open_gripper(self):
-139    self.set_gripper(0.4)
+            
    153    def open_gripper(self):
+154        self.set_gripper(0.4)
 
    
 
 
@@ -749,8 +805,8 @@
    - 
    141  def close_gripper(self):
-142    self.set_gripper(0)
+            
    156    def close_gripper(self):
+157        self.set_gripper(0)
 
    
 
 
@@ -768,30 +824,32 @@
    - 
    144  def move_to_pose_with_RRTConnect(self, pose, use_point_cloud=False, use_attach=False):
-145    """
-146    Plan and follow a path to a pose using RRTConnect
-147
-148    Args:
-149        pose: [x, y, z, qx, qy, qz, qw]
-150        use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
-151        use_attach (optional): if to take the attach into consideration for collision checking.
-152    """
-153    # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
-154    result = self.planner.plan_qpos_to_pose(
-155      pose,
-156      self.robot.get_qpos(),
-157      time_step=1/250,
-158      use_point_cloud=use_point_cloud,
-159      use_attach=use_attach,
-160      planner_name="RRTConnect"
-161    )
-162    if result['status'] != "Success":
-163      print(result['status'])
-164      return -1
-165    # do nothing if the planning fails; follow the path if the planning succeeds
-166    self.follow_path(result)
-167    return 0
+            
    159    def move_to_pose_with_RRTConnect(
+160        self, pose, use_point_cloud=False, use_attach=False
+161    ):
+162        """
+163        Plan and follow a path to a pose using RRTConnect
+164
+165        Args:
+166            pose: [x, y, z, qx, qy, qz, qw]
+167            use_point_cloud (optional): if to take the point cloud into consideration for collision checking.
+168            use_attach (optional): if to take the attach into consideration for collision checking.
+169        """
+170        # result is a dictionary with keys 'status', 'time', 'position', 'velocity', 'acceleration', 'duration'
+171        result = self.planner.plan_qpos_to_pose(
+172            pose,
+173            self.robot.get_qpos(),
+174            time_step=1 / 250,
+175            use_point_cloud=use_point_cloud,
+176            use_attach=use_attach,
+177            planner_name="RRTConnect",
+178        )
+179        if result["status"] != "Success":
+180            print(result["status"])
+181            return -1
+182        # do nothing if the planning fails; follow the path if the planning succeeds
+183        self.follow_path(result)
+184        return 0
 
    
 
 
@@ -816,19 +874,24 @@
    - 
    169  def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
-170    """
-171    Interpolative planning with screw motion.
-172    Will not avoid collision and will fail if the path contains collision.
-173    """
-174    result = self.planner.plan_screw(
-175      pose, self.robot.get_qpos(), time_step=1/250, use_point_cloud=use_point_cloud, use_attach=use_attach)
-176    if result['status'] == "Success":
-177      self.follow_path(result)
-178      return 0
-179    else:
-180      # fall back to RRTConnect if the screw motion fails (say contains collision)
-181      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+            
    186    def move_to_pose_with_screw(self, pose, use_point_cloud=False, use_attach=False):
+187        """
+188        Interpolative planning with screw motion.
+189        Will not avoid collision and will fail if the path contains collision.
+190        """
+191        result = self.planner.plan_screw(
+192            pose,
+193            self.robot.get_qpos(),
+194            time_step=1 / 250,
+195            use_point_cloud=use_point_cloud,
+196            use_attach=use_attach,
+197        )
+198        if result["status"] == "Success":
+199            self.follow_path(result)
+200            return 0
+201        else:
+202            # fall back to RRTConnect if the screw motion fails (say contains collision)
+203            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
 
    
 
 
@@ -849,12 +912,14 @@
    - 
    184  def move_to_pose(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):
-185    """ API to multiplex between the two planning methods """
-186    if with_screw:
-187      return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
-188    else:
-189      return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
+            
    205    def move_to_pose(
+206        self, pose, with_screw=True, use_point_cloud=False, use_attach=False
+207    ):
+208        """API to multiplex between the two planning methods"""
+209        if with_screw:
+210            return self.move_to_pose_with_screw(pose, use_point_cloud, use_attach)
+211        else:
+212            return self.move_to_pose_with_RRTConnect(pose, use_point_cloud, use_attach)
 
    
 
 
diff --git a/docs/mplib/examples/detect_collision.html b/docs/mplib/examples/detect_collision.html
index f72bc108..fa016e7b 100644
--- a/docs/mplib/examples/detect_collision.html
+++ b/docs/mplib/examples/detect_collision.html
@@ -66,74 +66,108 @@ 
    
 
                         
 
-                        
     1#!/usr/bin/env python3
- 2
- 3import mplib
- 4from .demo_setup import DemoSetup
- 5
- 6class DetectCollisionDemo(DemoSetup):
- 7  """
- 8  This demonstrates some of the collision detection functions in the planner.
- 9  """
-10  def __init__(self):
-11    """ Only the planner is needed this time. No simulation env required """
-12    super().__init__()
-13    self.setup_planner()
-14
-15  def print_collisions(self, collisions):
-16    """ Helper function to abstract away the printing of collisions """
-17    if len(collisions) == 0:
-18      print("No collision")
-19      return
-20    for collision in collisions:
-21      print(f"{collision.link_name1} of entity {collision.object_name1} collides with "
-22            f"{collision.link_name2} of entity {collision.object_name2}")
-23
-24  def demo(self):
-25    """
-26    We test several configurations:
-27    1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
-28    2. Set robot to a self-collision qpos and check for self-collision returns a collision
-29    3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
-30    4. Set robot to a env-collision qpos and check for env-collision returns a collision
-31    5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
-32    6. Remove the floor and check for env-collision returns no collision
-33    """
-34    floor = mplib.planner.fcl.Box([2,2,0.1])  # create a 2 x 2 x 0.1m box
-35    # create a collision object for the floor, with a 10cm offset in the z direction
-36    floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0])
-37    # update the planning world with the floor collision object
-38    self.planner.set_normal_object("floor", floor_fcl_collision_object)
-39
-40    print("\n----- self-collision-free qpos -----")
-41    # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
-42    self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
-43    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos))
-44    
-45    print("\n----- self-collision qpos -----")
-46    self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
-47    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos))
-48
-49    print("\n----- env-collision-free qpos -----")
-50    env_collision_free_qpos = self_collision_free_qpos
-51    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos))
-52
-53    print("\n----- env-collision qpos -----")
-54    env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0]  # this qpos causes several joints to dip below the floor
-55    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
-56    
-57    print("\n----- env-collision causing planner to timeout -----")
-58    status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos])
-59    print(status, path)
-60
-61    print("\n----- no more env-collision after removing the floor -----")
-62    self.planner.remove_normal_object("floor")
-63    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
-64
-65if __name__ == '__main__':
-66  """ Driver code """
-67  demo = DetectCollisionDemo()
-68  demo.demo()
+                        
      1#!/usr/bin/env python3
+  2
+  3import mplib
+  4
+  5from .demo_setup import DemoSetup
+  6
+  7
+  8class DetectCollisionDemo(DemoSetup):
+  9    """
+ 10    This demonstrates some of the collision detection functions in the planner.
+ 11    """
+ 12
+ 13    def __init__(self):
+ 14        """Only the planner is needed this time. No simulation env required"""
+ 15        super().__init__()
+ 16        self.setup_planner()
+ 17
+ 18    def print_collisions(self, collisions):
+ 19        """Helper function to abstract away the printing of collisions"""
+ 20        if len(collisions) == 0:
+ 21            print("No collision")
+ 22            return
+ 23        for collision in collisions:
+ 24            print(
+ 25                f"{collision.link_name1} of entity {collision.object_name1} collides"
+ 26                f" with {collision.link_name2} of entity {collision.object_name2}"
+ 27            )
+ 28
+ 29    def demo(self):
+ 30        """
+ 31        We test several configurations:
+ 32        1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
+ 33        2. Set robot to a self-collision qpos and check for self-collision returns a collision
+ 34        3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
+ 35        4. Set robot to a env-collision qpos and check for env-collision returns a collision
+ 36        5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
+ 37        6. Remove the floor and check for env-collision returns no collision
+ 38        """
+ 39        floor = mplib.planner.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
+ 40        # create a collision object for the floor, with a 10cm offset in the z direction
+ 41        floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(
+ 42            floor, [0, 0, -0.1], [1, 0, 0, 0]
+ 43        )
+ 44        # update the planning world with the floor collision object
+ 45        self.planner.set_normal_object("floor", floor_fcl_collision_object)
+ 46
+ 47        print("\n----- self-collision-free qpos -----")
+ 48        # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
+ 49        self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
+ 50        self.print_collisions(
+ 51            self.planner.check_for_self_collision(
+ 52                self.planner.robot, self_collision_free_qpos
+ 53            )
+ 54        )
+ 55
+ 56        print("\n----- self-collision qpos -----")
+ 57        self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
+ 58        self.print_collisions(
+ 59            self.planner.check_for_self_collision(
+ 60                self.planner.robot, self_collision_qpos
+ 61            )
+ 62        )
+ 63
+ 64        print("\n----- env-collision-free qpos -----")
+ 65        env_collision_free_qpos = self_collision_free_qpos
+ 66        self.print_collisions(
+ 67            self.planner.check_for_env_collision(
+ 68                self.planner.robot, env_collision_free_qpos
+ 69            )
+ 70        )
+ 71
+ 72        print("\n----- env-collision qpos -----")
+ 73        env_collision_qpos = [
+ 74            0,
+ 75            1.5,
+ 76            0,
+ 77            -1.5,
+ 78            0,
+ 79            0,
+ 80            0,
+ 81        ]  # this qpos causes several joints to dip below the floor
+ 82        self.print_collisions(
+ 83            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+ 84        )
+ 85
+ 86        print("\n----- env-collision causing planner to timeout -----")
+ 87        status, path = self.planner.planner.plan(
+ 88            start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]
+ 89        )
+ 90        print(status, path)
+ 91
+ 92        print("\n----- no more env-collision after removing the floor -----")
+ 93        self.planner.remove_normal_object("floor")
+ 94        self.print_collisions(
+ 95            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+ 96        )
+ 97
+ 98
+ 99if __name__ == "__main__":
+100    """Driver code"""
+101    demo = DetectCollisionDemo()
+102    demo.demo()
 
    
 
 
@@ -149,64 +183,95 @@ 
    
 
     
    
     
-            
     7class DetectCollisionDemo(DemoSetup):
- 8  """
- 9  This demonstrates some of the collision detection functions in the planner.
-10  """
-11  def __init__(self):
-12    """ Only the planner is needed this time. No simulation env required """
-13    super().__init__()
-14    self.setup_planner()
-15
-16  def print_collisions(self, collisions):
-17    """ Helper function to abstract away the printing of collisions """
-18    if len(collisions) == 0:
-19      print("No collision")
-20      return
-21    for collision in collisions:
-22      print(f"{collision.link_name1} of entity {collision.object_name1} collides with "
-23            f"{collision.link_name2} of entity {collision.object_name2}")
-24
-25  def demo(self):
-26    """
-27    We test several configurations:
-28    1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
-29    2. Set robot to a self-collision qpos and check for self-collision returns a collision
-30    3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
-31    4. Set robot to a env-collision qpos and check for env-collision returns a collision
-32    5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
-33    6. Remove the floor and check for env-collision returns no collision
-34    """
-35    floor = mplib.planner.fcl.Box([2,2,0.1])  # create a 2 x 2 x 0.1m box
-36    # create a collision object for the floor, with a 10cm offset in the z direction
-37    floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0])
-38    # update the planning world with the floor collision object
-39    self.planner.set_normal_object("floor", floor_fcl_collision_object)
-40
-41    print("\n----- self-collision-free qpos -----")
-42    # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
-43    self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
-44    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos))
-45    
-46    print("\n----- self-collision qpos -----")
-47    self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
-48    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos))
-49
-50    print("\n----- env-collision-free qpos -----")
-51    env_collision_free_qpos = self_collision_free_qpos
-52    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos))
-53
-54    print("\n----- env-collision qpos -----")
-55    env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0]  # this qpos causes several joints to dip below the floor
-56    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
-57    
-58    print("\n----- env-collision causing planner to timeout -----")
-59    status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos])
-60    print(status, path)
-61
-62    print("\n----- no more env-collision after removing the floor -----")
-63    self.planner.remove_normal_object("floor")
-64    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
+            
     9class DetectCollisionDemo(DemoSetup):
+10    """
+11    This demonstrates some of the collision detection functions in the planner.
+12    """
+13
+14    def __init__(self):
+15        """Only the planner is needed this time. No simulation env required"""
+16        super().__init__()
+17        self.setup_planner()
+18
+19    def print_collisions(self, collisions):
+20        """Helper function to abstract away the printing of collisions"""
+21        if len(collisions) == 0:
+22            print("No collision")
+23            return
+24        for collision in collisions:
+25            print(
+26                f"{collision.link_name1} of entity {collision.object_name1} collides"
+27                f" with {collision.link_name2} of entity {collision.object_name2}"
+28            )
+29
+30    def demo(self):
+31        """
+32        We test several configurations:
+33        1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
+34        2. Set robot to a self-collision qpos and check for self-collision returns a collision
+35        3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
+36        4. Set robot to a env-collision qpos and check for env-collision returns a collision
+37        5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
+38        6. Remove the floor and check for env-collision returns no collision
+39        """
+40        floor = mplib.planner.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
+41        # create a collision object for the floor, with a 10cm offset in the z direction
+42        floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(
+43            floor, [0, 0, -0.1], [1, 0, 0, 0]
+44        )
+45        # update the planning world with the floor collision object
+46        self.planner.set_normal_object("floor", floor_fcl_collision_object)
+47
+48        print("\n----- self-collision-free qpos -----")
+49        # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
+50        self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
+51        self.print_collisions(
+52            self.planner.check_for_self_collision(
+53                self.planner.robot, self_collision_free_qpos
+54            )
+55        )
+56
+57        print("\n----- self-collision qpos -----")
+58        self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
+59        self.print_collisions(
+60            self.planner.check_for_self_collision(
+61                self.planner.robot, self_collision_qpos
+62            )
+63        )
+64
+65        print("\n----- env-collision-free qpos -----")
+66        env_collision_free_qpos = self_collision_free_qpos
+67        self.print_collisions(
+68            self.planner.check_for_env_collision(
+69                self.planner.robot, env_collision_free_qpos
+70            )
+71        )
+72
+73        print("\n----- env-collision qpos -----")
+74        env_collision_qpos = [
+75            0,
+76            1.5,
+77            0,
+78            -1.5,
+79            0,
+80            0,
+81            0,
+82        ]  # this qpos causes several joints to dip below the floor
+83        self.print_collisions(
+84            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+85        )
+86
+87        print("\n----- env-collision causing planner to timeout -----")
+88        status, path = self.planner.planner.plan(
+89            start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]
+90        )
+91        print(status, path)
+92
+93        print("\n----- no more env-collision after removing the floor -----")
+94        self.planner.remove_normal_object("floor")
+95        self.print_collisions(
+96            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+97        )
 
    
 
 
@@ -224,10 +289,10 @@ 
    
 
     
    
     
-            
    11  def __init__(self):
-12    """ Only the planner is needed this time. No simulation env required """
-13    super().__init__()
-14    self.setup_planner()
+            
    14    def __init__(self):
+15        """Only the planner is needed this time. No simulation env required"""
+16        super().__init__()
+17        self.setup_planner()
 
    
 
 
@@ -247,14 +312,16 @@ 
    
 
     
    
     
-            
    16  def print_collisions(self, collisions):
-17    """ Helper function to abstract away the printing of collisions """
-18    if len(collisions) == 0:
-19      print("No collision")
-20      return
-21    for collision in collisions:
-22      print(f"{collision.link_name1} of entity {collision.object_name1} collides with "
-23            f"{collision.link_name2} of entity {collision.object_name2}")
+            
    19    def print_collisions(self, collisions):
+20        """Helper function to abstract away the printing of collisions"""
+21        if len(collisions) == 0:
+22            print("No collision")
+23            return
+24        for collision in collisions:
+25            print(
+26                f"{collision.link_name1} of entity {collision.object_name1} collides"
+27                f" with {collision.link_name2} of entity {collision.object_name2}"
+28            )
 
    
 
 
@@ -274,46 +341,74 @@ 
    
 
     
    
     
-            
    25  def demo(self):
-26    """
-27    We test several configurations:
-28    1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
-29    2. Set robot to a self-collision qpos and check for self-collision returns a collision
-30    3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
-31    4. Set robot to a env-collision qpos and check for env-collision returns a collision
-32    5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
-33    6. Remove the floor and check for env-collision returns no collision
-34    """
-35    floor = mplib.planner.fcl.Box([2,2,0.1])  # create a 2 x 2 x 0.1m box
-36    # create a collision object for the floor, with a 10cm offset in the z direction
-37    floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(floor, [0,0,-0.1], [1,0,0,0])
-38    # update the planning world with the floor collision object
-39    self.planner.set_normal_object("floor", floor_fcl_collision_object)
-40
-41    print("\n----- self-collision-free qpos -----")
-42    # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
-43    self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
-44    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_free_qpos))
-45    
-46    print("\n----- self-collision qpos -----")
-47    self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
-48    self.print_collisions(self.planner.check_for_self_collision(self.planner.robot, self_collision_qpos))
-49
-50    print("\n----- env-collision-free qpos -----")
-51    env_collision_free_qpos = self_collision_free_qpos
-52    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_free_qpos))
-53
-54    print("\n----- env-collision qpos -----")
-55    env_collision_qpos = [0, 1.5, 0, -1.5, 0, 0, 0]  # this qpos causes several joints to dip below the floor
-56    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
-57    
-58    print("\n----- env-collision causing planner to timeout -----")
-59    status, path = self.planner.planner.plan(start_state=self_collision_free_qpos, goal_states=[env_collision_qpos])
-60    print(status, path)
-61
-62    print("\n----- no more env-collision after removing the floor -----")
-63    self.planner.remove_normal_object("floor")
-64    self.print_collisions(self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos))
+            
    30    def demo(self):
+31        """
+32        We test several configurations:
+33        1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
+34        2. Set robot to a self-collision qpos and check for self-collision returns a collision
+35        3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
+36        4. Set robot to a env-collision qpos and check for env-collision returns a collision
+37        5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
+38        6. Remove the floor and check for env-collision returns no collision
+39        """
+40        floor = mplib.planner.fcl.Box([2, 2, 0.1])  # create a 2 x 2 x 0.1m box
+41        # create a collision object for the floor, with a 10cm offset in the z direction
+42        floor_fcl_collision_object = mplib.planner.fcl.CollisionObject(
+43            floor, [0, 0, -0.1], [1, 0, 0, 0]
+44        )
+45        # update the planning world with the floor collision object
+46        self.planner.set_normal_object("floor", floor_fcl_collision_object)
+47
+48        print("\n----- self-collision-free qpos -----")
+49        # if the joint qpos does not include the gripper joints, it will be set to the current gripper joint angle
+50        self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
+51        self.print_collisions(
+52            self.planner.check_for_self_collision(
+53                self.planner.robot, self_collision_free_qpos
+54            )
+55        )
+56
+57        print("\n----- self-collision qpos -----")
+58        self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
+59        self.print_collisions(
+60            self.planner.check_for_self_collision(
+61                self.planner.robot, self_collision_qpos
+62            )
+63        )
+64
+65        print("\n----- env-collision-free qpos -----")
+66        env_collision_free_qpos = self_collision_free_qpos
+67        self.print_collisions(
+68            self.planner.check_for_env_collision(
+69                self.planner.robot, env_collision_free_qpos
+70            )
+71        )
+72
+73        print("\n----- env-collision qpos -----")
+74        env_collision_qpos = [
+75            0,
+76            1.5,
+77            0,
+78            -1.5,
+79            0,
+80            0,
+81            0,
+82        ]  # this qpos causes several joints to dip below the floor
+83        self.print_collisions(
+84            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+85        )
+86
+87        print("\n----- env-collision causing planner to timeout -----")
+88        status, path = self.planner.planner.plan(
+89            start_state=self_collision_free_qpos, goal_states=[env_collision_qpos]
+90        )
+91        print(status, path)
+92
+93        print("\n----- no more env-collision after removing the floor -----")
+94        self.planner.remove_normal_object("floor")
+95        self.print_collisions(
+96            self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
+97        )
 
    
 
 
diff --git a/docs/mplib/examples/moving_robot.html b/docs/mplib/examples/moving_robot.html
index a9a12dfb..18d34b15 100644
--- a/docs/mplib/examples/moving_robot.html
+++ b/docs/mplib/examples/moving_robot.html
@@ -75,105 +75,108 @@ 
    
 
                         
 
-                        
     1import sapien.core as sapien
- 2from .demo_setup import DemoSetup
- 3
- 4class PlanningDemo(DemoSetup):
- 5    """ This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively """
- 6    def __init__(self):
- 7        """
- 8        Setting up the scene, the planner, and adding some objects to the scene
- 9        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]
-10        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-11        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
-12        """
-13        super().__init__()
-14        self.setup_scene()
-15        self.load_robot(robot_origin_xyz=[1, 1, 0])
-16        self.setup_planner()
-17
-18        # We also need to tell the planner where the base is since the sim and planner don't share info
-19        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
-20
-21        # Set initial joint positions
-22        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-23        self.robot.set_qpos(init_qpos)
-24
-25        # table top
-26        builder = self.scene.create_actor_builder()
-27        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-28        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-29        self.table = builder.build_kinematic(name='table')
-30        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
-31
-32        # boxes
-33        builder = self.scene.create_actor_builder()
-34        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-35        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-36        self.red_cube = builder.build(name='red_cube')
-37        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
-38
-39        builder = self.scene.create_actor_builder()
-40        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-41        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-42        self.green_cube = builder.build(name='green_cube')
-43        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
-44
-45        builder = self.scene.create_actor_builder()
-46        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-47        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-48        self.blue_cube = builder.build(name='blue_cube')
-49        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
-50
-51    def demo(self):
-52        """
-53        Same demo as demo.py.
-54        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
-55        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
-56        the poses are specified with respect to the world
-57        """
-58        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-59                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-60                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-61        for i in range(3):
-62            pose = poses[i]
-63            pose[2] += 0.2
-64            self.move_to_pose(pose)
-65            self.open_gripper()
-66            pose[2] -= 0.12
-67            self.move_to_pose(pose)
-68            self.close_gripper()
-69            pose[2] += 0.12
-70            self.move_to_pose(pose)
-71            pose[0] += 0.1
-72            self.move_to_pose(pose)
-73            pose[2] -= 0.12
-74            self.move_to_pose(pose)
-75            self.open_gripper()
-76            pose[2] += 0.12
-77            self.move_to_pose(pose)
-78
-79        # convert the poses to be w.r.t. the world
-80        for pose in poses:
-81            pose[0] += 1
-82            pose[1] += 1
-83
-84        # plan a path to the first pose
-85        result = self.planner.plan_qpos_to_pose(
-86            poses[0],
-87            self.planner.robot.get_qpos(),
-88            time_step=1/250,
-89            wrt_world=True
-90        )
-91        if result['status'] != "Success":
-92            print(result['status'])
-93            return -1
-94        self.follow_path(result)
-95
-96if __name__ == '__main__':
-97    """ Driver code """
-98    demo = PlanningDemo()
-99    demo.demo()
+                        
      1import sapien.core as sapien
+  2
+  3from .demo_setup import DemoSetup
+  4
+  5
+  6class PlanningDemo(DemoSetup):
+  7    """This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively"""
+  8
+  9    def __init__(self):
+ 10        """
+ 11        Setting up the scene, the planner, and adding some objects to the scene
+ 12        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]
+ 13        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+ 14        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
+ 15        """
+ 16        super().__init__()
+ 17        self.setup_scene()
+ 18        self.load_robot(robot_origin_xyz=[1, 1, 0])
+ 19        self.setup_planner()
+ 20
+ 21        # We also need to tell the planner where the base is since the sim and planner don't share info
+ 22        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
+ 23
+ 24        # Set initial joint positions
+ 25        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+ 26        self.robot.set_qpos(init_qpos)
+ 27
+ 28        # table top
+ 29        builder = self.scene.create_actor_builder()
+ 30        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+ 31        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+ 32        self.table = builder.build_kinematic(name="table")
+ 33        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
+ 34
+ 35        # boxes
+ 36        builder = self.scene.create_actor_builder()
+ 37        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+ 38        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+ 39        self.red_cube = builder.build(name="red_cube")
+ 40        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
+ 41
+ 42        builder = self.scene.create_actor_builder()
+ 43        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+ 44        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+ 45        self.green_cube = builder.build(name="green_cube")
+ 46        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
+ 47
+ 48        builder = self.scene.create_actor_builder()
+ 49        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+ 50        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+ 51        self.blue_cube = builder.build(name="blue_cube")
+ 52        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
+ 53
+ 54    def demo(self):
+ 55        """
+ 56        Same demo as demo.py.
+ 57        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
+ 58        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
+ 59        the poses are specified with respect to the world
+ 60        """
+ 61        poses = [
+ 62            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+ 63            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+ 64            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+ 65        ]
+ 66        for i in range(3):
+ 67            pose = poses[i]
+ 68            pose[2] += 0.2
+ 69            self.move_to_pose(pose)
+ 70            self.open_gripper()
+ 71            pose[2] -= 0.12
+ 72            self.move_to_pose(pose)
+ 73            self.close_gripper()
+ 74            pose[2] += 0.12
+ 75            self.move_to_pose(pose)
+ 76            pose[0] += 0.1
+ 77            self.move_to_pose(pose)
+ 78            pose[2] -= 0.12
+ 79            self.move_to_pose(pose)
+ 80            self.open_gripper()
+ 81            pose[2] += 0.12
+ 82            self.move_to_pose(pose)
+ 83
+ 84        # convert the poses to be w.r.t. the world
+ 85        for pose in poses:
+ 86            pose[0] += 1
+ 87            pose[1] += 1
+ 88
+ 89        # plan a path to the first pose
+ 90        result = self.planner.plan_qpos_to_pose(
+ 91            poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=True
+ 92        )
+ 93        if result["status"] != "Success":
+ 94            print(result["status"])
+ 95            return -1
+ 96        self.follow_path(result)
+ 97
+ 98
+ 99if __name__ == "__main__":
+100    """Driver code"""
+101    demo = PlanningDemo()
+102    demo.demo()
 
    
 
 
@@ -189,97 +192,97 @@ 
    
 
     
    
     
-            
     5class PlanningDemo(DemoSetup):
- 6    """ This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively """
- 7    def __init__(self):
- 8        """
- 9        Setting up the scene, the planner, and adding some objects to the scene
-10        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]
-11        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-12        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
-13        """
-14        super().__init__()
-15        self.setup_scene()
-16        self.load_robot(robot_origin_xyz=[1, 1, 0])
-17        self.setup_planner()
-18
-19        # We also need to tell the planner where the base is since the sim and planner don't share info
-20        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
+            
     7class PlanningDemo(DemoSetup):
+ 8    """This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively"""
+ 9
+10    def __init__(self):
+11        """
+12        Setting up the scene, the planner, and adding some objects to the scene
+13        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]
+14        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+15        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
+16        """
+17        super().__init__()
+18        self.setup_scene()
+19        self.load_robot(robot_origin_xyz=[1, 1, 0])
+20        self.setup_planner()
 21
-22        # Set initial joint positions
-23        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-24        self.robot.set_qpos(init_qpos)
-25
-26        # table top
-27        builder = self.scene.create_actor_builder()
-28        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-29        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-30        self.table = builder.build_kinematic(name='table')
-31        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
-32
-33        # boxes
-34        builder = self.scene.create_actor_builder()
-35        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-36        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-37        self.red_cube = builder.build(name='red_cube')
-38        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
-39
-40        builder = self.scene.create_actor_builder()
-41        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-42        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-43        self.green_cube = builder.build(name='green_cube')
-44        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
-45
-46        builder = self.scene.create_actor_builder()
-47        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-48        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-49        self.blue_cube = builder.build(name='blue_cube')
-50        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
-51
-52    def demo(self):
-53        """
-54        Same demo as demo.py.
-55        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
-56        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
-57        the poses are specified with respect to the world
-58        """
-59        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-60                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-61                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-62        for i in range(3):
-63            pose = poses[i]
-64            pose[2] += 0.2
-65            self.move_to_pose(pose)
-66            self.open_gripper()
-67            pose[2] -= 0.12
-68            self.move_to_pose(pose)
-69            self.close_gripper()
-70            pose[2] += 0.12
-71            self.move_to_pose(pose)
-72            pose[0] += 0.1
+22        # We also need to tell the planner where the base is since the sim and planner don't share info
+23        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
+24
+25        # Set initial joint positions
+26        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+27        self.robot.set_qpos(init_qpos)
+28
+29        # table top
+30        builder = self.scene.create_actor_builder()
+31        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+32        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+33        self.table = builder.build_kinematic(name="table")
+34        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
+35
+36        # boxes
+37        builder = self.scene.create_actor_builder()
+38        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+39        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+40        self.red_cube = builder.build(name="red_cube")
+41        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
+42
+43        builder = self.scene.create_actor_builder()
+44        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+45        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+46        self.green_cube = builder.build(name="green_cube")
+47        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
+48
+49        builder = self.scene.create_actor_builder()
+50        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+51        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+52        self.blue_cube = builder.build(name="blue_cube")
+53        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
+54
+55    def demo(self):
+56        """
+57        Same demo as demo.py.
+58        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
+59        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
+60        the poses are specified with respect to the world
+61        """
+62        poses = [
+63            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+64            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+65            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+66        ]
+67        for i in range(3):
+68            pose = poses[i]
+69            pose[2] += 0.2
+70            self.move_to_pose(pose)
+71            self.open_gripper()
+72            pose[2] -= 0.12
 73            self.move_to_pose(pose)
-74            pose[2] -= 0.12
-75            self.move_to_pose(pose)
-76            self.open_gripper()
-77            pose[2] += 0.12
+74            self.close_gripper()
+75            pose[2] += 0.12
+76            self.move_to_pose(pose)
+77            pose[0] += 0.1
 78            self.move_to_pose(pose)
-79
-80        # convert the poses to be w.r.t. the world
-81        for pose in poses:
-82            pose[0] += 1
-83            pose[1] += 1
+79            pose[2] -= 0.12
+80            self.move_to_pose(pose)
+81            self.open_gripper()
+82            pose[2] += 0.12
+83            self.move_to_pose(pose)
 84
-85        # plan a path to the first pose
-86        result = self.planner.plan_qpos_to_pose(
-87            poses[0],
-88            self.planner.robot.get_qpos(),
-89            time_step=1/250,
-90            wrt_world=True
-91        )
-92        if result['status'] != "Success":
-93            print(result['status'])
-94            return -1
-95        self.follow_path(result)
+85        # convert the poses to be w.r.t. the world
+86        for pose in poses:
+87            pose[0] += 1
+88            pose[1] += 1
+89
+90        # plan a path to the first pose
+91        result = self.planner.plan_qpos_to_pose(
+92            poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=True
+93        )
+94        if result["status"] != "Success":
+95            print(result["status"])
+96            return -1
+97        self.follow_path(result)
 
    
 
 
@@ -297,50 +300,50 @@ 
    
 
     
    
     
-            
     7    def __init__(self):
- 8        """
- 9        Setting up the scene, the planner, and adding some objects to the scene
-10        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]
-11        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
-12        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
-13        """
-14        super().__init__()
-15        self.setup_scene()
-16        self.load_robot(robot_origin_xyz=[1, 1, 0])
-17        self.setup_planner()
-18
-19        # We also need to tell the planner where the base is since the sim and planner don't share info
-20        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
+            
    10    def __init__(self):
+11        """
+12        Setting up the scene, the planner, and adding some objects to the scene
+13        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]
+14        Afterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
+15        Compared to demo.py, all the props are shifted by 1 meter in the x and y direction
+16        """
+17        super().__init__()
+18        self.setup_scene()
+19        self.load_robot(robot_origin_xyz=[1, 1, 0])
+20        self.setup_planner()
 21
-22        # Set initial joint positions
-23        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-24        self.robot.set_qpos(init_qpos)
-25
-26        # table top
-27        builder = self.scene.create_actor_builder()
-28        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-29        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-30        self.table = builder.build_kinematic(name='table')
-31        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
-32
-33        # boxes
-34        builder = self.scene.create_actor_builder()
-35        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-36        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-37        self.red_cube = builder.build(name='red_cube')
-38        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
-39
-40        builder = self.scene.create_actor_builder()
-41        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
-42        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
-43        self.green_cube = builder.build(name='green_cube')
-44        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
-45
-46        builder = self.scene.create_actor_builder()
-47        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
-48        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
-49        self.blue_cube = builder.build(name='blue_cube')
-50        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
+22        # We also need to tell the planner where the base is since the sim and planner don't share info
+23        self.planner.set_base_pose([1, 1, 0, 1, 0, 0, 0])
+24
+25        # Set initial joint positions
+26        init_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+27        self.robot.set_qpos(init_qpos)
+28
+29        # table top
+30        builder = self.scene.create_actor_builder()
+31        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+32        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+33        self.table = builder.build_kinematic(name="table")
+34        self.table.set_pose(sapien.Pose([1.56, 1, -0.025]))
+35
+36        # boxes
+37        builder = self.scene.create_actor_builder()
+38        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+39        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+40        self.red_cube = builder.build(name="red_cube")
+41        self.red_cube.set_pose(sapien.Pose([1.4, 1.3, 0.06]))
+42
+43        builder = self.scene.create_actor_builder()
+44        builder.add_box_collision(half_size=[0.02, 0.02, 0.04])
+45        builder.add_box_visual(half_size=[0.02, 0.02, 0.04], color=[0, 1, 0])
+46        self.green_cube = builder.build(name="green_cube")
+47        self.green_cube.set_pose(sapien.Pose([1.2, 0.7, 0.04]))
+48
+49        builder = self.scene.create_actor_builder()
+50        builder.add_box_collision(half_size=[0.02, 0.02, 0.07])
+51        builder.add_box_visual(half_size=[0.02, 0.02, 0.07], color=[0, 0, 1])
+52        self.blue_cube = builder.build(name="blue_cube")
+53        self.blue_cube.set_pose(sapien.Pose([1.6, 1.1, 0.07]))
 
    
 
 
@@ -407,50 +410,49 @@ 
    
 
     
    
     
-            
    52    def demo(self):
-53        """
-54        Same demo as demo.py.
-55        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
-56        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
-57        the poses are specified with respect to the world
-58        """
-59        poses = [[0.4, 0.3, 0.12, 0, 1, 0, 0],
-60                [0.2, -0.3, 0.08, 0, 1, 0, 0],
-61                [0.6, 0.1, 0.14, 0, 1, 0, 0]]
-62        for i in range(3):
-63            pose = poses[i]
-64            pose[2] += 0.2
-65            self.move_to_pose(pose)
-66            self.open_gripper()
-67            pose[2] -= 0.12
-68            self.move_to_pose(pose)
-69            self.close_gripper()
-70            pose[2] += 0.12
-71            self.move_to_pose(pose)
-72            pose[0] += 0.1
+            
    55    def demo(self):
+56        """
+57        Same demo as demo.py.
+58        Although we shifted everything, the poses have not changed because these are w.r.t. the robot base
+59        Alternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner
+60        the poses are specified with respect to the world
+61        """
+62        poses = [
+63            [0.4, 0.3, 0.12, 0, 1, 0, 0],
+64            [0.2, -0.3, 0.08, 0, 1, 0, 0],
+65            [0.6, 0.1, 0.14, 0, 1, 0, 0],
+66        ]
+67        for i in range(3):
+68            pose = poses[i]
+69            pose[2] += 0.2
+70            self.move_to_pose(pose)
+71            self.open_gripper()
+72            pose[2] -= 0.12
 73            self.move_to_pose(pose)
-74            pose[2] -= 0.12
-75            self.move_to_pose(pose)
-76            self.open_gripper()
-77            pose[2] += 0.12
+74            self.close_gripper()
+75            pose[2] += 0.12
+76            self.move_to_pose(pose)
+77            pose[0] += 0.1
 78            self.move_to_pose(pose)
-79
-80        # convert the poses to be w.r.t. the world
-81        for pose in poses:
-82            pose[0] += 1
-83            pose[1] += 1
+79            pose[2] -= 0.12
+80            self.move_to_pose(pose)
+81            self.open_gripper()
+82            pose[2] += 0.12
+83            self.move_to_pose(pose)
 84
-85        # plan a path to the first pose
-86        result = self.planner.plan_qpos_to_pose(
-87            poses[0],
-88            self.planner.robot.get_qpos(),
-89            time_step=1/250,
-90            wrt_world=True
-91        )
-92        if result['status'] != "Success":
-93            print(result['status'])
-94            return -1
-95        self.follow_path(result)
+85        # convert the poses to be w.r.t. the world
+86        for pose in poses:
+87            pose[0] += 1
+88            pose[1] += 1
+89
+90        # plan a path to the first pose
+91        result = self.planner.plan_qpos_to_pose(
+92            poses[0], self.planner.robot.get_qpos(), time_step=1 / 250, wrt_world=True
+93        )
+94        if result["status"] != "Success":
+95            print(result["status"])
+96            return -1
+97        self.follow_path(result)
 
    
 
 
diff --git a/docs/mplib/examples/two_stage_motion.html b/docs/mplib/examples/two_stage_motion.html
index 5efa77ab..77c8a2f7 100644
--- a/docs/mplib/examples/two_stage_motion.html
+++ b/docs/mplib/examples/two_stage_motion.html
@@ -84,152 +84,202 @@ 
    
 
                         
 
-                        
      1import sapien.core as sapien
-  2import mplib
-  3import numpy as np
-  4from .demo_setup import DemoSetup
+                        
      1import numpy as np
+  2import sapien.core as sapien
+  3
+  4import mplib
   5
-  6class PlanningDemo(DemoSetup):
-  7    """
-  8    This demo is the same as collision_avoidance.py except we added a track for the robot to move along
-  9    We reach the target in two stages:
- 10    1. First, we move the base while fixing the arm joints
- 11    2. Then, we move the arm while fixing the base joints
- 12    This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
- 13    """
- 14    def __init__(self):
- 15        """
- 16        We have modified the urdf file to include a track for the robot to move along
- 17        Otherwise, the setup is the same as collision_avoidance.py
- 18        """
- 19        super().__init__()
- 20        self.setup_scene()
- 21        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
- 22        link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"]
- 23        joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"]
- 24        self.setup_planner(
- 25            urdf_path="./data/panda/panda_on_rail.urdf",
- 26            srdf_path="./data/panda/panda_on_rail.srdf",
- 27            link_names=link_names,
- 28            joint_names=joint_names,
- 29            joint_vel_limits=np.ones(9),
- 30            joint_acc_limits=np.ones(9)
- 31        )
- 32
- 33        # Set initial joint positions
- 34        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
- 35        self.robot.set_qpos(init_qpos)
- 36
- 37        # table top
- 38        builder = self.scene.create_actor_builder()
- 39        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
- 40        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
- 41        self.table = builder.build_kinematic(name='table')
- 42        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
- 43
- 44        # boxes
- 45        builder = self.scene.create_actor_builder()
- 46        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
- 47        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
- 48        self.red_cube = builder.build(name='red_cube')
- 49        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
- 50
- 51        builder = self.scene.create_actor_builder()
- 52        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
- 53        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
- 54        self.green_cube = builder.build(name='green_cube')
- 55        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
- 56
- 57        builder = self.scene.create_actor_builder()
- 58        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
- 59        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
- 60        self.blue_cube = builder.build(name='blue_cube')
- 61        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+  6from .demo_setup import DemoSetup
+  7
+  8
+  9class PlanningDemo(DemoSetup):
+ 10    """
+ 11    This demo is the same as collision_avoidance.py except we added a track for the robot to move along
+ 12    We reach the target in two stages:
+ 13    1. First, we move the base while fixing the arm joints
+ 14    2. Then, we move the arm while fixing the base joints
+ 15    This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
+ 16    """
+ 17
+ 18    def __init__(self):
+ 19        """
+ 20        We have modified the urdf file to include a track for the robot to move along
+ 21        Otherwise, the setup is the same as collision_avoidance.py
+ 22        """
+ 23        super().__init__()
+ 24        self.setup_scene()
+ 25        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
+ 26        link_names = [
+ 27            "track_x",
+ 28            "track_y",
+ 29            "panda_link0",
+ 30            "panda_link1",
+ 31            "panda_link2",
+ 32            "panda_link3",
+ 33            "panda_link4",
+ 34            "panda_link5",
+ 35            "panda_link6",
+ 36            "panda_link7",
+ 37            "panda_hand",
+ 38            "panda_leftfinger",
+ 39            "panda_rightfinger",
+ 40        ]
+ 41        joint_names = [
+ 42            "move_x",
+ 43            "move_y",
+ 44            "panda_joint1",
+ 45            "panda_joint2",
+ 46            "panda_joint3",
+ 47            "panda_joint4",
+ 48            "panda_joint5",
+ 49            "panda_joint6",
+ 50            "panda_joint7",
+ 51            "panda_finger_joint1",
+ 52            "panda_finger_joint2",
+ 53        ]
+ 54        self.setup_planner(
+ 55            urdf_path="./data/panda/panda_on_rail.urdf",
+ 56            srdf_path="./data/panda/panda_on_rail.srdf",
+ 57            link_names=link_names,
+ 58            joint_names=joint_names,
+ 59            joint_vel_limits=np.ones(9),
+ 60            joint_acc_limits=np.ones(9),
+ 61        )
  62
- 63    def add_point_cloud(self):
- 64        """ see collision_avoidance.py for details """
- 65        import trimesh
- 66        box = trimesh.creation.box([0.1, 0.4, 0.2])
- 67        points, _ = trimesh.sample.sample_surface(box, 1000)
- 68        points += [0.55, 0, 0.1]
- 69        self.planner.update_point_cloud(points)
- 70        return 
- 71
- 72    def plan_without_base(self, pose, has_attach=False):
- 73        """ a subroutine to plan a path without moving the base """
- 74        # now do a partial ik to the pose with the base fixed
- 75        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0])
- 76        if status != "Success":
- 77            print("IK failed")
- 78            exit(1)
- 79        # now fix base and plan a path to the goal
- 80        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
- 81            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2))
- 82        return result
- 83
- 84    def move_in_two_stage(self, pose, has_attach=False):
- 85        """
- 86        first, we do a full IK but only generate motions for the base
- 87        then, do another partial IK for the arm and generate motions for the arm
- 88        """
- 89        # do a full ik to the pose
- 90        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
- 91        if status != "Success":
- 92            print("IK failed")
- 93            exit(1)
- 94        # now fix arm joints and plan a path to the goal
- 95        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
- 96            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9))
- 97        # execute the planned path
- 98        self.follow_path(result)
- 99        result = self.plan_without_base(pose, has_attach)
-100        # execute the planned path
-101        self.follow_path(result)
+ 63        # Set initial joint positions
+ 64        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+ 65        self.robot.set_qpos(init_qpos)
+ 66
+ 67        # table top
+ 68        builder = self.scene.create_actor_builder()
+ 69        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+ 70        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+ 71        self.table = builder.build_kinematic(name="table")
+ 72        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+ 73
+ 74        # boxes
+ 75        builder = self.scene.create_actor_builder()
+ 76        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+ 77        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+ 78        self.red_cube = builder.build(name="red_cube")
+ 79        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+ 80
+ 81        builder = self.scene.create_actor_builder()
+ 82        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+ 83        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+ 84        self.green_cube = builder.build(name="green_cube")
+ 85        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+ 86
+ 87        builder = self.scene.create_actor_builder()
+ 88        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+ 89        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+ 90        self.blue_cube = builder.build(name="blue_cube")
+ 91        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+ 92
+ 93    def add_point_cloud(self):
+ 94        """see collision_avoidance.py for details"""
+ 95        import trimesh
+ 96
+ 97        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 98        points, _ = trimesh.sample.sample_surface(box, 1000)
+ 99        points += [0.55, 0, 0.1]
+100        self.planner.update_point_cloud(points)
+101        return
 102
-103    def demo(self):
-104        """
-105        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
-106        """
-107        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-108        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-109        
-110        self.add_point_cloud()
-111        # also add the target as a collision object so we don't hit it
-112        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
-113        collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0])
-114        self.planner.planning_world.set_normal_object("target", collision_object)
-115        
-116        # go above the target
-117        pickup_pose[2] += 0.2
-118        self.move_in_two_stage(pickup_pose)
-119        self.open_gripper()
-120        # move down to pick
-121        self.planner.planning_world.remove_normal_object("target")  # remove the object so we don't report collision
-122        pickup_pose[2] -= 0.12
-123        result = self.plan_without_base(pickup_pose)
-124        self.follow_path(result)
-125        self.close_gripper()
-126
-127        # add the attached box to the planning world
-128        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
-129
-130        pickup_pose[2] += 0.12
-131        result = self.plan_without_base(pickup_pose, has_attach=True)
-132        self.follow_path(result)
-133        delivery_pose[2] += 0.2
-134        # now go to the drop off in two stages
-135        self.move_in_two_stage(delivery_pose, has_attach=True)
-136        delivery_pose[2] -= 0.12
-137        result = self.plan_without_base(delivery_pose, has_attach=True)
-138        self.follow_path(result)
-139        self.open_gripper()
-140        delivery_pose[2] += 0.12
-141        result = self.plan_without_base(delivery_pose)
-142        self.follow_path(result)
-143
-144if __name__ == '__main__':
-145    demo = PlanningDemo()
-146    demo.demo()
+103    def plan_without_base(self, pose, has_attach=False):
+104        """a subroutine to plan a path without moving the base"""
+105        # now do a partial ik to the pose with the base fixed
+106        status, goal_qposes = self.planner.IK(
+107            pose, self.robot.get_qpos(), mask=[1, 1, 0, 0, 0, 0, 0, 0, 0]
+108        )
+109        if status != "Success":
+110            print("IK failed")
+111            exit(1)
+112        # now fix base and plan a path to the goal
+113        result = self.planner.plan_qpos_to_qpos(
+114            goal_qposes,
+115            self.robot.get_qpos(),
+116            use_point_cloud=True,
+117            use_attach=has_attach,
+118            time_step=1 / 250,
+119            fixed_joint_indices=range(2),
+120        )
+121        return result
+122
+123    def move_in_two_stage(self, pose, has_attach=False):
+124        """
+125        first, we do a full IK but only generate motions for the base
+126        then, do another partial IK for the arm and generate motions for the arm
+127        """
+128        # do a full ik to the pose
+129        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
+130        if status != "Success":
+131            print("IK failed")
+132            exit(1)
+133        # now fix arm joints and plan a path to the goal
+134        result = self.planner.plan_qpos_to_qpos(
+135            goal_qposes,
+136            self.robot.get_qpos(),
+137            use_point_cloud=True,
+138            use_attach=has_attach,
+139            time_step=1 / 250,
+140            fixed_joint_indices=range(2, 9),
+141        )
+142        # execute the planned path
+143        self.follow_path(result)
+144        result = self.plan_without_base(pose, has_attach)
+145        # execute the planned path
+146        self.follow_path(result)
+147
+148    def demo(self):
+149        """
+150        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
+151        """
+152        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+153        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+154
+155        self.add_point_cloud()
+156        # also add the target as a collision object so we don't hit it
+157        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
+158        collision_object = mplib.planner.fcl.CollisionObject(
+159            fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]
+160        )
+161        self.planner.planning_world.set_normal_object("target", collision_object)
+162
+163        # go above the target
+164        pickup_pose[2] += 0.2
+165        self.move_in_two_stage(pickup_pose)
+166        self.open_gripper()
+167        # move down to pick
+168        self.planner.planning_world.remove_normal_object(
+169            "target"
+170        )  # remove the object so we don't report collision
+171        pickup_pose[2] -= 0.12
+172        result = self.plan_without_base(pickup_pose)
+173        self.follow_path(result)
+174        self.close_gripper()
+175
+176        # add the attached box to the planning world
+177        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
+178
+179        pickup_pose[2] += 0.12
+180        result = self.plan_without_base(pickup_pose, has_attach=True)
+181        self.follow_path(result)
+182        delivery_pose[2] += 0.2
+183        # now go to the drop off in two stages
+184        self.move_in_two_stage(delivery_pose, has_attach=True)
+185        delivery_pose[2] -= 0.12
+186        result = self.plan_without_base(delivery_pose, has_attach=True)
+187        self.follow_path(result)
+188        self.open_gripper()
+189        delivery_pose[2] += 0.12
+190        result = self.plan_without_base(delivery_pose)
+191        self.follow_path(result)
+192
+193
+194if __name__ == "__main__":
+195    demo = PlanningDemo()
+196    demo.demo()
 
    
 
 
@@ -245,143 +295,189 @@ 
    
 
     
    
     
-            
      7class PlanningDemo(DemoSetup):
-  8    """
-  9    This demo is the same as collision_avoidance.py except we added a track for the robot to move along
- 10    We reach the target in two stages:
- 11    1. First, we move the base while fixing the arm joints
- 12    2. Then, we move the arm while fixing the base joints
- 13    This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
- 14    """
- 15    def __init__(self):
- 16        """
- 17        We have modified the urdf file to include a track for the robot to move along
- 18        Otherwise, the setup is the same as collision_avoidance.py
- 19        """
- 20        super().__init__()
- 21        self.setup_scene()
- 22        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
- 23        link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"]
- 24        joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"]
- 25        self.setup_planner(
- 26            urdf_path="./data/panda/panda_on_rail.urdf",
- 27            srdf_path="./data/panda/panda_on_rail.srdf",
- 28            link_names=link_names,
- 29            joint_names=joint_names,
- 30            joint_vel_limits=np.ones(9),
- 31            joint_acc_limits=np.ones(9)
- 32        )
- 33
- 34        # Set initial joint positions
- 35        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
- 36        self.robot.set_qpos(init_qpos)
- 37
- 38        # table top
- 39        builder = self.scene.create_actor_builder()
- 40        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
- 41        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
- 42        self.table = builder.build_kinematic(name='table')
- 43        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
- 44
- 45        # boxes
- 46        builder = self.scene.create_actor_builder()
- 47        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
- 48        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
- 49        self.red_cube = builder.build(name='red_cube')
- 50        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
- 51
- 52        builder = self.scene.create_actor_builder()
- 53        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
- 54        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
- 55        self.green_cube = builder.build(name='green_cube')
- 56        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
- 57
- 58        builder = self.scene.create_actor_builder()
- 59        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
- 60        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
- 61        self.blue_cube = builder.build(name='blue_cube')
- 62        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+            
     10class PlanningDemo(DemoSetup):
+ 11    """
+ 12    This demo is the same as collision_avoidance.py except we added a track for the robot to move along
+ 13    We reach the target in two stages:
+ 14    1. First, we move the base while fixing the arm joints
+ 15    2. Then, we move the arm while fixing the base joints
+ 16    This corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
+ 17    """
+ 18
+ 19    def __init__(self):
+ 20        """
+ 21        We have modified the urdf file to include a track for the robot to move along
+ 22        Otherwise, the setup is the same as collision_avoidance.py
+ 23        """
+ 24        super().__init__()
+ 25        self.setup_scene()
+ 26        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
+ 27        link_names = [
+ 28            "track_x",
+ 29            "track_y",
+ 30            "panda_link0",
+ 31            "panda_link1",
+ 32            "panda_link2",
+ 33            "panda_link3",
+ 34            "panda_link4",
+ 35            "panda_link5",
+ 36            "panda_link6",
+ 37            "panda_link7",
+ 38            "panda_hand",
+ 39            "panda_leftfinger",
+ 40            "panda_rightfinger",
+ 41        ]
+ 42        joint_names = [
+ 43            "move_x",
+ 44            "move_y",
+ 45            "panda_joint1",
+ 46            "panda_joint2",
+ 47            "panda_joint3",
+ 48            "panda_joint4",
+ 49            "panda_joint5",
+ 50            "panda_joint6",
+ 51            "panda_joint7",
+ 52            "panda_finger_joint1",
+ 53            "panda_finger_joint2",
+ 54        ]
+ 55        self.setup_planner(
+ 56            urdf_path="./data/panda/panda_on_rail.urdf",
+ 57            srdf_path="./data/panda/panda_on_rail.srdf",
+ 58            link_names=link_names,
+ 59            joint_names=joint_names,
+ 60            joint_vel_limits=np.ones(9),
+ 61            joint_acc_limits=np.ones(9),
+ 62        )
  63
- 64    def add_point_cloud(self):
- 65        """ see collision_avoidance.py for details """
- 66        import trimesh
- 67        box = trimesh.creation.box([0.1, 0.4, 0.2])
- 68        points, _ = trimesh.sample.sample_surface(box, 1000)
- 69        points += [0.55, 0, 0.1]
- 70        self.planner.update_point_cloud(points)
- 71        return 
- 72
- 73    def plan_without_base(self, pose, has_attach=False):
- 74        """ a subroutine to plan a path without moving the base """
- 75        # now do a partial ik to the pose with the base fixed
- 76        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0])
- 77        if status != "Success":
- 78            print("IK failed")
- 79            exit(1)
- 80        # now fix base and plan a path to the goal
- 81        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
- 82            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2))
- 83        return result
- 84
- 85    def move_in_two_stage(self, pose, has_attach=False):
- 86        """
- 87        first, we do a full IK but only generate motions for the base
- 88        then, do another partial IK for the arm and generate motions for the arm
- 89        """
- 90        # do a full ik to the pose
- 91        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
- 92        if status != "Success":
- 93            print("IK failed")
- 94            exit(1)
- 95        # now fix arm joints and plan a path to the goal
- 96        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
- 97            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9))
- 98        # execute the planned path
- 99        self.follow_path(result)
-100        result = self.plan_without_base(pose, has_attach)
-101        # execute the planned path
-102        self.follow_path(result)
+ 64        # Set initial joint positions
+ 65        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+ 66        self.robot.set_qpos(init_qpos)
+ 67
+ 68        # table top
+ 69        builder = self.scene.create_actor_builder()
+ 70        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+ 71        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+ 72        self.table = builder.build_kinematic(name="table")
+ 73        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+ 74
+ 75        # boxes
+ 76        builder = self.scene.create_actor_builder()
+ 77        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+ 78        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+ 79        self.red_cube = builder.build(name="red_cube")
+ 80        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+ 81
+ 82        builder = self.scene.create_actor_builder()
+ 83        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+ 84        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+ 85        self.green_cube = builder.build(name="green_cube")
+ 86        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+ 87
+ 88        builder = self.scene.create_actor_builder()
+ 89        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+ 90        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+ 91        self.blue_cube = builder.build(name="blue_cube")
+ 92        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+ 93
+ 94    def add_point_cloud(self):
+ 95        """see collision_avoidance.py for details"""
+ 96        import trimesh
+ 97
+ 98        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 99        points, _ = trimesh.sample.sample_surface(box, 1000)
+100        points += [0.55, 0, 0.1]
+101        self.planner.update_point_cloud(points)
+102        return
 103
-104    def demo(self):
-105        """
-106        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
-107        """
-108        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-109        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-110        
-111        self.add_point_cloud()
-112        # also add the target as a collision object so we don't hit it
-113        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
-114        collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0])
-115        self.planner.planning_world.set_normal_object("target", collision_object)
-116        
-117        # go above the target
-118        pickup_pose[2] += 0.2
-119        self.move_in_two_stage(pickup_pose)
-120        self.open_gripper()
-121        # move down to pick
-122        self.planner.planning_world.remove_normal_object("target")  # remove the object so we don't report collision
-123        pickup_pose[2] -= 0.12
-124        result = self.plan_without_base(pickup_pose)
-125        self.follow_path(result)
-126        self.close_gripper()
-127
-128        # add the attached box to the planning world
-129        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
-130
-131        pickup_pose[2] += 0.12
-132        result = self.plan_without_base(pickup_pose, has_attach=True)
-133        self.follow_path(result)
-134        delivery_pose[2] += 0.2
-135        # now go to the drop off in two stages
-136        self.move_in_two_stage(delivery_pose, has_attach=True)
-137        delivery_pose[2] -= 0.12
-138        result = self.plan_without_base(delivery_pose, has_attach=True)
-139        self.follow_path(result)
-140        self.open_gripper()
-141        delivery_pose[2] += 0.12
-142        result = self.plan_without_base(delivery_pose)
-143        self.follow_path(result)
+104    def plan_without_base(self, pose, has_attach=False):
+105        """a subroutine to plan a path without moving the base"""
+106        # now do a partial ik to the pose with the base fixed
+107        status, goal_qposes = self.planner.IK(
+108            pose, self.robot.get_qpos(), mask=[1, 1, 0, 0, 0, 0, 0, 0, 0]
+109        )
+110        if status != "Success":
+111            print("IK failed")
+112            exit(1)
+113        # now fix base and plan a path to the goal
+114        result = self.planner.plan_qpos_to_qpos(
+115            goal_qposes,
+116            self.robot.get_qpos(),
+117            use_point_cloud=True,
+118            use_attach=has_attach,
+119            time_step=1 / 250,
+120            fixed_joint_indices=range(2),
+121        )
+122        return result
+123
+124    def move_in_two_stage(self, pose, has_attach=False):
+125        """
+126        first, we do a full IK but only generate motions for the base
+127        then, do another partial IK for the arm and generate motions for the arm
+128        """
+129        # do a full ik to the pose
+130        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
+131        if status != "Success":
+132            print("IK failed")
+133            exit(1)
+134        # now fix arm joints and plan a path to the goal
+135        result = self.planner.plan_qpos_to_qpos(
+136            goal_qposes,
+137            self.robot.get_qpos(),
+138            use_point_cloud=True,
+139            use_attach=has_attach,
+140            time_step=1 / 250,
+141            fixed_joint_indices=range(2, 9),
+142        )
+143        # execute the planned path
+144        self.follow_path(result)
+145        result = self.plan_without_base(pose, has_attach)
+146        # execute the planned path
+147        self.follow_path(result)
+148
+149    def demo(self):
+150        """
+151        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
+152        """
+153        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+154        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+155
+156        self.add_point_cloud()
+157        # also add the target as a collision object so we don't hit it
+158        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
+159        collision_object = mplib.planner.fcl.CollisionObject(
+160            fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]
+161        )
+162        self.planner.planning_world.set_normal_object("target", collision_object)
+163
+164        # go above the target
+165        pickup_pose[2] += 0.2
+166        self.move_in_two_stage(pickup_pose)
+167        self.open_gripper()
+168        # move down to pick
+169        self.planner.planning_world.remove_normal_object(
+170            "target"
+171        )  # remove the object so we don't report collision
+172        pickup_pose[2] -= 0.12
+173        result = self.plan_without_base(pickup_pose)
+174        self.follow_path(result)
+175        self.close_gripper()
+176
+177        # add the attached box to the planning world
+178        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
+179
+180        pickup_pose[2] += 0.12
+181        result = self.plan_without_base(pickup_pose, has_attach=True)
+182        self.follow_path(result)
+183        delivery_pose[2] += 0.2
+184        # now go to the drop off in two stages
+185        self.move_in_two_stage(delivery_pose, has_attach=True)
+186        delivery_pose[2] -= 0.12
+187        result = self.plan_without_base(delivery_pose, has_attach=True)
+188        self.follow_path(result)
+189        self.open_gripper()
+190        delivery_pose[2] += 0.12
+191        result = self.plan_without_base(delivery_pose)
+192        self.follow_path(result)
 
    
 
 
@@ -406,54 +502,80 @@ 
    
 
     
    
     
-            
    15    def __init__(self):
-16        """
-17        We have modified the urdf file to include a track for the robot to move along
-18        Otherwise, the setup is the same as collision_avoidance.py
-19        """
-20        super().__init__()
-21        self.setup_scene()
-22        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
-23        link_names = ["track_x", "track_y", "panda_link0", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link6", "panda_link7", "panda_hand", "panda_leftfinger", "panda_rightfinger"]
-24        joint_names = ["move_x", "move_y", "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7", "panda_finger_joint1", "panda_finger_joint2"]
-25        self.setup_planner(
-26            urdf_path="./data/panda/panda_on_rail.urdf",
-27            srdf_path="./data/panda/panda_on_rail.srdf",
-28            link_names=link_names,
-29            joint_names=joint_names,
-30            joint_vel_limits=np.ones(9),
-31            joint_acc_limits=np.ones(9)
-32        )
-33
-34        # Set initial joint positions
-35        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
-36        self.robot.set_qpos(init_qpos)
-37
-38        # table top
-39        builder = self.scene.create_actor_builder()
-40        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
-41        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
-42        self.table = builder.build_kinematic(name='table')
-43        self.table.set_pose(sapien.Pose([0.56, 0, - 0.025]))
-44
-45        # boxes
-46        builder = self.scene.create_actor_builder()
-47        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
-48        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
-49        self.red_cube = builder.build(name='red_cube')
-50        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
-51
-52        builder = self.scene.create_actor_builder()
-53        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
-54        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
-55        self.green_cube = builder.build(name='green_cube')
-56        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
-57
-58        builder = self.scene.create_actor_builder()
-59        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
-60        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
-61        self.blue_cube = builder.build(name='blue_cube')
-62        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
+            
    19    def __init__(self):
+20        """
+21        We have modified the urdf file to include a track for the robot to move along
+22        Otherwise, the setup is the same as collision_avoidance.py
+23        """
+24        super().__init__()
+25        self.setup_scene()
+26        self.load_robot(urdf_path="./data/panda/panda_on_rail.urdf")
+27        link_names = [
+28            "track_x",
+29            "track_y",
+30            "panda_link0",
+31            "panda_link1",
+32            "panda_link2",
+33            "panda_link3",
+34            "panda_link4",
+35            "panda_link5",
+36            "panda_link6",
+37            "panda_link7",
+38            "panda_hand",
+39            "panda_leftfinger",
+40            "panda_rightfinger",
+41        ]
+42        joint_names = [
+43            "move_x",
+44            "move_y",
+45            "panda_joint1",
+46            "panda_joint2",
+47            "panda_joint3",
+48            "panda_joint4",
+49            "panda_joint5",
+50            "panda_joint6",
+51            "panda_joint7",
+52            "panda_finger_joint1",
+53            "panda_finger_joint2",
+54        ]
+55        self.setup_planner(
+56            urdf_path="./data/panda/panda_on_rail.urdf",
+57            srdf_path="./data/panda/panda_on_rail.srdf",
+58            link_names=link_names,
+59            joint_names=joint_names,
+60            joint_vel_limits=np.ones(9),
+61            joint_acc_limits=np.ones(9),
+62        )
+63
+64        # Set initial joint positions
+65        init_qpos = [0, 0, 0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78, 0, 0]
+66        self.robot.set_qpos(init_qpos)
+67
+68        # table top
+69        builder = self.scene.create_actor_builder()
+70        builder.add_box_collision(half_size=[0.4, 0.4, 0.025])
+71        builder.add_box_visual(half_size=[0.4, 0.4, 0.025])
+72        self.table = builder.build_kinematic(name="table")
+73        self.table.set_pose(sapien.Pose([0.56, 0, -0.025]))
+74
+75        # boxes
+76        builder = self.scene.create_actor_builder()
+77        builder.add_box_collision(half_size=[0.02, 0.02, 0.06])
+78        builder.add_box_visual(half_size=[0.02, 0.02, 0.06], color=[1, 0, 0])
+79        self.red_cube = builder.build(name="red_cube")
+80        self.red_cube.set_pose(sapien.Pose([0.7, 0, 0.06]))
+81
+82        builder = self.scene.create_actor_builder()
+83        builder.add_box_collision(half_size=[0.04, 0.04, 0.005])
+84        builder.add_box_visual(half_size=[0.04, 0.04, 0.005], color=[0, 1, 0])
+85        self.green_cube = builder.build(name="green_cube")
+86        self.green_cube.set_pose(sapien.Pose([0.4, 0.3, 0.005]))
+87
+88        builder = self.scene.create_actor_builder()
+89        builder.add_box_collision(half_size=[0.05, 0.2, 0.1])
+90        builder.add_box_visual(half_size=[0.05, 0.2, 0.1], color=[0, 0, 1])
+91        self.blue_cube = builder.build(name="blue_cube")
+92        self.blue_cube.set_pose(sapien.Pose([0.55, 0, 0.1]))
 
    
 
 
@@ -518,14 +640,15 @@ 
    
 
     
    
     
-            
    64    def add_point_cloud(self):
-65        """ see collision_avoidance.py for details """
-66        import trimesh
-67        box = trimesh.creation.box([0.1, 0.4, 0.2])
-68        points, _ = trimesh.sample.sample_surface(box, 1000)
-69        points += [0.55, 0, 0.1]
-70        self.planner.update_point_cloud(points)
-71        return 
+            
     94    def add_point_cloud(self):
+ 95        """see collision_avoidance.py for details"""
+ 96        import trimesh
+ 97
+ 98        box = trimesh.creation.box([0.1, 0.4, 0.2])
+ 99        points, _ = trimesh.sample.sample_surface(box, 1000)
+100        points += [0.55, 0, 0.1]
+101        self.planner.update_point_cloud(points)
+102        return
 
    
 
 
@@ -545,17 +668,25 @@ 
    
 
     
    
     
-            
    73    def plan_without_base(self, pose, has_attach=False):
-74        """ a subroutine to plan a path without moving the base """
-75        # now do a partial ik to the pose with the base fixed
-76        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos(), mask=[1,1,0,0,0,0,0,0,0])
-77        if status != "Success":
-78            print("IK failed")
-79            exit(1)
-80        # now fix base and plan a path to the goal
-81        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
-82            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2))
-83        return result
+            
    104    def plan_without_base(self, pose, has_attach=False):
+105        """a subroutine to plan a path without moving the base"""
+106        # now do a partial ik to the pose with the base fixed
+107        status, goal_qposes = self.planner.IK(
+108            pose, self.robot.get_qpos(), mask=[1, 1, 0, 0, 0, 0, 0, 0, 0]
+109        )
+110        if status != "Success":
+111            print("IK failed")
+112            exit(1)
+113        # now fix base and plan a path to the goal
+114        result = self.planner.plan_qpos_to_qpos(
+115            goal_qposes,
+116            self.robot.get_qpos(),
+117            use_point_cloud=True,
+118            use_attach=has_attach,
+119            time_step=1 / 250,
+120            fixed_joint_indices=range(2),
+121        )
+122        return result
 
    
 
 
@@ -575,24 +706,30 @@ 
    
 
     
    
     
-            
     85    def move_in_two_stage(self, pose, has_attach=False):
- 86        """
- 87        first, we do a full IK but only generate motions for the base
- 88        then, do another partial IK for the arm and generate motions for the arm
- 89        """
- 90        # do a full ik to the pose
- 91        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
- 92        if status != "Success":
- 93            print("IK failed")
- 94            exit(1)
- 95        # now fix arm joints and plan a path to the goal
- 96        result = self.planner.plan_qpos_to_qpos(goal_qposes, self.robot.get_qpos(),
- 97            use_point_cloud=True, use_attach=has_attach, time_step=1/250, fixed_joint_indices=range(2, 9))
- 98        # execute the planned path
- 99        self.follow_path(result)
-100        result = self.plan_without_base(pose, has_attach)
-101        # execute the planned path
-102        self.follow_path(result)
+            
    124    def move_in_two_stage(self, pose, has_attach=False):
+125        """
+126        first, we do a full IK but only generate motions for the base
+127        then, do another partial IK for the arm and generate motions for the arm
+128        """
+129        # do a full ik to the pose
+130        status, goal_qposes = self.planner.IK(pose, self.robot.get_qpos())
+131        if status != "Success":
+132            print("IK failed")
+133            exit(1)
+134        # now fix arm joints and plan a path to the goal
+135        result = self.planner.plan_qpos_to_qpos(
+136            goal_qposes,
+137            self.robot.get_qpos(),
+138            use_point_cloud=True,
+139            use_attach=has_attach,
+140            time_step=1 / 250,
+141            fixed_joint_indices=range(2, 9),
+142        )
+143        # execute the planned path
+144        self.follow_path(result)
+145        result = self.plan_without_base(pose, has_attach)
+146        # execute the planned path
+147        self.follow_path(result)
 
    
 
 
@@ -613,46 +750,50 @@ 
    
 
     
    
     
-            
    104    def demo(self):
-105        """
-106        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
-107        """
-108        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
-109        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
-110        
-111        self.add_point_cloud()
-112        # also add the target as a collision object so we don't hit it
-113        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
-114        collision_object = mplib.planner.fcl.CollisionObject(fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0])
-115        self.planner.planning_world.set_normal_object("target", collision_object)
-116        
-117        # go above the target
-118        pickup_pose[2] += 0.2
-119        self.move_in_two_stage(pickup_pose)
-120        self.open_gripper()
-121        # move down to pick
-122        self.planner.planning_world.remove_normal_object("target")  # remove the object so we don't report collision
-123        pickup_pose[2] -= 0.12
-124        result = self.plan_without_base(pickup_pose)
-125        self.follow_path(result)
-126        self.close_gripper()
-127
-128        # add the attached box to the planning world
-129        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
-130
-131        pickup_pose[2] += 0.12
-132        result = self.plan_without_base(pickup_pose, has_attach=True)
-133        self.follow_path(result)
-134        delivery_pose[2] += 0.2
-135        # now go to the drop off in two stages
-136        self.move_in_two_stage(delivery_pose, has_attach=True)
-137        delivery_pose[2] -= 0.12
-138        result = self.plan_without_base(delivery_pose, has_attach=True)
-139        self.follow_path(result)
-140        self.open_gripper()
-141        delivery_pose[2] += 0.12
-142        result = self.plan_without_base(delivery_pose)
-143        self.follow_path(result)
+            
    149    def demo(self):
+150        """
+151        We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
+152        """
+153        pickup_pose = [0.7, 0, 0.12, 0, 1, 0, 0]
+154        delivery_pose = [0.4, 0.3, 0.13, 0, 1, 0, 0]
+155
+156        self.add_point_cloud()
+157        # also add the target as a collision object so we don't hit it
+158        fcl_red_cube = mplib.planner.fcl.Box([0.04, 0.04, 0.14])
+159        collision_object = mplib.planner.fcl.CollisionObject(
+160            fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]
+161        )
+162        self.planner.planning_world.set_normal_object("target", collision_object)
+163
+164        # go above the target
+165        pickup_pose[2] += 0.2
+166        self.move_in_two_stage(pickup_pose)
+167        self.open_gripper()
+168        # move down to pick
+169        self.planner.planning_world.remove_normal_object(
+170            "target"
+171        )  # remove the object so we don't report collision
+172        pickup_pose[2] -= 0.12
+173        result = self.plan_without_base(pickup_pose)
+174        self.follow_path(result)
+175        self.close_gripper()
+176
+177        # add the attached box to the planning world
+178        self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
+179
+180        pickup_pose[2] += 0.12
+181        result = self.plan_without_base(pickup_pose, has_attach=True)
+182        self.follow_path(result)
+183        delivery_pose[2] += 0.2
+184        # now go to the drop off in two stages
+185        self.move_in_two_stage(delivery_pose, has_attach=True)
+186        delivery_pose[2] -= 0.12
+187        result = self.plan_without_base(delivery_pose, has_attach=True)
+188        self.follow_path(result)
+189        self.open_gripper()
+190        delivery_pose[2] += 0.12
+191        result = self.plan_without_base(delivery_pose)
+192        self.follow_path(result)
 
    
 
 
diff --git a/docs/mplib/planner.html b/docs/mplib/planner.html
index 431a7490..67476054 100644
--- a/docs/mplib/planner.html
+++ b/docs/mplib/planner.html
@@ -171,14 +171,14 @@ 
    
 
                         
 
-                        
      1from typing import Sequence, Tuple, Union
-  2
-  3import os
+                        
      1import os
+  2from typing import Sequence, Tuple, Union
+  3
   4import numpy as np
-  5from transforms3d.quaternions import quat2mat, mat2quat
-  6import toppra as ta
+  5import toppra as ta
+  6import toppra.algorithm as algo
   7import toppra.constraint as constraint
-  8import toppra.algorithm as algo
+  8from transforms3d.quaternions import mat2quat, quat2mat
   9
  10from mplib.pymp import *
  11
@@ -199,7 +199,7 @@ 
    
  26        user_joint_names: Sequence[str] = [],
  27        joint_vel_limits: Union[Sequence[float], np.ndarray] = [],
  28        joint_acc_limits: Union[Sequence[float], np.ndarray] = [],
- 29        **kwargs
+ 29        **kwargs,
  30    ):
  31        r"""Motion planner for robots.
  32
@@ -221,7 +221,7 @@ 
    
  48        if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
  49            self.srdf = urdf.replace(".urdf", ".srdf")
  50            print(f"No SRDF file provided but found {self.srdf}")
- 51            
+ 51
  52        # replace package:// keyword if exists
  53        urdf = self.replace_package_keyword(package_keyword_replacement)
  54
@@ -240,7 +240,7 @@ 
    
  67            [self.robot],
  68            ["robot"],
  69            kwargs.get("normal_objects", []),
- 70            kwargs.get("normal_object_names", [])
+ 70            kwargs.get("normal_object_names", []),
  71        )
  72
  73        if srdf == "":
@@ -258,714 +258,755 @@ 
    
  85        for i, link in enumerate(self.user_link_names):
  86            self.link_name_2_idx[link] = i
  87
- 88        assert move_group in self.user_link_names,\
- 89            f"end-effector not found as one of the links in {self.user_link_names}"
- 90        self.move_group = move_group
- 91        self.robot.set_move_group(self.move_group)
- 92        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
- 93
- 94        self.joint_types = self.pinocchio_model.get_joint_types()
- 95        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
- 96        self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices))
- 97        self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices))
- 98        self.move_group_link_id = self.link_name_2_idx[self.move_group]
- 99        assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\
-100            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-101            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
-102        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\
-103            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-104            f"length of move_group ({len(self.move_group_joint_indices)})"
-105        assert len(self.joint_vel_limits) <= len(self.joint_limits),\
-106            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\
-107            f"number of total joints ({len(self.joint_limits)})"
-108        
-109        self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], [])
-110        self.planner = ompl.OMPLPlanner(world=self.planning_world)
-111
-112
-113    def replace_package_keyword(self, package_keyword_replacement):
-114        """
-115        some ROS URDF files use package:// keyword to refer the package dir
-116        replace it with the given string (default is empty)
-117
-118        Args:
-119            package_keyword_replacement: the string to replace 'package://' keyword
-120        """
-121        rtn_urdf = self.urdf
-122        with open(self.urdf, "r") as in_f:
-123            content = in_f.read()
-124            if "package://" in content:
-125                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
-126                content = content.replace("package://", package_keyword_replacement)
-127                if not os.path.exists(rtn_urdf):
-128                    with open(rtn_urdf, "w") as out_f:
-129                        out_f.write(content)
-130        return rtn_urdf
-131
-132    def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000):
-133        """
-134        we read the srdf file to get the link pairs that should not collide.
-135        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
-136        """
-137        print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.")
-138        n_link = len(self.user_link_names)
-139        cnt = np.zeros((n_link, n_link), dtype=np.int32)
-140        for i in range(sample_time):
-141            qpos = self.pinocchio_model.get_random_configuration()
-142            self.robot.set_qpos(qpos, True)
-143            collisions = self.planning_world.collide_full()
-144            for collision in collisions:
-145                u = self.link_name_2_idx[collision.link_name1]
-146                v = self.link_name_2_idx[collision.link_name2]
-147                cnt[u][v] += 1
-148            if i % echo_freq == 0:
-149                print("Finish %.1f%%!" % (i * 100 / sample_time))
-150        
-151        import xml.etree.ElementTree as ET
-152        from xml.dom import minidom
-153
-154        root = ET.Element('robot')
-155        robot_name = self.urdf.split('/')[-1].split('.')[0]
-156        root.set('name', robot_name)
-157        self.srdf = self.urdf.replace(".urdf", ".srdf")
-158
-159        for i in range(n_link):
-160            for j in range(n_link):
-161                if cnt[i][j] == sample_time:
-162                    link1 = self.user_link_names[i]
-163                    link2 = self.user_link_names[j]
-164                    print("Ignore collision pair: (%s, %s), reason:  always collide" % (link1, link2))
-165                    collision = ET.SubElement(root, 'disable_collisions')
-166                    collision.set('link1', link1)
-167                    collision.set('link2', link2)
-168                    collision.set('reason', 'Default')
-169        srdffile = open(self.srdf, "w")
-170        srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent="    "))
-171        srdffile.close()
-172        print("Saving the SRDF file to %s" % self.srdf)
-173
-174    def distance_6D(self, p1, q1, p2, q2):
-175        """
-176        compute the distance between two poses
-177        
-178        Args:
-179            p1: position of pose 1
-180            q1: quaternion of pose 1
-181            p2: position of pose 2
-182            q2: quaternion of pose 2
-183        """
-184        return np.linalg.norm(p1 - p2) + min(
-185            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
-186        )
-187
-188    def check_joint_limit(self, q):
-189        """
-190        check if the joint configuration is within the joint limits
-191
-192        Args:
-193            q: joint configuration
-194        
-195        Returns:
-196            True if the joint configuration is within the joint limits
-197        """
-198        n = len(q)
-199        flag = True
-200        for i in range(n):
-201            if self.joint_types[i].startswith("JointModelR"):
-202                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
-203                    continue
-204                q[i] -= (
-205                    2
-206                    * np.pi
-207                    * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
-208                )
-209                if q[i] > self.joint_limits[i][1] + 1e-3:
-210                    flag = False
-211            else:
-212                if (
-213                    q[i] < self.joint_limits[i][0] - 1e-3
-214                    or q[i] > self.joint_limits[i][1] + 1e-3
-215                ):
-216                    flag = False
-217        return flag
-218
-219    def pad_qpos(self, qpos, articulation=None):
-220        """
-221        if the user does not provide the full qpos but only the move_group joints,
-222        pad the qpos with the rest of the joints
-223        """
-224        if len(qpos) == len(self.move_group_joint_indices):
-225            tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos()
-226            tmp[:len(qpos)] = qpos
-227            qpos = tmp
-228
-229        assert len(qpos) == len(self.joint_limits),\
-230            f"length of qpos ({len(qpos)}) =/= "\
-231            f"number of total joints ({len(self.joint_limits)})"
-232
-233        return qpos
-234
-235    def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-236        """ helper function to check for collision """
-237        # handle no user input
-238        if articulation is None:
-239            articulation = self.robot
-240        if qpos is None:
-241            qpos = articulation.get_qpos()
-242        qpos = self.pad_qpos(qpos, articulation)
-243
-244        # first save the current qpos
-245        old_qpos = articulation.get_qpos()
-246        # set robot to new qpos
-247        articulation.set_qpos(qpos, True)
-248        # find the index of the articulation inside the array
-249        idx = self.planning_world.get_articulations().index(articulation)
-250        # check for self-collision
-251        collisions = collision_function(idx)
-252        # reset qpos
-253        articulation.set_qpos(old_qpos, True)
-254        return collisions
-255
-256    def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-257        """Check if the robot is in self-collision.
+ 88        assert (
+ 89            move_group in self.user_link_names
+ 90        ), f"end-effector not found as one of the links in {self.user_link_names}"
+ 91        self.move_group = move_group
+ 92        self.robot.set_move_group(self.move_group)
+ 93        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
+ 94
+ 95        self.joint_types = self.pinocchio_model.get_joint_types()
+ 96        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
+ 97        self.joint_vel_limits = (
+ 98            joint_vel_limits
+ 99            if len(joint_vel_limits)
+100            else np.ones(len(self.move_group_joint_indices))
+101        )
+102        self.joint_acc_limits = (
+103            joint_acc_limits
+104            if len(joint_acc_limits)
+105            else np.ones(len(self.move_group_joint_indices))
+106        )
+107        self.move_group_link_id = self.link_name_2_idx[self.move_group]
+108        assert len(self.joint_vel_limits) == len(self.joint_acc_limits), (
+109            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+110            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
+111        )
+112        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices), (
+113            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+114            f"length of move_group ({len(self.move_group_joint_indices)})"
+115        )
+116        assert len(self.joint_vel_limits) <= len(self.joint_limits), (
+117            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "
+118            f"number of total joints ({len(self.joint_limits)})"
+119        )
+120
+121        self.planning_world = planning_world.PlanningWorld(
+122            [self.robot], ["robot"], [], []
+123        )
+124        self.planner = ompl.OMPLPlanner(world=self.planning_world)
+125
+126    def replace_package_keyword(self, package_keyword_replacement):
+127        """
+128        some ROS URDF files use package:// keyword to refer the package dir
+129        replace it with the given string (default is empty)
+130
+131        Args:
+132            package_keyword_replacement: the string to replace 'package://' keyword
+133        """
+134        rtn_urdf = self.urdf
+135        with open(self.urdf, "r") as in_f:
+136            content = in_f.read()
+137            if "package://" in content:
+138                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
+139                content = content.replace("package://", package_keyword_replacement)
+140                if not os.path.exists(rtn_urdf):
+141                    with open(rtn_urdf, "w") as out_f:
+142                        out_f.write(content)
+143        return rtn_urdf
+144
+145    def generate_collision_pair(self, sample_time=1000000, echo_freq=100000):
+146        """
+147        we read the srdf file to get the link pairs that should not collide.
+148        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
+149        """
+150        print(
+151            "Since no SRDF file is provided. We will first detect link pairs that will"
+152            " always collide. This may take several minutes."
+153        )
+154        n_link = len(self.user_link_names)
+155        cnt = np.zeros((n_link, n_link), dtype=np.int32)
+156        for i in range(sample_time):
+157            qpos = self.pinocchio_model.get_random_configuration()
+158            self.robot.set_qpos(qpos, True)
+159            collisions = self.planning_world.collide_full()
+160            for collision in collisions:
+161                u = self.link_name_2_idx[collision.link_name1]
+162                v = self.link_name_2_idx[collision.link_name2]
+163                cnt[u][v] += 1
+164            if i % echo_freq == 0:
+165                print("Finish %.1f%%!" % (i * 100 / sample_time))
+166
+167        import xml.etree.ElementTree as ET
+168        from xml.dom import minidom
+169
+170        root = ET.Element("robot")
+171        robot_name = self.urdf.split("/")[-1].split(".")[0]
+172        root.set("name", robot_name)
+173        self.srdf = self.urdf.replace(".urdf", ".srdf")
+174
+175        for i in range(n_link):
+176            for j in range(n_link):
+177                if cnt[i][j] == sample_time:
+178                    link1 = self.user_link_names[i]
+179                    link2 = self.user_link_names[j]
+180                    print(
+181                        "Ignore collision pair: (%s, %s), reason:  always collide"
+182                        % (link1, link2)
+183                    )
+184                    collision = ET.SubElement(root, "disable_collisions")
+185                    collision.set("link1", link1)
+186                    collision.set("link2", link2)
+187                    collision.set("reason", "Default")
+188        srdffile = open(self.srdf, "w")
+189        srdffile.write(
+190            minidom.parseString(ET.tostring(root)).toprettyxml(indent="    ")
+191        )
+192        srdffile.close()
+193        print("Saving the SRDF file to %s" % self.srdf)
+194
+195    def distance_6D(self, p1, q1, p2, q2):
+196        """
+197        compute the distance between two poses
+198
+199        Args:
+200            p1: position of pose 1
+201            q1: quaternion of pose 1
+202            p2: position of pose 2
+203            q2: quaternion of pose 2
+204        """
+205        return np.linalg.norm(p1 - p2) + min(
+206            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
+207        )
+208
+209    def check_joint_limit(self, q):
+210        """
+211        check if the joint configuration is within the joint limits
+212
+213        Args:
+214            q: joint configuration
+215
+216        Returns:
+217            True if the joint configuration is within the joint limits
+218        """
+219        n = len(q)
+220        flag = True
+221        for i in range(n):
+222            if self.joint_types[i].startswith("JointModelR"):
+223                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
+224                    continue
+225                q[i] -= (
+226                    2 * np.pi * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
+227                )
+228                if q[i] > self.joint_limits[i][1] + 1e-3:
+229                    flag = False
+230            else:
+231                if (
+232                    q[i] < self.joint_limits[i][0] - 1e-3
+233                    or q[i] > self.joint_limits[i][1] + 1e-3
+234                ):
+235                    flag = False
+236        return flag
+237
+238    def pad_qpos(self, qpos, articulation=None):
+239        """
+240        if the user does not provide the full qpos but only the move_group joints,
+241        pad the qpos with the rest of the joints
+242        """
+243        if len(qpos) == len(self.move_group_joint_indices):
+244            tmp = (
+245                articulation.get_qpos()
+246                if articulation is not None
+247                else self.robot.get_qpos()
+248            )
+249            tmp[: len(qpos)] = qpos
+250            qpos = tmp
+251
+252        assert len(qpos) == len(self.joint_limits), (
+253            f"length of qpos ({len(qpos)}) =/= "
+254            f"number of total joints ({len(self.joint_limits)})"
+255        )
+256
+257        return qpos
 258
-259        Args:
-260            articulation: robot model. if none will be self.robot
-261            qpos: robot configuration. if none will be the current pose
-262
-263        Returns:
-264            A list of collisions.
-265        """
-266        return self.check_for_collision(self.planning_world.self_collide, articulation, qpos)
-267
-268    def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list:
-269        """Check if the robot is in collision with the environment
-270
-271        Args:
-272            articulation: robot model. if none will be self.robot
-273            qpos: robot configuration. if none will be the current pose
-274            with_point_cloud: whether to check collision against point cloud
-275            use_attach: whether to include the object attached to the end effector in collision checking
-276        Returns:
-277            A list of collisions.
-278        """
-279        # store previous results
-280        prev_use_point_cloud = self.planning_world.use_point_cloud
-281        prev_use_attach = self.planning_world.use_attach
-282        self.planning_world.set_use_point_cloud(with_point_cloud)
-283        self.planning_world.set_use_attach(use_attach)
+259    def check_for_collision(
+260        self,
+261        collision_function,
+262        articulation: articulation.ArticulatedModel = None,
+263        qpos: np.ndarray = None,
+264    ) -> list:
+265        """helper function to check for collision"""
+266        # handle no user input
+267        if articulation is None:
+268            articulation = self.robot
+269        if qpos is None:
+270            qpos = articulation.get_qpos()
+271        qpos = self.pad_qpos(qpos, articulation)
+272
+273        # first save the current qpos
+274        old_qpos = articulation.get_qpos()
+275        # set robot to new qpos
+276        articulation.set_qpos(qpos, True)
+277        # find the index of the articulation inside the array
+278        idx = self.planning_world.get_articulations().index(articulation)
+279        # check for self-collision
+280        collisions = collision_function(idx)
+281        # reset qpos
+282        articulation.set_qpos(old_qpos, True)
+283        return collisions
 284
-285        results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos)
-286
-287        # restore
-288        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
-289        self.planning_world.set_use_attach(prev_use_attach)
-290        return results
+285    def check_for_self_collision(
+286        self,
+287        articulation: articulation.ArticulatedModel = None,
+288        qpos: np.ndarray = None,
+289    ) -> list:
+290        """Check if the robot is in self-collision.
 291
-292    def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3):
-293        """
-294        Inverse kinematics
+292        Args:
+293            articulation: robot model. if none will be self.robot
+294            qpos: robot configuration. if none will be the current pose
 295
-296        Args:
-297            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-298            start_qpos: initial configuration
-299            mask: if the value at a given index is True, the joint is *not* used in the IK
-300            n_init_qpos: number of random initial configurations
-301            threshold: threshold for the distance between the goal pose and the result pose
-302        """
-303
-304        index = self.link_name_2_idx[self.move_group]
-305        min_dis = 1e9
-306        idx = self.move_group_joint_indices
-307        qpos0 = np.copy(start_qpos)
-308        results = []
-309        self.robot.set_qpos(start_qpos, True)
-310        for i in range(n_init_qpos):
-311            ik_results = self.pinocchio_model.compute_IK_CLIK(
-312                index, goal_pose, start_qpos, mask
-313            )
-314            flag = self.check_joint_limit(ik_results[0]) # will clip qpos
-315
-316            # check collision
-317            self.planning_world.set_qpos_all(ik_results[0][idx])
-318            if (len(self.planning_world.collide_full()) != 0):
-319                flag = False
-320
-321            if flag:
-322                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
-323                new_pose = self.pinocchio_model.get_link_pose(index)
-324                tmp_dis = self.distance_6D(
-325                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
-326                )
-327                if tmp_dis < min_dis:
-328                    min_dis = tmp_dis
-329                if tmp_dis < threshold:
-330                    result = ik_results[0] 
-331                    unique = True
-332                    for j in range(len(results)):
-333                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
-334                            unique = False
-335                    if unique:
-336                        results.append(result)
-337            start_qpos = self.pinocchio_model.get_random_configuration()
-338            mask_len = len(mask)
-339            if mask_len > 0:
-340                for j in range(mask_len):
-341                    if mask[j]:
-342                        start_qpos[j] = qpos0[j]
-343        if len(results) != 0:
-344            status = "Success"
-345        elif min_dis != 1e9:
-346            status = (
-347                "IK Failed! Distance %lf is greater than threshold %lf."
-348                % (min_dis, threshold)
-349            )
-350        else:
-351            status = "IK Failed! Cannot find valid solution."
-352        return status, results
-353
-354    def TOPP(self, path, step=0.1, verbose=False):
-355        """
-356        Time-Optimal Path Parameterization
-357
-358        Args:
-359            path: numpy array of shape (n, dof)
-360            step: step size for the discretization
-361            verbose: if True, will print the log of TOPPRA
-362        """
+296        Returns:
+297            A list of collisions.
+298        """
+299        return self.check_for_collision(
+300            self.planning_world.self_collide, articulation, qpos
+301        )
+302
+303    def check_for_env_collision(
+304        self,
+305        articulation: articulation.ArticulatedModel = None,
+306        qpos: np.ndarray = None,
+307        with_point_cloud=False,
+308        use_attach=False,
+309    ) -> list:
+310        """Check if the robot is in collision with the environment
+311
+312        Args:
+313            articulation: robot model. if none will be self.robot
+314            qpos: robot configuration. if none will be the current pose
+315            with_point_cloud: whether to check collision against point cloud
+316            use_attach: whether to include the object attached to the end effector in collision checking
+317        Returns:
+318            A list of collisions.
+319        """
+320        # store previous results
+321        prev_use_point_cloud = self.planning_world.use_point_cloud
+322        prev_use_attach = self.planning_world.use_attach
+323        self.planning_world.set_use_point_cloud(with_point_cloud)
+324        self.planning_world.set_use_attach(use_attach)
+325
+326        results = self.check_for_collision(
+327            self.planning_world.collide_with_others, articulation, qpos
+328        )
+329
+330        # restore
+331        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
+332        self.planning_world.set_use_attach(prev_use_attach)
+333        return results
+334
+335    def IK(self, goal_pose, start_qpos, mask=[], n_init_qpos=20, threshold=1e-3):
+336        """
+337        Inverse kinematics
+338
+339        Args:
+340            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+341            start_qpos: initial configuration
+342            mask: if the value at a given index is True, the joint is *not* used in the IK
+343            n_init_qpos: number of random initial configurations
+344            threshold: threshold for the distance between the goal pose and the result pose
+345        """
+346
+347        index = self.link_name_2_idx[self.move_group]
+348        min_dis = 1e9
+349        idx = self.move_group_joint_indices
+350        qpos0 = np.copy(start_qpos)
+351        results = []
+352        self.robot.set_qpos(start_qpos, True)
+353        for i in range(n_init_qpos):
+354            ik_results = self.pinocchio_model.compute_IK_CLIK(
+355                index, goal_pose, start_qpos, mask
+356            )
+357            flag = self.check_joint_limit(ik_results[0])  # will clip qpos
+358
+359            # check collision
+360            self.planning_world.set_qpos_all(ik_results[0][idx])
+361            if len(self.planning_world.collide_full()) != 0:
+362                flag = False
 363
-364        N_samples = path.shape[0]
-365        dof = path.shape[1]
-366        assert dof == len(self.joint_vel_limits)
-367        assert dof == len(self.joint_acc_limits)
-368        ss = np.linspace(0, 1, N_samples)
-369        path = ta.SplineInterpolator(ss, path)
-370        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
-371        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
-372        instance = algo.TOPPRA(
-373            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
-374        )
-375        jnt_traj = instance.compute_trajectory()
-376        ts_sample = np.linspace(
-377            0, jnt_traj.duration, int(jnt_traj.duration / step)
-378        )
-379        qs_sample = jnt_traj(ts_sample)
-380        qds_sample = jnt_traj(ts_sample, 1)
-381        qdds_sample = jnt_traj(ts_sample, 2)
-382        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
-383
-384    def update_point_cloud(self, pc, radius=1e-3):
-385        """ 
-386        Args:
-387            pc: numpy array of shape (n, 3)
-388            radius: radius of each point. This gives a buffer around each point that planner will avoid
-389        """
-390        self.planning_world.update_point_cloud(pc, radius)
-391
-392    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
-393        """ helper function to update the attached tool """
-394        if link_id == -1:
-395            link_id = self.move_group_link_id
-396        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
-397
-398    def update_attached_sphere(self, radius, pose, link_id=-1):
-399        """
-400        attach a sphere to some link
-401
-402        Args:
-403            radius: radius of the sphere
-404            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
-405            link_id: if not provided, the end effector will be the target.
-406        """
-407        if link_id == -1:
-408            link_id = self.move_group_link_id
-409        self.planning_world.update_attached_sphere(radius, link_id, pose)
-410
-411    def update_attached_box(self, size, pose, link_id=-1):
-412        """
-413        attach a box to some link
-414
-415        Args:
-416            size: [x,y,z] size of the box
-417            pose: [x,y,z,qw,qx,qy,qz] pose of the box
-418            link_id: if not provided, the end effector will be the target.
-419        """
-420        if link_id == -1:
-421            link_id = self.move_group_link_id
-422        self.planning_world.update_attached_box(size, link_id, pose)
-423    
-424    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
-425        """
-426        attach a mesh to some link
-427
-428        Args:
-429            mesh_path: path to the mesh
-430            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
-431            link_id: if not provided, the end effector will be the target.
-432        """
-433        if link_id == -1:
-434            link_id = self.move_group_link_id
-435        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
-436
-437    def set_base_pose(self, pose):
-438        """
-439        tell the planner where the base of the robot is w.r.t the world frame
-440
-441        Args:
-442            pose: [x,y,z,qw,qx,qy,qz] pose of the base
-443        """
-444        self.robot.set_base_pose(pose)
-445
-446    def set_normal_object(self, name, collision_object):
-447        """ adds or updates a non-articulated collision object in the scene """
-448        self.planning_world.set_normal_object(name, collision_object)
-449    
-450    def remove_normal_object(self, name):
-451        """ returns true if the object was removed, false if it was not found """
-452        return self.planning_world.remove_normal_object(name)
-453
-454    def plan_qpos_to_qpos(
-455        self,
-456        goal_qposes: list,
-457        current_qpos,
-458        time_step=0.1,
-459        rrt_range=0.1,
-460        planning_time=1,
-461        fix_joint_limits=True,
-462        use_point_cloud=False,
-463        use_attach=False,
-464        verbose=False,
-465        planner_name="RRTConnect",
-466        no_simplification=False,
-467        constraint_function=None,
-468        constraint_jacobian=None,
-469        constraint_tolerance=1e-3,
-470        fixed_joint_indices=[]
-471    ):
-472        """
-473        plan a path from a specified joint position to a goal pose
-474
-475        Args:
-476            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
-477            current_qpos: current joint configuration (either full or move_group joints)
-478            mask: mask for IK. When set, the IK will leave certain joints out of planning
-479            time_step: time step for TOPP
-480            rrt_range: step size for RRT
-481            planning_time: time limit for RRT
-482            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-483            use_point_cloud: if True, will use the point cloud to avoid collision
-484            use_attach: if True, will consider the attached tool collision when planning
-485            verbose: if True, will print the log of OMPL and TOPPRA
-486            planner_name: planner name pick from {"RRTConnect", "RRT*"}
-487            fixed_joint_indices: list of indices of joints that are fixed during planning
-488            constraint_function: evals to 0 when constraint is satisfied
-489            constraint_jacobian: jacobian of constraint_function
-490            constraint_tolerance: tolerance for constraint_function
-491            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
-492        """
-493        self.planning_world.set_use_point_cloud(use_point_cloud)
-494        self.planning_world.set_use_attach(use_attach)
-495        n = current_qpos.shape[0]
-496        if fix_joint_limits:
-497            for i in range(n):
-498                if current_qpos[i] < self.joint_limits[i][0]:
-499                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-500                if current_qpos[i] > self.joint_limits[i][1]:
-501                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-502
-503        current_qpos = self.pad_qpos(current_qpos)
-504
-505        self.robot.set_qpos(current_qpos, True)
-506        collisions = self.planning_world.collide_full()
-507        if len(collisions) != 0:
-508            print("Invalid start state!")
-509            for collision in collisions:
-510                print("%s and %s collide!" % (collision.link_name1, collision.link_name2))
-511
-512        idx = self.move_group_joint_indices
-513        
-514        goal_qpos_ = []
-515        for i in range(len(goal_qposes)):
-516            goal_qpos_.append(goal_qposes[i][idx])
-517        
-518        fixed_joints = set()
-519        for joint_idx in fixed_joint_indices:
-520            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
-521
-522        assert len(current_qpos[idx]) == len(goal_qpos_[0])
-523        status, path = self.planner.plan(
-524            current_qpos[idx],
-525            goal_qpos_,
-526            range=rrt_range,
-527            time=planning_time,
-528            fixed_joints=fixed_joints,
-529            verbose=verbose,
-530            planner_name=planner_name,
-531            no_simplification=no_simplification,
-532            constraint_function=constraint_function,
-533            constraint_jacobian=constraint_jacobian,
-534            constraint_tolerance=constraint_tolerance
-535        )
-536
-537        if status == "Exact solution":
-538            if verbose: ta.setup_logging("INFO")
-539            else: ta.setup_logging("WARNING")
-540            times, pos, vel, acc, duration = self.TOPP(path, time_step)
-541            return {
-542                "status": "Success",
-543                "time": times,
-544                "position": pos,
-545                "velocity": vel,
-546                "acceleration": acc,
-547                "duration": duration,
-548            }
-549        else:
-550            return {"status": "RRT Failed. %s" % status}
-551
-552    def plan_qpos_to_pose(
-553        self,
-554        goal_pose,
-555        current_qpos,
-556        mask = [],
-557        time_step=0.1,
-558        rrt_range=0.1,
-559        planning_time=1,
-560        fix_joint_limits=True,
-561        use_point_cloud=False,
-562        use_attach=False,
-563        verbose=False,
-564        wrt_world=False,
-565        planner_name="RRTConnect",
-566        no_simplification=False,
-567        constraint_function=None,
-568        constraint_jacobian=None,
-569        constraint_tolerance=1e-3
-570    ):
-571        """
-572        plan from a start configuration to a goal pose of the end-effector
-573
-574        Args:
-575            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-576            current_qpos: current joint configuration (either full or move_group joints)
-577            mask: if the value at a given index is True, the joint is *not* used in the IK
-578            time_step: time step for TOPPRA (time parameterization of path)
-579            rrt_range: step size for RRT
-580            planning_time: time limit for RRT
-581            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-582            use_point_cloud: if True, will use the point cloud to avoid collision
-583            use_attach: if True, will consider the attached tool collision when planning
-584            verbose: if True, will print the log of OMPL and TOPPRA
-585            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
-586        """
-587        n = current_qpos.shape[0]
-588        if fix_joint_limits:
-589            for i in range(n):
-590                if current_qpos[i] < self.joint_limits[i][0]:
-591                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-592                if current_qpos[i] > self.joint_limits[i][1]:
-593                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-594
-595        if wrt_world:
-596            base_pose = self.robot.get_base_pose()
-597            base_tf = np.eye(4)
-598            base_tf[0:3, 3] = base_pose[:3]
-599            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
-600            goal_tf = np.eye(4)
-601            goal_tf[0:3, 3] = goal_pose[:3]
-602            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
-603            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
-604            goal_pose[:3] = goal_tf[0:3, 3]
-605            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
-606
-607        idx = self.move_group_joint_indices  # we need to take only the move_group joints when planning
-608
-609        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-610        if ik_status != "Success":
-611            return {"status": ik_status}
-612
-613        if verbose:
-614            print("IK results:")
-615            for i in range(len(goal_qpos)):
-616               print(goal_qpos[i])
-617
-618        goal_qpos_ = []
-619        for i in range(len(goal_qpos)):
-620            goal_qpos_.append(goal_qpos[i][idx])
-621        self.robot.set_qpos(current_qpos, True)
-622
-623        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-624        if ik_status != "Success":
-625            return {"status": ik_status}
-626
-627        if verbose:
-628            print("IK results:")
-629            for i in range(len(goal_qpos)):
-630               print(goal_qpos[i])
-631
-632        return self.plan_qpos_to_qpos(
-633            goal_qpos,
-634            current_qpos,
-635            time_step,
-636            rrt_range,
-637            planning_time,
-638            fix_joint_limits,
-639            use_point_cloud,
-640            use_attach,
-641            verbose,
-642            planner_name,
-643            no_simplification,
-644            constraint_function,
-645            constraint_jacobian,
-646            constraint_tolerance
-647        )
-648
-649    def plan_screw(
-650        self,
-651        target_pose,
-652        qpos,
-653        qpos_step=0.1,
-654        time_step=0.1,
-655        use_point_cloud=False,
-656        use_attach=False,
-657        verbose=False,
-658    ):
-659        """
-660        plan from a start configuration to a goal pose of the end-effector using screw motion
-661
-662        Args:
-663            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-664            qpos: current joint configuration (either full or move_group joints)
-665            qpos_step: size of the random step for RRT
-666            time_step: time step for the discretization
-667            use_point_cloud: if True, will use the point cloud to avoid collision
-668            use_attach: if True, will use the attached tool to avoid collision
-669            verbose: if True, will print the log of TOPPRA
-670        """
-671        self.planning_world.set_use_point_cloud(use_point_cloud)
-672        self.planning_world.set_use_attach(use_attach)
-673        qpos = self.pad_qpos(qpos.copy())
-674        self.robot.set_qpos(qpos, True)
-675
-676        def pose7D2mat(pose):
-677            mat = np.eye(4)
-678            mat[0:3, 3] = pose[:3]
-679            mat[0:3, 0:3] = quat2mat(pose[3:])
-680            return mat
-681
-682        def skew(vec):
-683            return np.array(
-684                [
-685                    [0, -vec[2], vec[1]],
-686                    [vec[2], 0, -vec[0]],
-687                    [-vec[1], vec[0], 0],
-688                ]
-689            )
-690
-691        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
-692            def rot2so3(rotation: np.ndarray):
-693                assert rotation.shape == (3, 3)
-694                if np.isclose(rotation.trace(), 3):
-695                    return np.zeros(3), 1
-696                if np.isclose(rotation.trace(), -1):
-697                    return np.zeros(3), -1e6
-698                theta = np.arccos((rotation.trace() - 1) / 2)
-699                omega = (
-700                    1
-701                    / 2
-702                    / np.sin(theta)
-703                    * np.array(
-704                        [
-705                            rotation[2, 1] - rotation[1, 2],
-706                            rotation[0, 2] - rotation[2, 0],
-707                            rotation[1, 0] - rotation[0, 1],
-708                        ]
-709                    ).T
-710                )
-711                return omega, theta
-712
-713            omega, theta = rot2so3(pose[:3, :3])
-714            if theta < -1e5:
-715                return omega, theta
-716            ss = skew(omega)
-717            inv_left_jacobian = (
-718                np.eye(3) / theta
-719                - 0.5 * ss
-720                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
-721            )
-722            v = inv_left_jacobian @ pose[:3, 3]
-723            return np.concatenate([v, omega]), theta
-724
-725        self.pinocchio_model.compute_forward_kinematics(qpos)
-726        ee_index = self.link_name_2_idx[self.move_group]
-727        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
-728        target_p = pose7D2mat(target_pose)
-729        relative_transform = target_p @ np.linalg.inv(current_p)
-730
-731        omega, theta = pose2exp_coordinate(relative_transform)
-732
-733        if theta < -1e4:
-734            return {"status": "screw plan failed."}
-735        omega = omega.reshape((-1, 1)) * theta
-736
-737        index = self.move_group_joint_indices
-738        path = [np.copy(qpos[index])]
-739
-740        while True:
-741            self.pinocchio_model.compute_full_jacobian(qpos)
-742            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
-743            delta_q = np.linalg.pinv(J) @ omega
-744            delta_q *= qpos_step / (np.linalg.norm(delta_q))
-745            delta_twist = J @ delta_q
-746
-747            flag = False
-748            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
-749                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
-750                delta_q = delta_q * ratio
-751                delta_twist = delta_twist * ratio
-752                flag = True
-753
-754            qpos += delta_q.reshape(-1)
-755            omega -= delta_twist
-756
-757            def check_joint_limit(q):
-758                n = len(q)
-759                for i in range(n):
-760                    if (
-761                        q[i] < self.joint_limits[i][0] - 1e-3
-762                        or q[i] > self.joint_limits[i][1] + 1e-3
-763                    ):
-764                        return False
-765                return True
-766
-767            within_joint_limit = check_joint_limit(qpos)
-768            self.planning_world.set_qpos_all(qpos[index])
-769            collide = self.planning_world.collide()
-770
-771            if (
-772                np.linalg.norm(delta_twist) < 1e-4
-773                or collide
-774                or within_joint_limit == False
-775            ):
-776                return {"status": "screw plan failed"}
-777
-778            path.append(np.copy(qpos[index]))
+364            if flag:
+365                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
+366                new_pose = self.pinocchio_model.get_link_pose(index)
+367                tmp_dis = self.distance_6D(
+368                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
+369                )
+370                if tmp_dis < min_dis:
+371                    min_dis = tmp_dis
+372                if tmp_dis < threshold:
+373                    result = ik_results[0]
+374                    unique = True
+375                    for j in range(len(results)):
+376                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
+377                            unique = False
+378                    if unique:
+379                        results.append(result)
+380            start_qpos = self.pinocchio_model.get_random_configuration()
+381            mask_len = len(mask)
+382            if mask_len > 0:
+383                for j in range(mask_len):
+384                    if mask[j]:
+385                        start_qpos[j] = qpos0[j]
+386        if len(results) != 0:
+387            status = "Success"
+388        elif min_dis != 1e9:
+389            status = "IK Failed! Distance %lf is greater than threshold %lf." % (
+390                min_dis,
+391                threshold,
+392            )
+393        else:
+394            status = "IK Failed! Cannot find valid solution."
+395        return status, results
+396
+397    def TOPP(self, path, step=0.1, verbose=False):
+398        """
+399        Time-Optimal Path Parameterization
+400
+401        Args:
+402            path: numpy array of shape (n, dof)
+403            step: step size for the discretization
+404            verbose: if True, will print the log of TOPPRA
+405        """
+406
+407        N_samples = path.shape[0]
+408        dof = path.shape[1]
+409        assert dof == len(self.joint_vel_limits)
+410        assert dof == len(self.joint_acc_limits)
+411        ss = np.linspace(0, 1, N_samples)
+412        path = ta.SplineInterpolator(ss, path)
+413        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
+414        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
+415        instance = algo.TOPPRA(
+416            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
+417        )
+418        jnt_traj = instance.compute_trajectory()
+419        ts_sample = np.linspace(0, jnt_traj.duration, int(jnt_traj.duration / step))
+420        qs_sample = jnt_traj(ts_sample)
+421        qds_sample = jnt_traj(ts_sample, 1)
+422        qdds_sample = jnt_traj(ts_sample, 2)
+423        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
+424
+425    def update_point_cloud(self, pc, radius=1e-3):
+426        """
+427        Args:
+428            pc: numpy array of shape (n, 3)
+429            radius: radius of each point. This gives a buffer around each point that planner will avoid
+430        """
+431        self.planning_world.update_point_cloud(pc, radius)
+432
+433    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
+434        """helper function to update the attached tool"""
+435        if link_id == -1:
+436            link_id = self.move_group_link_id
+437        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
+438
+439    def update_attached_sphere(self, radius, pose, link_id=-1):
+440        """
+441        attach a sphere to some link
+442
+443        Args:
+444            radius: radius of the sphere
+445            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
+446            link_id: if not provided, the end effector will be the target.
+447        """
+448        if link_id == -1:
+449            link_id = self.move_group_link_id
+450        self.planning_world.update_attached_sphere(radius, link_id, pose)
+451
+452    def update_attached_box(self, size, pose, link_id=-1):
+453        """
+454        attach a box to some link
+455
+456        Args:
+457            size: [x,y,z] size of the box
+458            pose: [x,y,z,qw,qx,qy,qz] pose of the box
+459            link_id: if not provided, the end effector will be the target.
+460        """
+461        if link_id == -1:
+462            link_id = self.move_group_link_id
+463        self.planning_world.update_attached_box(size, link_id, pose)
+464
+465    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
+466        """
+467        attach a mesh to some link
+468
+469        Args:
+470            mesh_path: path to the mesh
+471            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
+472            link_id: if not provided, the end effector will be the target.
+473        """
+474        if link_id == -1:
+475            link_id = self.move_group_link_id
+476        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
+477
+478    def set_base_pose(self, pose):
+479        """
+480        tell the planner where the base of the robot is w.r.t the world frame
+481
+482        Args:
+483            pose: [x,y,z,qw,qx,qy,qz] pose of the base
+484        """
+485        self.robot.set_base_pose(pose)
+486
+487    def set_normal_object(self, name, collision_object):
+488        """adds or updates a non-articulated collision object in the scene"""
+489        self.planning_world.set_normal_object(name, collision_object)
+490
+491    def remove_normal_object(self, name):
+492        """returns true if the object was removed, false if it was not found"""
+493        return self.planning_world.remove_normal_object(name)
+494
+495    def plan_qpos_to_qpos(
+496        self,
+497        goal_qposes: list,
+498        current_qpos,
+499        time_step=0.1,
+500        rrt_range=0.1,
+501        planning_time=1,
+502        fix_joint_limits=True,
+503        use_point_cloud=False,
+504        use_attach=False,
+505        verbose=False,
+506        planner_name="RRTConnect",
+507        no_simplification=False,
+508        constraint_function=None,
+509        constraint_jacobian=None,
+510        constraint_tolerance=1e-3,
+511        fixed_joint_indices=[],
+512    ):
+513        """
+514        plan a path from a specified joint position to a goal pose
+515
+516        Args:
+517            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
+518            current_qpos: current joint configuration (either full or move_group joints)
+519            mask: mask for IK. When set, the IK will leave certain joints out of planning
+520            time_step: time step for TOPP
+521            rrt_range: step size for RRT
+522            planning_time: time limit for RRT
+523            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+524            use_point_cloud: if True, will use the point cloud to avoid collision
+525            use_attach: if True, will consider the attached tool collision when planning
+526            verbose: if True, will print the log of OMPL and TOPPRA
+527            planner_name: planner name pick from {"RRTConnect", "RRT*"}
+528            fixed_joint_indices: list of indices of joints that are fixed during planning
+529            constraint_function: evals to 0 when constraint is satisfied
+530            constraint_jacobian: jacobian of constraint_function
+531            constraint_tolerance: tolerance for constraint_function
+532            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
+533        """
+534        self.planning_world.set_use_point_cloud(use_point_cloud)
+535        self.planning_world.set_use_attach(use_attach)
+536        n = current_qpos.shape[0]
+537        if fix_joint_limits:
+538            for i in range(n):
+539                if current_qpos[i] < self.joint_limits[i][0]:
+540                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+541                if current_qpos[i] > self.joint_limits[i][1]:
+542                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+543
+544        current_qpos = self.pad_qpos(current_qpos)
+545
+546        self.robot.set_qpos(current_qpos, True)
+547        collisions = self.planning_world.collide_full()
+548        if len(collisions) != 0:
+549            print("Invalid start state!")
+550            for collision in collisions:
+551                print(
+552                    "%s and %s collide!" % (collision.link_name1, collision.link_name2)
+553                )
+554
+555        idx = self.move_group_joint_indices
+556
+557        goal_qpos_ = []
+558        for i in range(len(goal_qposes)):
+559            goal_qpos_.append(goal_qposes[i][idx])
+560
+561        fixed_joints = set()
+562        for joint_idx in fixed_joint_indices:
+563            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
+564
+565        assert len(current_qpos[idx]) == len(goal_qpos_[0])
+566        status, path = self.planner.plan(
+567            current_qpos[idx],
+568            goal_qpos_,
+569            range=rrt_range,
+570            time=planning_time,
+571            fixed_joints=fixed_joints,
+572            verbose=verbose,
+573            planner_name=planner_name,
+574            no_simplification=no_simplification,
+575            constraint_function=constraint_function,
+576            constraint_jacobian=constraint_jacobian,
+577            constraint_tolerance=constraint_tolerance,
+578        )
+579
+580        if status == "Exact solution":
+581            if verbose:
+582                ta.setup_logging("INFO")
+583            else:
+584                ta.setup_logging("WARNING")
+585            times, pos, vel, acc, duration = self.TOPP(path, time_step)
+586            return {
+587                "status": "Success",
+588                "time": times,
+589                "position": pos,
+590                "velocity": vel,
+591                "acceleration": acc,
+592                "duration": duration,
+593            }
+594        else:
+595            return {"status": "RRT Failed. %s" % status}
+596
+597    def plan_qpos_to_pose(
+598        self,
+599        goal_pose,
+600        current_qpos,
+601        mask=[],
+602        time_step=0.1,
+603        rrt_range=0.1,
+604        planning_time=1,
+605        fix_joint_limits=True,
+606        use_point_cloud=False,
+607        use_attach=False,
+608        verbose=False,
+609        wrt_world=False,
+610        planner_name="RRTConnect",
+611        no_simplification=False,
+612        constraint_function=None,
+613        constraint_jacobian=None,
+614        constraint_tolerance=1e-3,
+615    ):
+616        """
+617        plan from a start configuration to a goal pose of the end-effector
+618
+619        Args:
+620            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+621            current_qpos: current joint configuration (either full or move_group joints)
+622            mask: if the value at a given index is True, the joint is *not* used in the IK
+623            time_step: time step for TOPPRA (time parameterization of path)
+624            rrt_range: step size for RRT
+625            planning_time: time limit for RRT
+626            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+627            use_point_cloud: if True, will use the point cloud to avoid collision
+628            use_attach: if True, will consider the attached tool collision when planning
+629            verbose: if True, will print the log of OMPL and TOPPRA
+630            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+631        """
+632        n = current_qpos.shape[0]
+633        if fix_joint_limits:
+634            for i in range(n):
+635                if current_qpos[i] < self.joint_limits[i][0]:
+636                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+637                if current_qpos[i] > self.joint_limits[i][1]:
+638                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+639
+640        if wrt_world:
+641            base_pose = self.robot.get_base_pose()
+642            base_tf = np.eye(4)
+643            base_tf[0:3, 3] = base_pose[:3]
+644            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
+645            goal_tf = np.eye(4)
+646            goal_tf[0:3, 3] = goal_pose[:3]
+647            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
+648            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
+649            goal_pose[:3] = goal_tf[0:3, 3]
+650            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
+651
+652        idx = (
+653            self.move_group_joint_indices
+654        )  # we need to take only the move_group joints when planning
+655
+656        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+657        if ik_status != "Success":
+658            return {"status": ik_status}
+659
+660        if verbose:
+661            print("IK results:")
+662            for i in range(len(goal_qpos)):
+663                print(goal_qpos[i])
+664
+665        goal_qpos_ = []
+666        for i in range(len(goal_qpos)):
+667            goal_qpos_.append(goal_qpos[i][idx])
+668        self.robot.set_qpos(current_qpos, True)
+669
+670        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+671        if ik_status != "Success":
+672            return {"status": ik_status}
+673
+674        if verbose:
+675            print("IK results:")
+676            for i in range(len(goal_qpos)):
+677                print(goal_qpos[i])
+678
+679        return self.plan_qpos_to_qpos(
+680            goal_qpos,
+681            current_qpos,
+682            time_step,
+683            rrt_range,
+684            planning_time,
+685            fix_joint_limits,
+686            use_point_cloud,
+687            use_attach,
+688            verbose,
+689            planner_name,
+690            no_simplification,
+691            constraint_function,
+692            constraint_jacobian,
+693            constraint_tolerance,
+694        )
+695
+696    def plan_screw(
+697        self,
+698        target_pose,
+699        qpos,
+700        qpos_step=0.1,
+701        time_step=0.1,
+702        use_point_cloud=False,
+703        use_attach=False,
+704        verbose=False,
+705    ):
+706        """
+707        plan from a start configuration to a goal pose of the end-effector using screw motion
+708
+709        Args:
+710            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+711            qpos: current joint configuration (either full or move_group joints)
+712            qpos_step: size of the random step for RRT
+713            time_step: time step for the discretization
+714            use_point_cloud: if True, will use the point cloud to avoid collision
+715            use_attach: if True, will use the attached tool to avoid collision
+716            verbose: if True, will print the log of TOPPRA
+717        """
+718        self.planning_world.set_use_point_cloud(use_point_cloud)
+719        self.planning_world.set_use_attach(use_attach)
+720        qpos = self.pad_qpos(qpos.copy())
+721        self.robot.set_qpos(qpos, True)
+722
+723        def pose7D2mat(pose):
+724            mat = np.eye(4)
+725            mat[0:3, 3] = pose[:3]
+726            mat[0:3, 0:3] = quat2mat(pose[3:])
+727            return mat
+728
+729        def skew(vec):
+730            return np.array([
+731                [0, -vec[2], vec[1]],
+732                [vec[2], 0, -vec[0]],
+733                [-vec[1], vec[0], 0],
+734            ])
+735
+736        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
+737            def rot2so3(rotation: np.ndarray):
+738                assert rotation.shape == (3, 3)
+739                if np.isclose(rotation.trace(), 3):
+740                    return np.zeros(3), 1
+741                if np.isclose(rotation.trace(), -1):
+742                    return np.zeros(3), -1e6
+743                theta = np.arccos((rotation.trace() - 1) / 2)
+744                omega = (
+745                    1
+746                    / 2
+747                    / np.sin(theta)
+748                    * np.array([
+749                        rotation[2, 1] - rotation[1, 2],
+750                        rotation[0, 2] - rotation[2, 0],
+751                        rotation[1, 0] - rotation[0, 1],
+752                    ]).T
+753                )
+754                return omega, theta
+755
+756            omega, theta = rot2so3(pose[:3, :3])
+757            if theta < -1e5:
+758                return omega, theta
+759            ss = skew(omega)
+760            inv_left_jacobian = (
+761                np.eye(3) / theta
+762                - 0.5 * ss
+763                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
+764            )
+765            v = inv_left_jacobian @ pose[:3, 3]
+766            return np.concatenate([v, omega]), theta
+767
+768        self.pinocchio_model.compute_forward_kinematics(qpos)
+769        ee_index = self.link_name_2_idx[self.move_group]
+770        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
+771        target_p = pose7D2mat(target_pose)
+772        relative_transform = target_p @ np.linalg.inv(current_p)
+773
+774        omega, theta = pose2exp_coordinate(relative_transform)
+775
+776        if theta < -1e4:
+777            return {"status": "screw plan failed."}
+778        omega = omega.reshape((-1, 1)) * theta
 779
-780            if flag:
-781                if verbose:
-782                    ta.setup_logging("INFO")
-783                else:
-784                    ta.setup_logging("WARNING")
-785                times, pos, vel, acc, duration = self.TOPP(
-786                    np.vstack(path), time_step
-787                )
-788                return {
-789                    "status": "Success",
-790                    "time": times,
-791                    "position": pos,
-792                    "velocity": vel,
-793                    "acceleration": acc,
-794                    "duration": duration,
-795                }
+780        index = self.move_group_joint_indices
+781        path = [np.copy(qpos[index])]
+782
+783        while True:
+784            self.pinocchio_model.compute_full_jacobian(qpos)
+785            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
+786            delta_q = np.linalg.pinv(J) @ omega
+787            delta_q *= qpos_step / (np.linalg.norm(delta_q))
+788            delta_twist = J @ delta_q
+789
+790            flag = False
+791            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
+792                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
+793                delta_q = delta_q * ratio
+794                delta_twist = delta_twist * ratio
+795                flag = True
+796
+797            qpos += delta_q.reshape(-1)
+798            omega -= delta_twist
+799
+800            def check_joint_limit(q):
+801                n = len(q)
+802                for i in range(n):
+803                    if (
+804                        q[i] < self.joint_limits[i][0] - 1e-3
+805                        or q[i] > self.joint_limits[i][1] + 1e-3
+806                    ):
+807                        return False
+808                return True
+809
+810            within_joint_limit = check_joint_limit(qpos)
+811            self.planning_world.set_qpos_all(qpos[index])
+812            collide = self.planning_world.collide()
+813
+814            if (
+815                np.linalg.norm(delta_twist) < 1e-4
+816                or collide
+817                or within_joint_limit == False
+818            ):
+819                return {"status": "screw plan failed"}
+820
+821            path.append(np.copy(qpos[index]))
+822
+823            if flag:
+824                if verbose:
+825                    ta.setup_logging("INFO")
+826                else:
+827                    ta.setup_logging("WARNING")
+828                times, pos, vel, acc, duration = self.TOPP(np.vstack(path), time_step)
+829                return {
+830                    "status": "Success",
+831                    "time": times,
+832                    "position": pos,
+833                    "velocity": vel,
+834                    "acceleration": acc,
+835                    "duration": duration,
+836                }
 
    
 
 
@@ -997,7 +1038,7 @@ 
    
  27        user_joint_names: Sequence[str] = [],
  28        joint_vel_limits: Union[Sequence[float], np.ndarray] = [],
  29        joint_acc_limits: Union[Sequence[float], np.ndarray] = [],
- 30        **kwargs
+ 30        **kwargs,
  31    ):
  32        r"""Motion planner for robots.
  33
@@ -1019,7 +1060,7 @@ 
    
  49        if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
  50            self.srdf = urdf.replace(".urdf", ".srdf")
  51            print(f"No SRDF file provided but found {self.srdf}")
- 52            
+ 52
  53        # replace package:// keyword if exists
  54        urdf = self.replace_package_keyword(package_keyword_replacement)
  55
@@ -1038,7 +1079,7 @@ 
    
  68            [self.robot],
  69            ["robot"],
  70            kwargs.get("normal_objects", []),
- 71            kwargs.get("normal_object_names", [])
+ 71            kwargs.get("normal_object_names", []),
  72        )
  73
  74        if srdf == "":
@@ -1056,714 +1097,755 @@ 
    
  86        for i, link in enumerate(self.user_link_names):
  87            self.link_name_2_idx[link] = i
  88
- 89        assert move_group in self.user_link_names,\
- 90            f"end-effector not found as one of the links in {self.user_link_names}"
- 91        self.move_group = move_group
- 92        self.robot.set_move_group(self.move_group)
- 93        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
- 94
- 95        self.joint_types = self.pinocchio_model.get_joint_types()
- 96        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
- 97        self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices))
- 98        self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices))
- 99        self.move_group_link_id = self.link_name_2_idx[self.move_group]
-100        assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\
-101            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-102            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
-103        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\
-104            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-105            f"length of move_group ({len(self.move_group_joint_indices)})"
-106        assert len(self.joint_vel_limits) <= len(self.joint_limits),\
-107            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\
-108            f"number of total joints ({len(self.joint_limits)})"
-109        
-110        self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], [])
-111        self.planner = ompl.OMPLPlanner(world=self.planning_world)
-112
-113
-114    def replace_package_keyword(self, package_keyword_replacement):
-115        """
-116        some ROS URDF files use package:// keyword to refer the package dir
-117        replace it with the given string (default is empty)
-118
-119        Args:
-120            package_keyword_replacement: the string to replace 'package://' keyword
-121        """
-122        rtn_urdf = self.urdf
-123        with open(self.urdf, "r") as in_f:
-124            content = in_f.read()
-125            if "package://" in content:
-126                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
-127                content = content.replace("package://", package_keyword_replacement)
-128                if not os.path.exists(rtn_urdf):
-129                    with open(rtn_urdf, "w") as out_f:
-130                        out_f.write(content)
-131        return rtn_urdf
-132
-133    def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000):
-134        """
-135        we read the srdf file to get the link pairs that should not collide.
-136        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
-137        """
-138        print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.")
-139        n_link = len(self.user_link_names)
-140        cnt = np.zeros((n_link, n_link), dtype=np.int32)
-141        for i in range(sample_time):
-142            qpos = self.pinocchio_model.get_random_configuration()
-143            self.robot.set_qpos(qpos, True)
-144            collisions = self.planning_world.collide_full()
-145            for collision in collisions:
-146                u = self.link_name_2_idx[collision.link_name1]
-147                v = self.link_name_2_idx[collision.link_name2]
-148                cnt[u][v] += 1
-149            if i % echo_freq == 0:
-150                print("Finish %.1f%%!" % (i * 100 / sample_time))
-151        
-152        import xml.etree.ElementTree as ET
-153        from xml.dom import minidom
-154
-155        root = ET.Element('robot')
-156        robot_name = self.urdf.split('/')[-1].split('.')[0]
-157        root.set('name', robot_name)
-158        self.srdf = self.urdf.replace(".urdf", ".srdf")
-159
-160        for i in range(n_link):
-161            for j in range(n_link):
-162                if cnt[i][j] == sample_time:
-163                    link1 = self.user_link_names[i]
-164                    link2 = self.user_link_names[j]
-165                    print("Ignore collision pair: (%s, %s), reason:  always collide" % (link1, link2))
-166                    collision = ET.SubElement(root, 'disable_collisions')
-167                    collision.set('link1', link1)
-168                    collision.set('link2', link2)
-169                    collision.set('reason', 'Default')
-170        srdffile = open(self.srdf, "w")
-171        srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent="    "))
-172        srdffile.close()
-173        print("Saving the SRDF file to %s" % self.srdf)
-174
-175    def distance_6D(self, p1, q1, p2, q2):
-176        """
-177        compute the distance between two poses
-178        
-179        Args:
-180            p1: position of pose 1
-181            q1: quaternion of pose 1
-182            p2: position of pose 2
-183            q2: quaternion of pose 2
-184        """
-185        return np.linalg.norm(p1 - p2) + min(
-186            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
-187        )
-188
-189    def check_joint_limit(self, q):
-190        """
-191        check if the joint configuration is within the joint limits
-192
-193        Args:
-194            q: joint configuration
-195        
-196        Returns:
-197            True if the joint configuration is within the joint limits
-198        """
-199        n = len(q)
-200        flag = True
-201        for i in range(n):
-202            if self.joint_types[i].startswith("JointModelR"):
-203                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
-204                    continue
-205                q[i] -= (
-206                    2
-207                    * np.pi
-208                    * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
-209                )
-210                if q[i] > self.joint_limits[i][1] + 1e-3:
-211                    flag = False
-212            else:
-213                if (
-214                    q[i] < self.joint_limits[i][0] - 1e-3
-215                    or q[i] > self.joint_limits[i][1] + 1e-3
-216                ):
-217                    flag = False
-218        return flag
-219
-220    def pad_qpos(self, qpos, articulation=None):
-221        """
-222        if the user does not provide the full qpos but only the move_group joints,
-223        pad the qpos with the rest of the joints
-224        """
-225        if len(qpos) == len(self.move_group_joint_indices):
-226            tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos()
-227            tmp[:len(qpos)] = qpos
-228            qpos = tmp
-229
-230        assert len(qpos) == len(self.joint_limits),\
-231            f"length of qpos ({len(qpos)}) =/= "\
-232            f"number of total joints ({len(self.joint_limits)})"
-233
-234        return qpos
-235
-236    def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-237        """ helper function to check for collision """
-238        # handle no user input
-239        if articulation is None:
-240            articulation = self.robot
-241        if qpos is None:
-242            qpos = articulation.get_qpos()
-243        qpos = self.pad_qpos(qpos, articulation)
-244
-245        # first save the current qpos
-246        old_qpos = articulation.get_qpos()
-247        # set robot to new qpos
-248        articulation.set_qpos(qpos, True)
-249        # find the index of the articulation inside the array
-250        idx = self.planning_world.get_articulations().index(articulation)
-251        # check for self-collision
-252        collisions = collision_function(idx)
-253        # reset qpos
-254        articulation.set_qpos(old_qpos, True)
-255        return collisions
-256
-257    def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-258        """Check if the robot is in self-collision.
+ 89        assert (
+ 90            move_group in self.user_link_names
+ 91        ), f"end-effector not found as one of the links in {self.user_link_names}"
+ 92        self.move_group = move_group
+ 93        self.robot.set_move_group(self.move_group)
+ 94        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
+ 95
+ 96        self.joint_types = self.pinocchio_model.get_joint_types()
+ 97        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
+ 98        self.joint_vel_limits = (
+ 99            joint_vel_limits
+100            if len(joint_vel_limits)
+101            else np.ones(len(self.move_group_joint_indices))
+102        )
+103        self.joint_acc_limits = (
+104            joint_acc_limits
+105            if len(joint_acc_limits)
+106            else np.ones(len(self.move_group_joint_indices))
+107        )
+108        self.move_group_link_id = self.link_name_2_idx[self.move_group]
+109        assert len(self.joint_vel_limits) == len(self.joint_acc_limits), (
+110            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+111            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
+112        )
+113        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices), (
+114            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+115            f"length of move_group ({len(self.move_group_joint_indices)})"
+116        )
+117        assert len(self.joint_vel_limits) <= len(self.joint_limits), (
+118            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "
+119            f"number of total joints ({len(self.joint_limits)})"
+120        )
+121
+122        self.planning_world = planning_world.PlanningWorld(
+123            [self.robot], ["robot"], [], []
+124        )
+125        self.planner = ompl.OMPLPlanner(world=self.planning_world)
+126
+127    def replace_package_keyword(self, package_keyword_replacement):
+128        """
+129        some ROS URDF files use package:// keyword to refer the package dir
+130        replace it with the given string (default is empty)
+131
+132        Args:
+133            package_keyword_replacement: the string to replace 'package://' keyword
+134        """
+135        rtn_urdf = self.urdf
+136        with open(self.urdf, "r") as in_f:
+137            content = in_f.read()
+138            if "package://" in content:
+139                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
+140                content = content.replace("package://", package_keyword_replacement)
+141                if not os.path.exists(rtn_urdf):
+142                    with open(rtn_urdf, "w") as out_f:
+143                        out_f.write(content)
+144        return rtn_urdf
+145
+146    def generate_collision_pair(self, sample_time=1000000, echo_freq=100000):
+147        """
+148        we read the srdf file to get the link pairs that should not collide.
+149        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
+150        """
+151        print(
+152            "Since no SRDF file is provided. We will first detect link pairs that will"
+153            " always collide. This may take several minutes."
+154        )
+155        n_link = len(self.user_link_names)
+156        cnt = np.zeros((n_link, n_link), dtype=np.int32)
+157        for i in range(sample_time):
+158            qpos = self.pinocchio_model.get_random_configuration()
+159            self.robot.set_qpos(qpos, True)
+160            collisions = self.planning_world.collide_full()
+161            for collision in collisions:
+162                u = self.link_name_2_idx[collision.link_name1]
+163                v = self.link_name_2_idx[collision.link_name2]
+164                cnt[u][v] += 1
+165            if i % echo_freq == 0:
+166                print("Finish %.1f%%!" % (i * 100 / sample_time))
+167
+168        import xml.etree.ElementTree as ET
+169        from xml.dom import minidom
+170
+171        root = ET.Element("robot")
+172        robot_name = self.urdf.split("/")[-1].split(".")[0]
+173        root.set("name", robot_name)
+174        self.srdf = self.urdf.replace(".urdf", ".srdf")
+175
+176        for i in range(n_link):
+177            for j in range(n_link):
+178                if cnt[i][j] == sample_time:
+179                    link1 = self.user_link_names[i]
+180                    link2 = self.user_link_names[j]
+181                    print(
+182                        "Ignore collision pair: (%s, %s), reason:  always collide"
+183                        % (link1, link2)
+184                    )
+185                    collision = ET.SubElement(root, "disable_collisions")
+186                    collision.set("link1", link1)
+187                    collision.set("link2", link2)
+188                    collision.set("reason", "Default")
+189        srdffile = open(self.srdf, "w")
+190        srdffile.write(
+191            minidom.parseString(ET.tostring(root)).toprettyxml(indent="    ")
+192        )
+193        srdffile.close()
+194        print("Saving the SRDF file to %s" % self.srdf)
+195
+196    def distance_6D(self, p1, q1, p2, q2):
+197        """
+198        compute the distance between two poses
+199
+200        Args:
+201            p1: position of pose 1
+202            q1: quaternion of pose 1
+203            p2: position of pose 2
+204            q2: quaternion of pose 2
+205        """
+206        return np.linalg.norm(p1 - p2) + min(
+207            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
+208        )
+209
+210    def check_joint_limit(self, q):
+211        """
+212        check if the joint configuration is within the joint limits
+213
+214        Args:
+215            q: joint configuration
+216
+217        Returns:
+218            True if the joint configuration is within the joint limits
+219        """
+220        n = len(q)
+221        flag = True
+222        for i in range(n):
+223            if self.joint_types[i].startswith("JointModelR"):
+224                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
+225                    continue
+226                q[i] -= (
+227                    2 * np.pi * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
+228                )
+229                if q[i] > self.joint_limits[i][1] + 1e-3:
+230                    flag = False
+231            else:
+232                if (
+233                    q[i] < self.joint_limits[i][0] - 1e-3
+234                    or q[i] > self.joint_limits[i][1] + 1e-3
+235                ):
+236                    flag = False
+237        return flag
+238
+239    def pad_qpos(self, qpos, articulation=None):
+240        """
+241        if the user does not provide the full qpos but only the move_group joints,
+242        pad the qpos with the rest of the joints
+243        """
+244        if len(qpos) == len(self.move_group_joint_indices):
+245            tmp = (
+246                articulation.get_qpos()
+247                if articulation is not None
+248                else self.robot.get_qpos()
+249            )
+250            tmp[: len(qpos)] = qpos
+251            qpos = tmp
+252
+253        assert len(qpos) == len(self.joint_limits), (
+254            f"length of qpos ({len(qpos)}) =/= "
+255            f"number of total joints ({len(self.joint_limits)})"
+256        )
+257
+258        return qpos
 259
-260        Args:
-261            articulation: robot model. if none will be self.robot
-262            qpos: robot configuration. if none will be the current pose
-263
-264        Returns:
-265            A list of collisions.
-266        """
-267        return self.check_for_collision(self.planning_world.self_collide, articulation, qpos)
-268
-269    def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list:
-270        """Check if the robot is in collision with the environment
-271
-272        Args:
-273            articulation: robot model. if none will be self.robot
-274            qpos: robot configuration. if none will be the current pose
-275            with_point_cloud: whether to check collision against point cloud
-276            use_attach: whether to include the object attached to the end effector in collision checking
-277        Returns:
-278            A list of collisions.
-279        """
-280        # store previous results
-281        prev_use_point_cloud = self.planning_world.use_point_cloud
-282        prev_use_attach = self.planning_world.use_attach
-283        self.planning_world.set_use_point_cloud(with_point_cloud)
-284        self.planning_world.set_use_attach(use_attach)
+260    def check_for_collision(
+261        self,
+262        collision_function,
+263        articulation: articulation.ArticulatedModel = None,
+264        qpos: np.ndarray = None,
+265    ) -> list:
+266        """helper function to check for collision"""
+267        # handle no user input
+268        if articulation is None:
+269            articulation = self.robot
+270        if qpos is None:
+271            qpos = articulation.get_qpos()
+272        qpos = self.pad_qpos(qpos, articulation)
+273
+274        # first save the current qpos
+275        old_qpos = articulation.get_qpos()
+276        # set robot to new qpos
+277        articulation.set_qpos(qpos, True)
+278        # find the index of the articulation inside the array
+279        idx = self.planning_world.get_articulations().index(articulation)
+280        # check for self-collision
+281        collisions = collision_function(idx)
+282        # reset qpos
+283        articulation.set_qpos(old_qpos, True)
+284        return collisions
 285
-286        results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos)
-287
-288        # restore
-289        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
-290        self.planning_world.set_use_attach(prev_use_attach)
-291        return results
+286    def check_for_self_collision(
+287        self,
+288        articulation: articulation.ArticulatedModel = None,
+289        qpos: np.ndarray = None,
+290    ) -> list:
+291        """Check if the robot is in self-collision.
 292
-293    def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3):
-294        """
-295        Inverse kinematics
+293        Args:
+294            articulation: robot model. if none will be self.robot
+295            qpos: robot configuration. if none will be the current pose
 296
-297        Args:
-298            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-299            start_qpos: initial configuration
-300            mask: if the value at a given index is True, the joint is *not* used in the IK
-301            n_init_qpos: number of random initial configurations
-302            threshold: threshold for the distance between the goal pose and the result pose
-303        """
-304
-305        index = self.link_name_2_idx[self.move_group]
-306        min_dis = 1e9
-307        idx = self.move_group_joint_indices
-308        qpos0 = np.copy(start_qpos)
-309        results = []
-310        self.robot.set_qpos(start_qpos, True)
-311        for i in range(n_init_qpos):
-312            ik_results = self.pinocchio_model.compute_IK_CLIK(
-313                index, goal_pose, start_qpos, mask
-314            )
-315            flag = self.check_joint_limit(ik_results[0]) # will clip qpos
-316
-317            # check collision
-318            self.planning_world.set_qpos_all(ik_results[0][idx])
-319            if (len(self.planning_world.collide_full()) != 0):
-320                flag = False
-321
-322            if flag:
-323                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
-324                new_pose = self.pinocchio_model.get_link_pose(index)
-325                tmp_dis = self.distance_6D(
-326                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
-327                )
-328                if tmp_dis < min_dis:
-329                    min_dis = tmp_dis
-330                if tmp_dis < threshold:
-331                    result = ik_results[0] 
-332                    unique = True
-333                    for j in range(len(results)):
-334                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
-335                            unique = False
-336                    if unique:
-337                        results.append(result)
-338            start_qpos = self.pinocchio_model.get_random_configuration()
-339            mask_len = len(mask)
-340            if mask_len > 0:
-341                for j in range(mask_len):
-342                    if mask[j]:
-343                        start_qpos[j] = qpos0[j]
-344        if len(results) != 0:
-345            status = "Success"
-346        elif min_dis != 1e9:
-347            status = (
-348                "IK Failed! Distance %lf is greater than threshold %lf."
-349                % (min_dis, threshold)
-350            )
-351        else:
-352            status = "IK Failed! Cannot find valid solution."
-353        return status, results
-354
-355    def TOPP(self, path, step=0.1, verbose=False):
-356        """
-357        Time-Optimal Path Parameterization
-358
-359        Args:
-360            path: numpy array of shape (n, dof)
-361            step: step size for the discretization
-362            verbose: if True, will print the log of TOPPRA
-363        """
+297        Returns:
+298            A list of collisions.
+299        """
+300        return self.check_for_collision(
+301            self.planning_world.self_collide, articulation, qpos
+302        )
+303
+304    def check_for_env_collision(
+305        self,
+306        articulation: articulation.ArticulatedModel = None,
+307        qpos: np.ndarray = None,
+308        with_point_cloud=False,
+309        use_attach=False,
+310    ) -> list:
+311        """Check if the robot is in collision with the environment
+312
+313        Args:
+314            articulation: robot model. if none will be self.robot
+315            qpos: robot configuration. if none will be the current pose
+316            with_point_cloud: whether to check collision against point cloud
+317            use_attach: whether to include the object attached to the end effector in collision checking
+318        Returns:
+319            A list of collisions.
+320        """
+321        # store previous results
+322        prev_use_point_cloud = self.planning_world.use_point_cloud
+323        prev_use_attach = self.planning_world.use_attach
+324        self.planning_world.set_use_point_cloud(with_point_cloud)
+325        self.planning_world.set_use_attach(use_attach)
+326
+327        results = self.check_for_collision(
+328            self.planning_world.collide_with_others, articulation, qpos
+329        )
+330
+331        # restore
+332        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
+333        self.planning_world.set_use_attach(prev_use_attach)
+334        return results
+335
+336    def IK(self, goal_pose, start_qpos, mask=[], n_init_qpos=20, threshold=1e-3):
+337        """
+338        Inverse kinematics
+339
+340        Args:
+341            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+342            start_qpos: initial configuration
+343            mask: if the value at a given index is True, the joint is *not* used in the IK
+344            n_init_qpos: number of random initial configurations
+345            threshold: threshold for the distance between the goal pose and the result pose
+346        """
+347
+348        index = self.link_name_2_idx[self.move_group]
+349        min_dis = 1e9
+350        idx = self.move_group_joint_indices
+351        qpos0 = np.copy(start_qpos)
+352        results = []
+353        self.robot.set_qpos(start_qpos, True)
+354        for i in range(n_init_qpos):
+355            ik_results = self.pinocchio_model.compute_IK_CLIK(
+356                index, goal_pose, start_qpos, mask
+357            )
+358            flag = self.check_joint_limit(ik_results[0])  # will clip qpos
+359
+360            # check collision
+361            self.planning_world.set_qpos_all(ik_results[0][idx])
+362            if len(self.planning_world.collide_full()) != 0:
+363                flag = False
 364
-365        N_samples = path.shape[0]
-366        dof = path.shape[1]
-367        assert dof == len(self.joint_vel_limits)
-368        assert dof == len(self.joint_acc_limits)
-369        ss = np.linspace(0, 1, N_samples)
-370        path = ta.SplineInterpolator(ss, path)
-371        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
-372        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
-373        instance = algo.TOPPRA(
-374            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
-375        )
-376        jnt_traj = instance.compute_trajectory()
-377        ts_sample = np.linspace(
-378            0, jnt_traj.duration, int(jnt_traj.duration / step)
-379        )
-380        qs_sample = jnt_traj(ts_sample)
-381        qds_sample = jnt_traj(ts_sample, 1)
-382        qdds_sample = jnt_traj(ts_sample, 2)
-383        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
-384
-385    def update_point_cloud(self, pc, radius=1e-3):
-386        """ 
-387        Args:
-388            pc: numpy array of shape (n, 3)
-389            radius: radius of each point. This gives a buffer around each point that planner will avoid
-390        """
-391        self.planning_world.update_point_cloud(pc, radius)
-392
-393    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
-394        """ helper function to update the attached tool """
-395        if link_id == -1:
-396            link_id = self.move_group_link_id
-397        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
-398
-399    def update_attached_sphere(self, radius, pose, link_id=-1):
-400        """
-401        attach a sphere to some link
-402
-403        Args:
-404            radius: radius of the sphere
-405            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
-406            link_id: if not provided, the end effector will be the target.
-407        """
-408        if link_id == -1:
-409            link_id = self.move_group_link_id
-410        self.planning_world.update_attached_sphere(radius, link_id, pose)
-411
-412    def update_attached_box(self, size, pose, link_id=-1):
-413        """
-414        attach a box to some link
-415
-416        Args:
-417            size: [x,y,z] size of the box
-418            pose: [x,y,z,qw,qx,qy,qz] pose of the box
-419            link_id: if not provided, the end effector will be the target.
-420        """
-421        if link_id == -1:
-422            link_id = self.move_group_link_id
-423        self.planning_world.update_attached_box(size, link_id, pose)
-424    
-425    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
-426        """
-427        attach a mesh to some link
-428
-429        Args:
-430            mesh_path: path to the mesh
-431            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
-432            link_id: if not provided, the end effector will be the target.
-433        """
-434        if link_id == -1:
-435            link_id = self.move_group_link_id
-436        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
-437
-438    def set_base_pose(self, pose):
-439        """
-440        tell the planner where the base of the robot is w.r.t the world frame
-441
-442        Args:
-443            pose: [x,y,z,qw,qx,qy,qz] pose of the base
-444        """
-445        self.robot.set_base_pose(pose)
-446
-447    def set_normal_object(self, name, collision_object):
-448        """ adds or updates a non-articulated collision object in the scene """
-449        self.planning_world.set_normal_object(name, collision_object)
-450    
-451    def remove_normal_object(self, name):
-452        """ returns true if the object was removed, false if it was not found """
-453        return self.planning_world.remove_normal_object(name)
-454
-455    def plan_qpos_to_qpos(
-456        self,
-457        goal_qposes: list,
-458        current_qpos,
-459        time_step=0.1,
-460        rrt_range=0.1,
-461        planning_time=1,
-462        fix_joint_limits=True,
-463        use_point_cloud=False,
-464        use_attach=False,
-465        verbose=False,
-466        planner_name="RRTConnect",
-467        no_simplification=False,
-468        constraint_function=None,
-469        constraint_jacobian=None,
-470        constraint_tolerance=1e-3,
-471        fixed_joint_indices=[]
-472    ):
-473        """
-474        plan a path from a specified joint position to a goal pose
-475
-476        Args:
-477            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
-478            current_qpos: current joint configuration (either full or move_group joints)
-479            mask: mask for IK. When set, the IK will leave certain joints out of planning
-480            time_step: time step for TOPP
-481            rrt_range: step size for RRT
-482            planning_time: time limit for RRT
-483            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-484            use_point_cloud: if True, will use the point cloud to avoid collision
-485            use_attach: if True, will consider the attached tool collision when planning
-486            verbose: if True, will print the log of OMPL and TOPPRA
-487            planner_name: planner name pick from {"RRTConnect", "RRT*"}
-488            fixed_joint_indices: list of indices of joints that are fixed during planning
-489            constraint_function: evals to 0 when constraint is satisfied
-490            constraint_jacobian: jacobian of constraint_function
-491            constraint_tolerance: tolerance for constraint_function
-492            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
-493        """
-494        self.planning_world.set_use_point_cloud(use_point_cloud)
-495        self.planning_world.set_use_attach(use_attach)
-496        n = current_qpos.shape[0]
-497        if fix_joint_limits:
-498            for i in range(n):
-499                if current_qpos[i] < self.joint_limits[i][0]:
-500                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-501                if current_qpos[i] > self.joint_limits[i][1]:
-502                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-503
-504        current_qpos = self.pad_qpos(current_qpos)
-505
-506        self.robot.set_qpos(current_qpos, True)
-507        collisions = self.planning_world.collide_full()
-508        if len(collisions) != 0:
-509            print("Invalid start state!")
-510            for collision in collisions:
-511                print("%s and %s collide!" % (collision.link_name1, collision.link_name2))
-512
-513        idx = self.move_group_joint_indices
-514        
-515        goal_qpos_ = []
-516        for i in range(len(goal_qposes)):
-517            goal_qpos_.append(goal_qposes[i][idx])
-518        
-519        fixed_joints = set()
-520        for joint_idx in fixed_joint_indices:
-521            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
-522
-523        assert len(current_qpos[idx]) == len(goal_qpos_[0])
-524        status, path = self.planner.plan(
-525            current_qpos[idx],
-526            goal_qpos_,
-527            range=rrt_range,
-528            time=planning_time,
-529            fixed_joints=fixed_joints,
-530            verbose=verbose,
-531            planner_name=planner_name,
-532            no_simplification=no_simplification,
-533            constraint_function=constraint_function,
-534            constraint_jacobian=constraint_jacobian,
-535            constraint_tolerance=constraint_tolerance
-536        )
-537
-538        if status == "Exact solution":
-539            if verbose: ta.setup_logging("INFO")
-540            else: ta.setup_logging("WARNING")
-541            times, pos, vel, acc, duration = self.TOPP(path, time_step)
-542            return {
-543                "status": "Success",
-544                "time": times,
-545                "position": pos,
-546                "velocity": vel,
-547                "acceleration": acc,
-548                "duration": duration,
-549            }
-550        else:
-551            return {"status": "RRT Failed. %s" % status}
-552
-553    def plan_qpos_to_pose(
-554        self,
-555        goal_pose,
-556        current_qpos,
-557        mask = [],
-558        time_step=0.1,
-559        rrt_range=0.1,
-560        planning_time=1,
-561        fix_joint_limits=True,
-562        use_point_cloud=False,
-563        use_attach=False,
-564        verbose=False,
-565        wrt_world=False,
-566        planner_name="RRTConnect",
-567        no_simplification=False,
-568        constraint_function=None,
-569        constraint_jacobian=None,
-570        constraint_tolerance=1e-3
-571    ):
-572        """
-573        plan from a start configuration to a goal pose of the end-effector
-574
-575        Args:
-576            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-577            current_qpos: current joint configuration (either full or move_group joints)
-578            mask: if the value at a given index is True, the joint is *not* used in the IK
-579            time_step: time step for TOPPRA (time parameterization of path)
-580            rrt_range: step size for RRT
-581            planning_time: time limit for RRT
-582            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-583            use_point_cloud: if True, will use the point cloud to avoid collision
-584            use_attach: if True, will consider the attached tool collision when planning
-585            verbose: if True, will print the log of OMPL and TOPPRA
-586            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
-587        """
-588        n = current_qpos.shape[0]
-589        if fix_joint_limits:
-590            for i in range(n):
-591                if current_qpos[i] < self.joint_limits[i][0]:
-592                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-593                if current_qpos[i] > self.joint_limits[i][1]:
-594                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-595
-596        if wrt_world:
-597            base_pose = self.robot.get_base_pose()
-598            base_tf = np.eye(4)
-599            base_tf[0:3, 3] = base_pose[:3]
-600            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
-601            goal_tf = np.eye(4)
-602            goal_tf[0:3, 3] = goal_pose[:3]
-603            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
-604            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
-605            goal_pose[:3] = goal_tf[0:3, 3]
-606            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
-607
-608        idx = self.move_group_joint_indices  # we need to take only the move_group joints when planning
-609
-610        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-611        if ik_status != "Success":
-612            return {"status": ik_status}
-613
-614        if verbose:
-615            print("IK results:")
-616            for i in range(len(goal_qpos)):
-617               print(goal_qpos[i])
-618
-619        goal_qpos_ = []
-620        for i in range(len(goal_qpos)):
-621            goal_qpos_.append(goal_qpos[i][idx])
-622        self.robot.set_qpos(current_qpos, True)
-623
-624        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-625        if ik_status != "Success":
-626            return {"status": ik_status}
-627
-628        if verbose:
-629            print("IK results:")
-630            for i in range(len(goal_qpos)):
-631               print(goal_qpos[i])
-632
-633        return self.plan_qpos_to_qpos(
-634            goal_qpos,
-635            current_qpos,
-636            time_step,
-637            rrt_range,
-638            planning_time,
-639            fix_joint_limits,
-640            use_point_cloud,
-641            use_attach,
-642            verbose,
-643            planner_name,
-644            no_simplification,
-645            constraint_function,
-646            constraint_jacobian,
-647            constraint_tolerance
-648        )
-649
-650    def plan_screw(
-651        self,
-652        target_pose,
-653        qpos,
-654        qpos_step=0.1,
-655        time_step=0.1,
-656        use_point_cloud=False,
-657        use_attach=False,
-658        verbose=False,
-659    ):
-660        """
-661        plan from a start configuration to a goal pose of the end-effector using screw motion
-662
-663        Args:
-664            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-665            qpos: current joint configuration (either full or move_group joints)
-666            qpos_step: size of the random step for RRT
-667            time_step: time step for the discretization
-668            use_point_cloud: if True, will use the point cloud to avoid collision
-669            use_attach: if True, will use the attached tool to avoid collision
-670            verbose: if True, will print the log of TOPPRA
-671        """
-672        self.planning_world.set_use_point_cloud(use_point_cloud)
-673        self.planning_world.set_use_attach(use_attach)
-674        qpos = self.pad_qpos(qpos.copy())
-675        self.robot.set_qpos(qpos, True)
-676
-677        def pose7D2mat(pose):
-678            mat = np.eye(4)
-679            mat[0:3, 3] = pose[:3]
-680            mat[0:3, 0:3] = quat2mat(pose[3:])
-681            return mat
-682
-683        def skew(vec):
-684            return np.array(
-685                [
-686                    [0, -vec[2], vec[1]],
-687                    [vec[2], 0, -vec[0]],
-688                    [-vec[1], vec[0], 0],
-689                ]
-690            )
-691
-692        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
-693            def rot2so3(rotation: np.ndarray):
-694                assert rotation.shape == (3, 3)
-695                if np.isclose(rotation.trace(), 3):
-696                    return np.zeros(3), 1
-697                if np.isclose(rotation.trace(), -1):
-698                    return np.zeros(3), -1e6
-699                theta = np.arccos((rotation.trace() - 1) / 2)
-700                omega = (
-701                    1
-702                    / 2
-703                    / np.sin(theta)
-704                    * np.array(
-705                        [
-706                            rotation[2, 1] - rotation[1, 2],
-707                            rotation[0, 2] - rotation[2, 0],
-708                            rotation[1, 0] - rotation[0, 1],
-709                        ]
-710                    ).T
-711                )
-712                return omega, theta
-713
-714            omega, theta = rot2so3(pose[:3, :3])
-715            if theta < -1e5:
-716                return omega, theta
-717            ss = skew(omega)
-718            inv_left_jacobian = (
-719                np.eye(3) / theta
-720                - 0.5 * ss
-721                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
-722            )
-723            v = inv_left_jacobian @ pose[:3, 3]
-724            return np.concatenate([v, omega]), theta
-725
-726        self.pinocchio_model.compute_forward_kinematics(qpos)
-727        ee_index = self.link_name_2_idx[self.move_group]
-728        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
-729        target_p = pose7D2mat(target_pose)
-730        relative_transform = target_p @ np.linalg.inv(current_p)
-731
-732        omega, theta = pose2exp_coordinate(relative_transform)
-733
-734        if theta < -1e4:
-735            return {"status": "screw plan failed."}
-736        omega = omega.reshape((-1, 1)) * theta
-737
-738        index = self.move_group_joint_indices
-739        path = [np.copy(qpos[index])]
-740
-741        while True:
-742            self.pinocchio_model.compute_full_jacobian(qpos)
-743            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
-744            delta_q = np.linalg.pinv(J) @ omega
-745            delta_q *= qpos_step / (np.linalg.norm(delta_q))
-746            delta_twist = J @ delta_q
-747
-748            flag = False
-749            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
-750                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
-751                delta_q = delta_q * ratio
-752                delta_twist = delta_twist * ratio
-753                flag = True
-754
-755            qpos += delta_q.reshape(-1)
-756            omega -= delta_twist
-757
-758            def check_joint_limit(q):
-759                n = len(q)
-760                for i in range(n):
-761                    if (
-762                        q[i] < self.joint_limits[i][0] - 1e-3
-763                        or q[i] > self.joint_limits[i][1] + 1e-3
-764                    ):
-765                        return False
-766                return True
-767
-768            within_joint_limit = check_joint_limit(qpos)
-769            self.planning_world.set_qpos_all(qpos[index])
-770            collide = self.planning_world.collide()
-771
-772            if (
-773                np.linalg.norm(delta_twist) < 1e-4
-774                or collide
-775                or within_joint_limit == False
-776            ):
-777                return {"status": "screw plan failed"}
-778
-779            path.append(np.copy(qpos[index]))
+365            if flag:
+366                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
+367                new_pose = self.pinocchio_model.get_link_pose(index)
+368                tmp_dis = self.distance_6D(
+369                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
+370                )
+371                if tmp_dis < min_dis:
+372                    min_dis = tmp_dis
+373                if tmp_dis < threshold:
+374                    result = ik_results[0]
+375                    unique = True
+376                    for j in range(len(results)):
+377                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
+378                            unique = False
+379                    if unique:
+380                        results.append(result)
+381            start_qpos = self.pinocchio_model.get_random_configuration()
+382            mask_len = len(mask)
+383            if mask_len > 0:
+384                for j in range(mask_len):
+385                    if mask[j]:
+386                        start_qpos[j] = qpos0[j]
+387        if len(results) != 0:
+388            status = "Success"
+389        elif min_dis != 1e9:
+390            status = "IK Failed! Distance %lf is greater than threshold %lf." % (
+391                min_dis,
+392                threshold,
+393            )
+394        else:
+395            status = "IK Failed! Cannot find valid solution."
+396        return status, results
+397
+398    def TOPP(self, path, step=0.1, verbose=False):
+399        """
+400        Time-Optimal Path Parameterization
+401
+402        Args:
+403            path: numpy array of shape (n, dof)
+404            step: step size for the discretization
+405            verbose: if True, will print the log of TOPPRA
+406        """
+407
+408        N_samples = path.shape[0]
+409        dof = path.shape[1]
+410        assert dof == len(self.joint_vel_limits)
+411        assert dof == len(self.joint_acc_limits)
+412        ss = np.linspace(0, 1, N_samples)
+413        path = ta.SplineInterpolator(ss, path)
+414        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
+415        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
+416        instance = algo.TOPPRA(
+417            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
+418        )
+419        jnt_traj = instance.compute_trajectory()
+420        ts_sample = np.linspace(0, jnt_traj.duration, int(jnt_traj.duration / step))
+421        qs_sample = jnt_traj(ts_sample)
+422        qds_sample = jnt_traj(ts_sample, 1)
+423        qdds_sample = jnt_traj(ts_sample, 2)
+424        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
+425
+426    def update_point_cloud(self, pc, radius=1e-3):
+427        """
+428        Args:
+429            pc: numpy array of shape (n, 3)
+430            radius: radius of each point. This gives a buffer around each point that planner will avoid
+431        """
+432        self.planning_world.update_point_cloud(pc, radius)
+433
+434    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
+435        """helper function to update the attached tool"""
+436        if link_id == -1:
+437            link_id = self.move_group_link_id
+438        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
+439
+440    def update_attached_sphere(self, radius, pose, link_id=-1):
+441        """
+442        attach a sphere to some link
+443
+444        Args:
+445            radius: radius of the sphere
+446            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
+447            link_id: if not provided, the end effector will be the target.
+448        """
+449        if link_id == -1:
+450            link_id = self.move_group_link_id
+451        self.planning_world.update_attached_sphere(radius, link_id, pose)
+452
+453    def update_attached_box(self, size, pose, link_id=-1):
+454        """
+455        attach a box to some link
+456
+457        Args:
+458            size: [x,y,z] size of the box
+459            pose: [x,y,z,qw,qx,qy,qz] pose of the box
+460            link_id: if not provided, the end effector will be the target.
+461        """
+462        if link_id == -1:
+463            link_id = self.move_group_link_id
+464        self.planning_world.update_attached_box(size, link_id, pose)
+465
+466    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
+467        """
+468        attach a mesh to some link
+469
+470        Args:
+471            mesh_path: path to the mesh
+472            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
+473            link_id: if not provided, the end effector will be the target.
+474        """
+475        if link_id == -1:
+476            link_id = self.move_group_link_id
+477        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
+478
+479    def set_base_pose(self, pose):
+480        """
+481        tell the planner where the base of the robot is w.r.t the world frame
+482
+483        Args:
+484            pose: [x,y,z,qw,qx,qy,qz] pose of the base
+485        """
+486        self.robot.set_base_pose(pose)
+487
+488    def set_normal_object(self, name, collision_object):
+489        """adds or updates a non-articulated collision object in the scene"""
+490        self.planning_world.set_normal_object(name, collision_object)
+491
+492    def remove_normal_object(self, name):
+493        """returns true if the object was removed, false if it was not found"""
+494        return self.planning_world.remove_normal_object(name)
+495
+496    def plan_qpos_to_qpos(
+497        self,
+498        goal_qposes: list,
+499        current_qpos,
+500        time_step=0.1,
+501        rrt_range=0.1,
+502        planning_time=1,
+503        fix_joint_limits=True,
+504        use_point_cloud=False,
+505        use_attach=False,
+506        verbose=False,
+507        planner_name="RRTConnect",
+508        no_simplification=False,
+509        constraint_function=None,
+510        constraint_jacobian=None,
+511        constraint_tolerance=1e-3,
+512        fixed_joint_indices=[],
+513    ):
+514        """
+515        plan a path from a specified joint position to a goal pose
+516
+517        Args:
+518            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
+519            current_qpos: current joint configuration (either full or move_group joints)
+520            mask: mask for IK. When set, the IK will leave certain joints out of planning
+521            time_step: time step for TOPP
+522            rrt_range: step size for RRT
+523            planning_time: time limit for RRT
+524            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+525            use_point_cloud: if True, will use the point cloud to avoid collision
+526            use_attach: if True, will consider the attached tool collision when planning
+527            verbose: if True, will print the log of OMPL and TOPPRA
+528            planner_name: planner name pick from {"RRTConnect", "RRT*"}
+529            fixed_joint_indices: list of indices of joints that are fixed during planning
+530            constraint_function: evals to 0 when constraint is satisfied
+531            constraint_jacobian: jacobian of constraint_function
+532            constraint_tolerance: tolerance for constraint_function
+533            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
+534        """
+535        self.planning_world.set_use_point_cloud(use_point_cloud)
+536        self.planning_world.set_use_attach(use_attach)
+537        n = current_qpos.shape[0]
+538        if fix_joint_limits:
+539            for i in range(n):
+540                if current_qpos[i] < self.joint_limits[i][0]:
+541                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+542                if current_qpos[i] > self.joint_limits[i][1]:
+543                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+544
+545        current_qpos = self.pad_qpos(current_qpos)
+546
+547        self.robot.set_qpos(current_qpos, True)
+548        collisions = self.planning_world.collide_full()
+549        if len(collisions) != 0:
+550            print("Invalid start state!")
+551            for collision in collisions:
+552                print(
+553                    "%s and %s collide!" % (collision.link_name1, collision.link_name2)
+554                )
+555
+556        idx = self.move_group_joint_indices
+557
+558        goal_qpos_ = []
+559        for i in range(len(goal_qposes)):
+560            goal_qpos_.append(goal_qposes[i][idx])
+561
+562        fixed_joints = set()
+563        for joint_idx in fixed_joint_indices:
+564            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
+565
+566        assert len(current_qpos[idx]) == len(goal_qpos_[0])
+567        status, path = self.planner.plan(
+568            current_qpos[idx],
+569            goal_qpos_,
+570            range=rrt_range,
+571            time=planning_time,
+572            fixed_joints=fixed_joints,
+573            verbose=verbose,
+574            planner_name=planner_name,
+575            no_simplification=no_simplification,
+576            constraint_function=constraint_function,
+577            constraint_jacobian=constraint_jacobian,
+578            constraint_tolerance=constraint_tolerance,
+579        )
+580
+581        if status == "Exact solution":
+582            if verbose:
+583                ta.setup_logging("INFO")
+584            else:
+585                ta.setup_logging("WARNING")
+586            times, pos, vel, acc, duration = self.TOPP(path, time_step)
+587            return {
+588                "status": "Success",
+589                "time": times,
+590                "position": pos,
+591                "velocity": vel,
+592                "acceleration": acc,
+593                "duration": duration,
+594            }
+595        else:
+596            return {"status": "RRT Failed. %s" % status}
+597
+598    def plan_qpos_to_pose(
+599        self,
+600        goal_pose,
+601        current_qpos,
+602        mask=[],
+603        time_step=0.1,
+604        rrt_range=0.1,
+605        planning_time=1,
+606        fix_joint_limits=True,
+607        use_point_cloud=False,
+608        use_attach=False,
+609        verbose=False,
+610        wrt_world=False,
+611        planner_name="RRTConnect",
+612        no_simplification=False,
+613        constraint_function=None,
+614        constraint_jacobian=None,
+615        constraint_tolerance=1e-3,
+616    ):
+617        """
+618        plan from a start configuration to a goal pose of the end-effector
+619
+620        Args:
+621            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+622            current_qpos: current joint configuration (either full or move_group joints)
+623            mask: if the value at a given index is True, the joint is *not* used in the IK
+624            time_step: time step for TOPPRA (time parameterization of path)
+625            rrt_range: step size for RRT
+626            planning_time: time limit for RRT
+627            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+628            use_point_cloud: if True, will use the point cloud to avoid collision
+629            use_attach: if True, will consider the attached tool collision when planning
+630            verbose: if True, will print the log of OMPL and TOPPRA
+631            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+632        """
+633        n = current_qpos.shape[0]
+634        if fix_joint_limits:
+635            for i in range(n):
+636                if current_qpos[i] < self.joint_limits[i][0]:
+637                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+638                if current_qpos[i] > self.joint_limits[i][1]:
+639                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+640
+641        if wrt_world:
+642            base_pose = self.robot.get_base_pose()
+643            base_tf = np.eye(4)
+644            base_tf[0:3, 3] = base_pose[:3]
+645            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
+646            goal_tf = np.eye(4)
+647            goal_tf[0:3, 3] = goal_pose[:3]
+648            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
+649            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
+650            goal_pose[:3] = goal_tf[0:3, 3]
+651            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
+652
+653        idx = (
+654            self.move_group_joint_indices
+655        )  # we need to take only the move_group joints when planning
+656
+657        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+658        if ik_status != "Success":
+659            return {"status": ik_status}
+660
+661        if verbose:
+662            print("IK results:")
+663            for i in range(len(goal_qpos)):
+664                print(goal_qpos[i])
+665
+666        goal_qpos_ = []
+667        for i in range(len(goal_qpos)):
+668            goal_qpos_.append(goal_qpos[i][idx])
+669        self.robot.set_qpos(current_qpos, True)
+670
+671        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+672        if ik_status != "Success":
+673            return {"status": ik_status}
+674
+675        if verbose:
+676            print("IK results:")
+677            for i in range(len(goal_qpos)):
+678                print(goal_qpos[i])
+679
+680        return self.plan_qpos_to_qpos(
+681            goal_qpos,
+682            current_qpos,
+683            time_step,
+684            rrt_range,
+685            planning_time,
+686            fix_joint_limits,
+687            use_point_cloud,
+688            use_attach,
+689            verbose,
+690            planner_name,
+691            no_simplification,
+692            constraint_function,
+693            constraint_jacobian,
+694            constraint_tolerance,
+695        )
+696
+697    def plan_screw(
+698        self,
+699        target_pose,
+700        qpos,
+701        qpos_step=0.1,
+702        time_step=0.1,
+703        use_point_cloud=False,
+704        use_attach=False,
+705        verbose=False,
+706    ):
+707        """
+708        plan from a start configuration to a goal pose of the end-effector using screw motion
+709
+710        Args:
+711            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+712            qpos: current joint configuration (either full or move_group joints)
+713            qpos_step: size of the random step for RRT
+714            time_step: time step for the discretization
+715            use_point_cloud: if True, will use the point cloud to avoid collision
+716            use_attach: if True, will use the attached tool to avoid collision
+717            verbose: if True, will print the log of TOPPRA
+718        """
+719        self.planning_world.set_use_point_cloud(use_point_cloud)
+720        self.planning_world.set_use_attach(use_attach)
+721        qpos = self.pad_qpos(qpos.copy())
+722        self.robot.set_qpos(qpos, True)
+723
+724        def pose7D2mat(pose):
+725            mat = np.eye(4)
+726            mat[0:3, 3] = pose[:3]
+727            mat[0:3, 0:3] = quat2mat(pose[3:])
+728            return mat
+729
+730        def skew(vec):
+731            return np.array([
+732                [0, -vec[2], vec[1]],
+733                [vec[2], 0, -vec[0]],
+734                [-vec[1], vec[0], 0],
+735            ])
+736
+737        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
+738            def rot2so3(rotation: np.ndarray):
+739                assert rotation.shape == (3, 3)
+740                if np.isclose(rotation.trace(), 3):
+741                    return np.zeros(3), 1
+742                if np.isclose(rotation.trace(), -1):
+743                    return np.zeros(3), -1e6
+744                theta = np.arccos((rotation.trace() - 1) / 2)
+745                omega = (
+746                    1
+747                    / 2
+748                    / np.sin(theta)
+749                    * np.array([
+750                        rotation[2, 1] - rotation[1, 2],
+751                        rotation[0, 2] - rotation[2, 0],
+752                        rotation[1, 0] - rotation[0, 1],
+753                    ]).T
+754                )
+755                return omega, theta
+756
+757            omega, theta = rot2so3(pose[:3, :3])
+758            if theta < -1e5:
+759                return omega, theta
+760            ss = skew(omega)
+761            inv_left_jacobian = (
+762                np.eye(3) / theta
+763                - 0.5 * ss
+764                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
+765            )
+766            v = inv_left_jacobian @ pose[:3, 3]
+767            return np.concatenate([v, omega]), theta
+768
+769        self.pinocchio_model.compute_forward_kinematics(qpos)
+770        ee_index = self.link_name_2_idx[self.move_group]
+771        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
+772        target_p = pose7D2mat(target_pose)
+773        relative_transform = target_p @ np.linalg.inv(current_p)
+774
+775        omega, theta = pose2exp_coordinate(relative_transform)
+776
+777        if theta < -1e4:
+778            return {"status": "screw plan failed."}
+779        omega = omega.reshape((-1, 1)) * theta
 780
-781            if flag:
-782                if verbose:
-783                    ta.setup_logging("INFO")
-784                else:
-785                    ta.setup_logging("WARNING")
-786                times, pos, vel, acc, duration = self.TOPP(
-787                    np.vstack(path), time_step
-788                )
-789                return {
-790                    "status": "Success",
-791                    "time": times,
-792                    "position": pos,
-793                    "velocity": vel,
-794                    "acceleration": acc,
-795                    "duration": duration,
-796                }
+781        index = self.move_group_joint_indices
+782        path = [np.copy(qpos[index])]
+783
+784        while True:
+785            self.pinocchio_model.compute_full_jacobian(qpos)
+786            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
+787            delta_q = np.linalg.pinv(J) @ omega
+788            delta_q *= qpos_step / (np.linalg.norm(delta_q))
+789            delta_twist = J @ delta_q
+790
+791            flag = False
+792            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
+793                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
+794                delta_q = delta_q * ratio
+795                delta_twist = delta_twist * ratio
+796                flag = True
+797
+798            qpos += delta_q.reshape(-1)
+799            omega -= delta_twist
+800
+801            def check_joint_limit(q):
+802                n = len(q)
+803                for i in range(n):
+804                    if (
+805                        q[i] < self.joint_limits[i][0] - 1e-3
+806                        or q[i] > self.joint_limits[i][1] + 1e-3
+807                    ):
+808                        return False
+809                return True
+810
+811            within_joint_limit = check_joint_limit(qpos)
+812            self.planning_world.set_qpos_all(qpos[index])
+813            collide = self.planning_world.collide()
+814
+815            if (
+816                np.linalg.norm(delta_twist) < 1e-4
+817                or collide
+818                or within_joint_limit == False
+819            ):
+820                return {"status": "screw plan failed"}
+821
+822            path.append(np.copy(qpos[index]))
+823
+824            if flag:
+825                if verbose:
+826                    ta.setup_logging("INFO")
+827                else:
+828                    ta.setup_logging("WARNING")
+829                times, pos, vel, acc, duration = self.TOPP(np.vstack(path), time_step)
+830                return {
+831                    "status": "Success",
+832                    "time": times,
+833                    "position": pos,
+834                    "velocity": vel,
+835                    "acceleration": acc,
+836                    "duration": duration,
+837                }
 
    
 
 
@@ -1791,7 +1873,7 @@ 
    
  27        user_joint_names: Sequence[str] = [],
  28        joint_vel_limits: Union[Sequence[float], np.ndarray] = [],
  29        joint_acc_limits: Union[Sequence[float], np.ndarray] = [],
- 30        **kwargs
+ 30        **kwargs,
  31    ):
  32        r"""Motion planner for robots.
  33
@@ -1813,7 +1895,7 @@ 
    
  49        if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
  50            self.srdf = urdf.replace(".urdf", ".srdf")
  51            print(f"No SRDF file provided but found {self.srdf}")
- 52            
+ 52
  53        # replace package:// keyword if exists
  54        urdf = self.replace_package_keyword(package_keyword_replacement)
  55
@@ -1832,7 +1914,7 @@ 
    
  68            [self.robot],
  69            ["robot"],
  70            kwargs.get("normal_objects", []),
- 71            kwargs.get("normal_object_names", [])
+ 71            kwargs.get("normal_object_names", []),
  72        )
  73
  74        if srdf == "":
@@ -1850,46 +1932,61 @@ 
    
  86        for i, link in enumerate(self.user_link_names):
  87            self.link_name_2_idx[link] = i
  88
- 89        assert move_group in self.user_link_names,\
- 90            f"end-effector not found as one of the links in {self.user_link_names}"
- 91        self.move_group = move_group
- 92        self.robot.set_move_group(self.move_group)
- 93        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
- 94
- 95        self.joint_types = self.pinocchio_model.get_joint_types()
- 96        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
- 97        self.joint_vel_limits = joint_vel_limits if len(joint_vel_limits) else np.ones(len(self.move_group_joint_indices))
- 98        self.joint_acc_limits = joint_acc_limits if len(joint_acc_limits) else np.ones(len(self.move_group_joint_indices))
- 99        self.move_group_link_id = self.link_name_2_idx[self.move_group]
-100        assert len(self.joint_vel_limits) == len(self.joint_acc_limits),\
-101            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-102            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
-103        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices),\
-104            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "\
-105            f"length of move_group ({len(self.move_group_joint_indices)})"
-106        assert len(self.joint_vel_limits) <= len(self.joint_limits),\
-107            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "\
-108            f"number of total joints ({len(self.joint_limits)})"
-109        
-110        self.planning_world = planning_world.PlanningWorld([self.robot], ["robot"], [], [])
-111        self.planner = ompl.OMPLPlanner(world=self.planning_world)
+ 89        assert (
+ 90            move_group in self.user_link_names
+ 91        ), f"end-effector not found as one of the links in {self.user_link_names}"
+ 92        self.move_group = move_group
+ 93        self.robot.set_move_group(self.move_group)
+ 94        self.move_group_joint_indices = self.robot.get_move_group_joint_indices()
+ 95
+ 96        self.joint_types = self.pinocchio_model.get_joint_types()
+ 97        self.joint_limits = np.concatenate(self.pinocchio_model.get_joint_limits())
+ 98        self.joint_vel_limits = (
+ 99            joint_vel_limits
+100            if len(joint_vel_limits)
+101            else np.ones(len(self.move_group_joint_indices))
+102        )
+103        self.joint_acc_limits = (
+104            joint_acc_limits
+105            if len(joint_acc_limits)
+106            else np.ones(len(self.move_group_joint_indices))
+107        )
+108        self.move_group_link_id = self.link_name_2_idx[self.move_group]
+109        assert len(self.joint_vel_limits) == len(self.joint_acc_limits), (
+110            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+111            f"length of joint_acc_limits ({len(self.joint_acc_limits)})"
+112        )
+113        assert len(self.joint_vel_limits) == len(self.move_group_joint_indices), (
+114            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) =/= "
+115            f"length of move_group ({len(self.move_group_joint_indices)})"
+116        )
+117        assert len(self.joint_vel_limits) <= len(self.joint_limits), (
+118            f"length of joint_vel_limits ({len(self.joint_vel_limits)}) > "
+119            f"number of total joints ({len(self.joint_limits)})"
+120        )
+121
+122        self.planning_world = planning_world.PlanningWorld(
+123            [self.robot], ["robot"], [], []
+124        )
+125        self.planner = ompl.OMPLPlanner(world=self.planning_world)
 
    
 
 
             
    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.
-    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
-    joint_acc_limits: maximum joint accelerations for time parameterization,
-        which should have the same length as
-    srdf: Semantic Robot Description Format file.
-References:
-    http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
    
+
        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.
+        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
+        joint_acc_limits: maximum joint accelerations for time parameterization,
+            which should have the same length as
+        srdf: Semantic Robot Description Format file.
+    References:
+        http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
+
    
 
    
 
 
@@ -2081,24 +2178,24 @@ 
    
 
     
    
     
-            
    114    def replace_package_keyword(self, package_keyword_replacement):
-115        """
-116        some ROS URDF files use package:// keyword to refer the package dir
-117        replace it with the given string (default is empty)
-118
-119        Args:
-120            package_keyword_replacement: the string to replace 'package://' keyword
-121        """
-122        rtn_urdf = self.urdf
-123        with open(self.urdf, "r") as in_f:
-124            content = in_f.read()
-125            if "package://" in content:
-126                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
-127                content = content.replace("package://", package_keyword_replacement)
-128                if not os.path.exists(rtn_urdf):
-129                    with open(rtn_urdf, "w") as out_f:
-130                        out_f.write(content)
-131        return rtn_urdf
+            
    127    def replace_package_keyword(self, package_keyword_replacement):
+128        """
+129        some ROS URDF files use package:// keyword to refer the package dir
+130        replace it with the given string (default is empty)
+131
+132        Args:
+133            package_keyword_replacement: the string to replace 'package://' keyword
+134        """
+135        rtn_urdf = self.urdf
+136        with open(self.urdf, "r") as in_f:
+137            content = in_f.read()
+138            if "package://" in content:
+139                rtn_urdf = self.urdf.replace(".urdf", "_package_keyword_replaced.urdf")
+140                content = content.replace("package://", package_keyword_replacement)
+141                if not os.path.exists(rtn_urdf):
+142                    with open(rtn_urdf, "w") as out_f:
+143                        out_f.write(content)
+144        return rtn_urdf
 
    
 
 
@@ -2122,47 +2219,55 @@ 
    
 
     
    
     
-            
    133    def generate_collision_pair(self, sample_time = 1000000, echo_freq = 100000):
-134        """
-135        we read the srdf file to get the link pairs that should not collide.
-136        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
-137        """
-138        print("Since no SRDF file is provided. We will first detect link pairs that will always collide. This may take several minutes.")
-139        n_link = len(self.user_link_names)
-140        cnt = np.zeros((n_link, n_link), dtype=np.int32)
-141        for i in range(sample_time):
-142            qpos = self.pinocchio_model.get_random_configuration()
-143            self.robot.set_qpos(qpos, True)
-144            collisions = self.planning_world.collide_full()
-145            for collision in collisions:
-146                u = self.link_name_2_idx[collision.link_name1]
-147                v = self.link_name_2_idx[collision.link_name2]
-148                cnt[u][v] += 1
-149            if i % echo_freq == 0:
-150                print("Finish %.1f%%!" % (i * 100 / sample_time))
-151        
-152        import xml.etree.ElementTree as ET
-153        from xml.dom import minidom
-154
-155        root = ET.Element('robot')
-156        robot_name = self.urdf.split('/')[-1].split('.')[0]
-157        root.set('name', robot_name)
-158        self.srdf = self.urdf.replace(".urdf", ".srdf")
-159
-160        for i in range(n_link):
-161            for j in range(n_link):
-162                if cnt[i][j] == sample_time:
-163                    link1 = self.user_link_names[i]
-164                    link2 = self.user_link_names[j]
-165                    print("Ignore collision pair: (%s, %s), reason:  always collide" % (link1, link2))
-166                    collision = ET.SubElement(root, 'disable_collisions')
-167                    collision.set('link1', link1)
-168                    collision.set('link2', link2)
-169                    collision.set('reason', 'Default')
-170        srdffile = open(self.srdf, "w")
-171        srdffile.write(minidom.parseString(ET.tostring(root)).toprettyxml(indent="    "))
-172        srdffile.close()
-173        print("Saving the SRDF file to %s" % self.srdf)
+            
    146    def generate_collision_pair(self, sample_time=1000000, echo_freq=100000):
+147        """
+148        we read the srdf file to get the link pairs that should not collide.
+149        if not provided, we need to randomly sample configurations to find the link pairs that will always collide.
+150        """
+151        print(
+152            "Since no SRDF file is provided. We will first detect link pairs that will"
+153            " always collide. This may take several minutes."
+154        )
+155        n_link = len(self.user_link_names)
+156        cnt = np.zeros((n_link, n_link), dtype=np.int32)
+157        for i in range(sample_time):
+158            qpos = self.pinocchio_model.get_random_configuration()
+159            self.robot.set_qpos(qpos, True)
+160            collisions = self.planning_world.collide_full()
+161            for collision in collisions:
+162                u = self.link_name_2_idx[collision.link_name1]
+163                v = self.link_name_2_idx[collision.link_name2]
+164                cnt[u][v] += 1
+165            if i % echo_freq == 0:
+166                print("Finish %.1f%%!" % (i * 100 / sample_time))
+167
+168        import xml.etree.ElementTree as ET
+169        from xml.dom import minidom
+170
+171        root = ET.Element("robot")
+172        robot_name = self.urdf.split("/")[-1].split(".")[0]
+173        root.set("name", robot_name)
+174        self.srdf = self.urdf.replace(".urdf", ".srdf")
+175
+176        for i in range(n_link):
+177            for j in range(n_link):
+178                if cnt[i][j] == sample_time:
+179                    link1 = self.user_link_names[i]
+180                    link2 = self.user_link_names[j]
+181                    print(
+182                        "Ignore collision pair: (%s, %s), reason:  always collide"
+183                        % (link1, link2)
+184                    )
+185                    collision = ET.SubElement(root, "disable_collisions")
+186                    collision.set("link1", link1)
+187                    collision.set("link2", link2)
+188                    collision.set("reason", "Default")
+189        srdffile = open(self.srdf, "w")
+190        srdffile.write(
+191            minidom.parseString(ET.tostring(root)).toprettyxml(indent="    ")
+192        )
+193        srdffile.close()
+194        print("Saving the SRDF file to %s" % self.srdf)
 
    
 
 
@@ -2183,19 +2288,19 @@ 
    
 
     
    
     
-            
    175    def distance_6D(self, p1, q1, p2, q2):
-176        """
-177        compute the distance between two poses
-178        
-179        Args:
-180            p1: position of pose 1
-181            q1: quaternion of pose 1
-182            p2: position of pose 2
-183            q2: quaternion of pose 2
-184        """
-185        return np.linalg.norm(p1 - p2) + min(
-186            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
-187        )
+            
    196    def distance_6D(self, p1, q1, p2, q2):
+197        """
+198        compute the distance between two poses
+199
+200        Args:
+201            p1: position of pose 1
+202            q1: quaternion of pose 1
+203            p2: position of pose 2
+204            q2: quaternion of pose 2
+205        """
+206        return np.linalg.norm(p1 - p2) + min(
+207            np.linalg.norm(q1 - q2), np.linalg.norm(q1 + q2)
+208        )
 
    
 
 
@@ -2221,36 +2326,34 @@ 
    
 
     
    
     
-            
    189    def check_joint_limit(self, q):
-190        """
-191        check if the joint configuration is within the joint limits
-192
-193        Args:
-194            q: joint configuration
-195        
-196        Returns:
-197            True if the joint configuration is within the joint limits
-198        """
-199        n = len(q)
-200        flag = True
-201        for i in range(n):
-202            if self.joint_types[i].startswith("JointModelR"):
-203                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
-204                    continue
-205                q[i] -= (
-206                    2
-207                    * np.pi
-208                    * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
-209                )
-210                if q[i] > self.joint_limits[i][1] + 1e-3:
-211                    flag = False
-212            else:
-213                if (
-214                    q[i] < self.joint_limits[i][0] - 1e-3
-215                    or q[i] > self.joint_limits[i][1] + 1e-3
-216                ):
-217                    flag = False
-218        return flag
+            
    210    def check_joint_limit(self, q):
+211        """
+212        check if the joint configuration is within the joint limits
+213
+214        Args:
+215            q: joint configuration
+216
+217        Returns:
+218            True if the joint configuration is within the joint limits
+219        """
+220        n = len(q)
+221        flag = True
+222        for i in range(n):
+223            if self.joint_types[i].startswith("JointModelR"):
+224                if np.abs(q[i] - self.joint_limits[i][0]) < 1e-3:
+225                    continue
+226                q[i] -= (
+227                    2 * np.pi * np.floor((q[i] - self.joint_limits[i][0]) / (2 * np.pi))
+228                )
+229                if q[i] > self.joint_limits[i][1] + 1e-3:
+230                    flag = False
+231            else:
+232                if (
+233                    q[i] < self.joint_limits[i][0] - 1e-3
+234                    or q[i] > self.joint_limits[i][1] + 1e-3
+235                ):
+236                    flag = False
+237        return flag
 
    
 
 
@@ -2276,21 +2379,26 @@ 
    
 
     
    
     
-            
    220    def pad_qpos(self, qpos, articulation=None):
-221        """
-222        if the user does not provide the full qpos but only the move_group joints,
-223        pad the qpos with the rest of the joints
-224        """
-225        if len(qpos) == len(self.move_group_joint_indices):
-226            tmp = articulation.get_qpos() if articulation is not None else self.robot.get_qpos()
-227            tmp[:len(qpos)] = qpos
-228            qpos = tmp
-229
-230        assert len(qpos) == len(self.joint_limits),\
-231            f"length of qpos ({len(qpos)}) =/= "\
-232            f"number of total joints ({len(self.joint_limits)})"
-233
-234        return qpos
+            
    239    def pad_qpos(self, qpos, articulation=None):
+240        """
+241        if the user does not provide the full qpos but only the move_group joints,
+242        pad the qpos with the rest of the joints
+243        """
+244        if len(qpos) == len(self.move_group_joint_indices):
+245            tmp = (
+246                articulation.get_qpos()
+247                if articulation is not None
+248                else self.robot.get_qpos()
+249            )
+250            tmp[: len(qpos)] = qpos
+251            qpos = tmp
+252
+253        assert len(qpos) == len(self.joint_limits), (
+254            f"length of qpos ({len(qpos)}) =/= "
+255            f"number of total joints ({len(self.joint_limits)})"
+256        )
+257
+258        return qpos
 
    
 
 
@@ -2311,26 +2419,31 @@ 
    
 
     
    
     
-            
    236    def check_for_collision(self, collision_function, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-237        """ helper function to check for collision """
-238        # handle no user input
-239        if articulation is None:
-240            articulation = self.robot
-241        if qpos is None:
-242            qpos = articulation.get_qpos()
-243        qpos = self.pad_qpos(qpos, articulation)
-244
-245        # first save the current qpos
-246        old_qpos = articulation.get_qpos()
-247        # set robot to new qpos
-248        articulation.set_qpos(qpos, True)
-249        # find the index of the articulation inside the array
-250        idx = self.planning_world.get_articulations().index(articulation)
-251        # check for self-collision
-252        collisions = collision_function(idx)
-253        # reset qpos
-254        articulation.set_qpos(old_qpos, True)
-255        return collisions
+            
    260    def check_for_collision(
+261        self,
+262        collision_function,
+263        articulation: articulation.ArticulatedModel = None,
+264        qpos: np.ndarray = None,
+265    ) -> list:
+266        """helper function to check for collision"""
+267        # handle no user input
+268        if articulation is None:
+269            articulation = self.robot
+270        if qpos is None:
+271            qpos = articulation.get_qpos()
+272        qpos = self.pad_qpos(qpos, articulation)
+273
+274        # first save the current qpos
+275        old_qpos = articulation.get_qpos()
+276        # set robot to new qpos
+277        articulation.set_qpos(qpos, True)
+278        # find the index of the articulation inside the array
+279        idx = self.planning_world.get_articulations().index(articulation)
+280        # check for self-collision
+281        collisions = collision_function(idx)
+282        # reset qpos
+283        articulation.set_qpos(old_qpos, True)
+284        return collisions
 
    
 
 
@@ -2350,28 +2463,35 @@ 
    
 
     
    
     
-            
    257    def check_for_self_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None) -> list:
-258        """Check if the robot is in self-collision.
-259
-260        Args:
-261            articulation: robot model. if none will be self.robot
-262            qpos: robot configuration. if none will be the current pose
-263
-264        Returns:
-265            A list of collisions.
-266        """
-267        return self.check_for_collision(self.planning_world.self_collide, articulation, qpos)
+            
    286    def check_for_self_collision(
+287        self,
+288        articulation: articulation.ArticulatedModel = None,
+289        qpos: np.ndarray = None,
+290    ) -> list:
+291        """Check if the robot is in self-collision.
+292
+293        Args:
+294            articulation: robot model. if none will be self.robot
+295            qpos: robot configuration. if none will be the current pose
+296
+297        Returns:
+298            A list of collisions.
+299        """
+300        return self.check_for_collision(
+301            self.planning_world.self_collide, articulation, qpos
+302        )
 
    
 
 
             
    Check if the robot is in self-collision.
    
 
-
    Args:
-    articulation: robot model. if none will be self.robot
-    qpos: robot configuration. if none will be the current pose
    
+
        Args:
+        articulation: robot model. if none will be self.robot
+        qpos: robot configuration. if none will be the current pose
 
-
    Returns:
-    A list of collisions.
    
+    Returns:
+        A list of collisions.
+
    
 
    
 
 
@@ -2387,41 +2507,50 @@ 
    
 
     
    
     
-            
    269    def check_for_env_collision(self, articulation: articulation.ArticulatedModel=None, qpos: np.ndarray=None, with_point_cloud=False, use_attach=False) -> list:
-270        """Check if the robot is in collision with the environment
-271
-272        Args:
-273            articulation: robot model. if none will be self.robot
-274            qpos: robot configuration. if none will be the current pose
-275            with_point_cloud: whether to check collision against point cloud
-276            use_attach: whether to include the object attached to the end effector in collision checking
-277        Returns:
-278            A list of collisions.
-279        """
-280        # store previous results
-281        prev_use_point_cloud = self.planning_world.use_point_cloud
-282        prev_use_attach = self.planning_world.use_attach
-283        self.planning_world.set_use_point_cloud(with_point_cloud)
-284        self.planning_world.set_use_attach(use_attach)
-285
-286        results = self.check_for_collision(self.planning_world.collide_with_others, articulation, qpos)
-287
-288        # restore
-289        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
-290        self.planning_world.set_use_attach(prev_use_attach)
-291        return results
+            
    304    def check_for_env_collision(
+305        self,
+306        articulation: articulation.ArticulatedModel = None,
+307        qpos: np.ndarray = None,
+308        with_point_cloud=False,
+309        use_attach=False,
+310    ) -> list:
+311        """Check if the robot is in collision with the environment
+312
+313        Args:
+314            articulation: robot model. if none will be self.robot
+315            qpos: robot configuration. if none will be the current pose
+316            with_point_cloud: whether to check collision against point cloud
+317            use_attach: whether to include the object attached to the end effector in collision checking
+318        Returns:
+319            A list of collisions.
+320        """
+321        # store previous results
+322        prev_use_point_cloud = self.planning_world.use_point_cloud
+323        prev_use_attach = self.planning_world.use_attach
+324        self.planning_world.set_use_point_cloud(with_point_cloud)
+325        self.planning_world.set_use_attach(use_attach)
+326
+327        results = self.check_for_collision(
+328            self.planning_world.collide_with_others, articulation, qpos
+329        )
+330
+331        # restore
+332        self.planning_world.set_use_point_cloud(prev_use_point_cloud)
+333        self.planning_world.set_use_attach(prev_use_attach)
+334        return results
 
    
 
 
             
    Check if the robot is in collision with the environment
    
 
-
    Args:
-    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
-Returns:
-    A list of collisions.
    
+
        Args:
+        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
+    Returns:
+        A list of collisions.
+
    
 
    
 
 
@@ -2437,67 +2566,67 @@ 
    
 
     
    
     
-            
    293    def IK(self, goal_pose, start_qpos, mask = [], n_init_qpos=20, threshold=1e-3):
-294        """
-295        Inverse kinematics
-296
-297        Args:
-298            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-299            start_qpos: initial configuration
-300            mask: if the value at a given index is True, the joint is *not* used in the IK
-301            n_init_qpos: number of random initial configurations
-302            threshold: threshold for the distance between the goal pose and the result pose
-303        """
-304
-305        index = self.link_name_2_idx[self.move_group]
-306        min_dis = 1e9
-307        idx = self.move_group_joint_indices
-308        qpos0 = np.copy(start_qpos)
-309        results = []
-310        self.robot.set_qpos(start_qpos, True)
-311        for i in range(n_init_qpos):
-312            ik_results = self.pinocchio_model.compute_IK_CLIK(
-313                index, goal_pose, start_qpos, mask
-314            )
-315            flag = self.check_joint_limit(ik_results[0]) # will clip qpos
-316
-317            # check collision
-318            self.planning_world.set_qpos_all(ik_results[0][idx])
-319            if (len(self.planning_world.collide_full()) != 0):
-320                flag = False
-321
-322            if flag:
-323                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
-324                new_pose = self.pinocchio_model.get_link_pose(index)
-325                tmp_dis = self.distance_6D(
-326                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
-327                )
-328                if tmp_dis < min_dis:
-329                    min_dis = tmp_dis
-330                if tmp_dis < threshold:
-331                    result = ik_results[0] 
-332                    unique = True
-333                    for j in range(len(results)):
-334                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
-335                            unique = False
-336                    if unique:
-337                        results.append(result)
-338            start_qpos = self.pinocchio_model.get_random_configuration()
-339            mask_len = len(mask)
-340            if mask_len > 0:
-341                for j in range(mask_len):
-342                    if mask[j]:
-343                        start_qpos[j] = qpos0[j]
-344        if len(results) != 0:
-345            status = "Success"
-346        elif min_dis != 1e9:
-347            status = (
-348                "IK Failed! Distance %lf is greater than threshold %lf."
-349                % (min_dis, threshold)
-350            )
-351        else:
-352            status = "IK Failed! Cannot find valid solution."
-353        return status, results
+            
    336    def IK(self, goal_pose, start_qpos, mask=[], n_init_qpos=20, threshold=1e-3):
+337        """
+338        Inverse kinematics
+339
+340        Args:
+341            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+342            start_qpos: initial configuration
+343            mask: if the value at a given index is True, the joint is *not* used in the IK
+344            n_init_qpos: number of random initial configurations
+345            threshold: threshold for the distance between the goal pose and the result pose
+346        """
+347
+348        index = self.link_name_2_idx[self.move_group]
+349        min_dis = 1e9
+350        idx = self.move_group_joint_indices
+351        qpos0 = np.copy(start_qpos)
+352        results = []
+353        self.robot.set_qpos(start_qpos, True)
+354        for i in range(n_init_qpos):
+355            ik_results = self.pinocchio_model.compute_IK_CLIK(
+356                index, goal_pose, start_qpos, mask
+357            )
+358            flag = self.check_joint_limit(ik_results[0])  # will clip qpos
+359
+360            # check collision
+361            self.planning_world.set_qpos_all(ik_results[0][idx])
+362            if len(self.planning_world.collide_full()) != 0:
+363                flag = False
+364
+365            if flag:
+366                self.pinocchio_model.compute_forward_kinematics(ik_results[0])
+367                new_pose = self.pinocchio_model.get_link_pose(index)
+368                tmp_dis = self.distance_6D(
+369                    goal_pose[:3], goal_pose[3:], new_pose[:3], new_pose[3:]
+370                )
+371                if tmp_dis < min_dis:
+372                    min_dis = tmp_dis
+373                if tmp_dis < threshold:
+374                    result = ik_results[0]
+375                    unique = True
+376                    for j in range(len(results)):
+377                        if np.linalg.norm(results[j][idx] - result[idx]) < 0.1:
+378                            unique = False
+379                    if unique:
+380                        results.append(result)
+381            start_qpos = self.pinocchio_model.get_random_configuration()
+382            mask_len = len(mask)
+383            if mask_len > 0:
+384                for j in range(mask_len):
+385                    if mask[j]:
+386                        start_qpos[j] = qpos0[j]
+387        if len(results) != 0:
+388            status = "Success"
+389        elif min_dis != 1e9:
+390            status = "IK Failed! Distance %lf is greater than threshold %lf." % (
+391                min_dis,
+392                threshold,
+393            )
+394        else:
+395            status = "IK Failed! Cannot find valid solution."
+396        return status, results
 
    
 
 
@@ -2524,35 +2653,33 @@ 
    
 
     
    
     
-            
    355    def TOPP(self, path, step=0.1, verbose=False):
-356        """
-357        Time-Optimal Path Parameterization
-358
-359        Args:
-360            path: numpy array of shape (n, dof)
-361            step: step size for the discretization
-362            verbose: if True, will print the log of TOPPRA
-363        """
-364
-365        N_samples = path.shape[0]
-366        dof = path.shape[1]
-367        assert dof == len(self.joint_vel_limits)
-368        assert dof == len(self.joint_acc_limits)
-369        ss = np.linspace(0, 1, N_samples)
-370        path = ta.SplineInterpolator(ss, path)
-371        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
-372        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
-373        instance = algo.TOPPRA(
-374            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
-375        )
-376        jnt_traj = instance.compute_trajectory()
-377        ts_sample = np.linspace(
-378            0, jnt_traj.duration, int(jnt_traj.duration / step)
-379        )
-380        qs_sample = jnt_traj(ts_sample)
-381        qds_sample = jnt_traj(ts_sample, 1)
-382        qdds_sample = jnt_traj(ts_sample, 2)
-383        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
+            
    398    def TOPP(self, path, step=0.1, verbose=False):
+399        """
+400        Time-Optimal Path Parameterization
+401
+402        Args:
+403            path: numpy array of shape (n, dof)
+404            step: step size for the discretization
+405            verbose: if True, will print the log of TOPPRA
+406        """
+407
+408        N_samples = path.shape[0]
+409        dof = path.shape[1]
+410        assert dof == len(self.joint_vel_limits)
+411        assert dof == len(self.joint_acc_limits)
+412        ss = np.linspace(0, 1, N_samples)
+413        path = ta.SplineInterpolator(ss, path)
+414        pc_vel = constraint.JointVelocityConstraint(self.joint_vel_limits)
+415        pc_acc = constraint.JointAccelerationConstraint(self.joint_acc_limits)
+416        instance = algo.TOPPRA(
+417            [pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel"
+418        )
+419        jnt_traj = instance.compute_trajectory()
+420        ts_sample = np.linspace(0, jnt_traj.duration, int(jnt_traj.duration / step))
+421        qs_sample = jnt_traj(ts_sample)
+422        qds_sample = jnt_traj(ts_sample, 1)
+423        qdds_sample = jnt_traj(ts_sample, 2)
+424        return ts_sample, qs_sample, qds_sample, qdds_sample, jnt_traj.duration
 
    
 
 
@@ -2577,13 +2704,13 @@ 
    
 
     
    
     
-            
    385    def update_point_cloud(self, pc, radius=1e-3):
-386        """ 
-387        Args:
-388            pc: numpy array of shape (n, 3)
-389            radius: radius of each point. This gives a buffer around each point that planner will avoid
-390        """
-391        self.planning_world.update_point_cloud(pc, radius)
+            
    426    def update_point_cloud(self, pc, radius=1e-3):
+427        """
+428        Args:
+429            pc: numpy array of shape (n, 3)
+430            radius: radius of each point. This gives a buffer around each point that planner will avoid
+431        """
+432        self.planning_world.update_point_cloud(pc, radius)
 
    
 
 
@@ -2605,11 +2732,11 @@ 
    
 
     
    
     
-            
    393    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
-394        """ helper function to update the attached tool """
-395        if link_id == -1:
-396            link_id = self.move_group_link_id
-397        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
+            
    434    def update_attached_tool(self, fcl_collision_geometry, pose, link_id=-1):
+435        """helper function to update the attached tool"""
+436        if link_id == -1:
+437            link_id = self.move_group_link_id
+438        self.planning_world.update_attached_tool(fcl_collision_geometry, link_id, pose)
 
    
 
 
@@ -2629,18 +2756,18 @@ 
    
 
     
    
     
-            
    399    def update_attached_sphere(self, radius, pose, link_id=-1):
-400        """
-401        attach a sphere to some link
-402
-403        Args:
-404            radius: radius of the sphere
-405            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
-406            link_id: if not provided, the end effector will be the target.
-407        """
-408        if link_id == -1:
-409            link_id = self.move_group_link_id
-410        self.planning_world.update_attached_sphere(radius, link_id, pose)
+            
    440    def update_attached_sphere(self, radius, pose, link_id=-1):
+441        """
+442        attach a sphere to some link
+443
+444        Args:
+445            radius: radius of the sphere
+446            pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
+447            link_id: if not provided, the end effector will be the target.
+448        """
+449        if link_id == -1:
+450            link_id = self.move_group_link_id
+451        self.planning_world.update_attached_sphere(radius, link_id, pose)
 
    
 
 
@@ -2665,18 +2792,18 @@ 
    
 
     
    
     
-            
    412    def update_attached_box(self, size, pose, link_id=-1):
-413        """
-414        attach a box to some link
-415
-416        Args:
-417            size: [x,y,z] size of the box
-418            pose: [x,y,z,qw,qx,qy,qz] pose of the box
-419            link_id: if not provided, the end effector will be the target.
-420        """
-421        if link_id == -1:
-422            link_id = self.move_group_link_id
-423        self.planning_world.update_attached_box(size, link_id, pose)
+            
    453    def update_attached_box(self, size, pose, link_id=-1):
+454        """
+455        attach a box to some link
+456
+457        Args:
+458            size: [x,y,z] size of the box
+459            pose: [x,y,z,qw,qx,qy,qz] pose of the box
+460            link_id: if not provided, the end effector will be the target.
+461        """
+462        if link_id == -1:
+463            link_id = self.move_group_link_id
+464        self.planning_world.update_attached_box(size, link_id, pose)
 
    
 
 
@@ -2701,18 +2828,18 @@ 
    
 
     
    
     
-            
    425    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
-426        """
-427        attach a mesh to some link
-428
-429        Args:
-430            mesh_path: path to the mesh
-431            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
-432            link_id: if not provided, the end effector will be the target.
-433        """
-434        if link_id == -1:
-435            link_id = self.move_group_link_id
-436        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
+            
    466    def update_attached_mesh(self, mesh_path, pose, link_id=-1):
+467        """
+468        attach a mesh to some link
+469
+470        Args:
+471            mesh_path: path to the mesh
+472            pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
+473            link_id: if not provided, the end effector will be the target.
+474        """
+475        if link_id == -1:
+476            link_id = self.move_group_link_id
+477        self.planning_world.update_attached_mesh(mesh_path, link_id, pose)
 
    
 
 
@@ -2737,14 +2864,14 @@ 
    
 
     
    
     
-            
    438    def set_base_pose(self, pose):
-439        """
-440        tell the planner where the base of the robot is w.r.t the world frame
-441
-442        Args:
-443            pose: [x,y,z,qw,qx,qy,qz] pose of the base
-444        """
-445        self.robot.set_base_pose(pose)
+            
    479    def set_base_pose(self, pose):
+480        """
+481        tell the planner where the base of the robot is w.r.t the world frame
+482
+483        Args:
+484            pose: [x,y,z,qw,qx,qy,qz] pose of the base
+485        """
+486        self.robot.set_base_pose(pose)
 
    
 
 
@@ -2767,9 +2894,9 @@ 
    
 
     
    
     
-            
    447    def set_normal_object(self, name, collision_object):
-448        """ adds or updates a non-articulated collision object in the scene """
-449        self.planning_world.set_normal_object(name, collision_object)
+            
    488    def set_normal_object(self, name, collision_object):
+489        """adds or updates a non-articulated collision object in the scene"""
+490        self.planning_world.set_normal_object(name, collision_object)
 
    
 
 
@@ -2789,9 +2916,9 @@ 
    
 
     
    
     
-            
    451    def remove_normal_object(self, name):
-452        """ returns true if the object was removed, false if it was not found """
-453        return self.planning_world.remove_normal_object(name)
+            
    492    def remove_normal_object(self, name):
+493        """returns true if the object was removed, false if it was not found"""
+494        return self.planning_world.remove_normal_object(name)
 
    
 
 
@@ -2811,103 +2938,107 @@ 
    
 
     
    
     
-            
    455    def plan_qpos_to_qpos(
-456        self,
-457        goal_qposes: list,
-458        current_qpos,
-459        time_step=0.1,
-460        rrt_range=0.1,
-461        planning_time=1,
-462        fix_joint_limits=True,
-463        use_point_cloud=False,
-464        use_attach=False,
-465        verbose=False,
-466        planner_name="RRTConnect",
-467        no_simplification=False,
-468        constraint_function=None,
-469        constraint_jacobian=None,
-470        constraint_tolerance=1e-3,
-471        fixed_joint_indices=[]
-472    ):
-473        """
-474        plan a path from a specified joint position to a goal pose
-475
-476        Args:
-477            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
-478            current_qpos: current joint configuration (either full or move_group joints)
-479            mask: mask for IK. When set, the IK will leave certain joints out of planning
-480            time_step: time step for TOPP
-481            rrt_range: step size for RRT
-482            planning_time: time limit for RRT
-483            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-484            use_point_cloud: if True, will use the point cloud to avoid collision
-485            use_attach: if True, will consider the attached tool collision when planning
-486            verbose: if True, will print the log of OMPL and TOPPRA
-487            planner_name: planner name pick from {"RRTConnect", "RRT*"}
-488            fixed_joint_indices: list of indices of joints that are fixed during planning
-489            constraint_function: evals to 0 when constraint is satisfied
-490            constraint_jacobian: jacobian of constraint_function
-491            constraint_tolerance: tolerance for constraint_function
-492            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
-493        """
-494        self.planning_world.set_use_point_cloud(use_point_cloud)
-495        self.planning_world.set_use_attach(use_attach)
-496        n = current_qpos.shape[0]
-497        if fix_joint_limits:
-498            for i in range(n):
-499                if current_qpos[i] < self.joint_limits[i][0]:
-500                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-501                if current_qpos[i] > self.joint_limits[i][1]:
-502                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-503
-504        current_qpos = self.pad_qpos(current_qpos)
-505
-506        self.robot.set_qpos(current_qpos, True)
-507        collisions = self.planning_world.collide_full()
-508        if len(collisions) != 0:
-509            print("Invalid start state!")
-510            for collision in collisions:
-511                print("%s and %s collide!" % (collision.link_name1, collision.link_name2))
-512
-513        idx = self.move_group_joint_indices
-514        
-515        goal_qpos_ = []
-516        for i in range(len(goal_qposes)):
-517            goal_qpos_.append(goal_qposes[i][idx])
-518        
-519        fixed_joints = set()
-520        for joint_idx in fixed_joint_indices:
-521            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
-522
-523        assert len(current_qpos[idx]) == len(goal_qpos_[0])
-524        status, path = self.planner.plan(
-525            current_qpos[idx],
-526            goal_qpos_,
-527            range=rrt_range,
-528            time=planning_time,
-529            fixed_joints=fixed_joints,
-530            verbose=verbose,
-531            planner_name=planner_name,
-532            no_simplification=no_simplification,
-533            constraint_function=constraint_function,
-534            constraint_jacobian=constraint_jacobian,
-535            constraint_tolerance=constraint_tolerance
-536        )
-537
-538        if status == "Exact solution":
-539            if verbose: ta.setup_logging("INFO")
-540            else: ta.setup_logging("WARNING")
-541            times, pos, vel, acc, duration = self.TOPP(path, time_step)
-542            return {
-543                "status": "Success",
-544                "time": times,
-545                "position": pos,
-546                "velocity": vel,
-547                "acceleration": acc,
-548                "duration": duration,
-549            }
-550        else:
-551            return {"status": "RRT Failed. %s" % status}
+            
    496    def plan_qpos_to_qpos(
+497        self,
+498        goal_qposes: list,
+499        current_qpos,
+500        time_step=0.1,
+501        rrt_range=0.1,
+502        planning_time=1,
+503        fix_joint_limits=True,
+504        use_point_cloud=False,
+505        use_attach=False,
+506        verbose=False,
+507        planner_name="RRTConnect",
+508        no_simplification=False,
+509        constraint_function=None,
+510        constraint_jacobian=None,
+511        constraint_tolerance=1e-3,
+512        fixed_joint_indices=[],
+513    ):
+514        """
+515        plan a path from a specified joint position to a goal pose
+516
+517        Args:
+518            goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]
+519            current_qpos: current joint configuration (either full or move_group joints)
+520            mask: mask for IK. When set, the IK will leave certain joints out of planning
+521            time_step: time step for TOPP
+522            rrt_range: step size for RRT
+523            planning_time: time limit for RRT
+524            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+525            use_point_cloud: if True, will use the point cloud to avoid collision
+526            use_attach: if True, will consider the attached tool collision when planning
+527            verbose: if True, will print the log of OMPL and TOPPRA
+528            planner_name: planner name pick from {"RRTConnect", "RRT*"}
+529            fixed_joint_indices: list of indices of joints that are fixed during planning
+530            constraint_function: evals to 0 when constraint is satisfied
+531            constraint_jacobian: jacobian of constraint_function
+532            constraint_tolerance: tolerance for constraint_function
+533            no_simplification: if true, will not simplify the path. constraint planning does not support simplification
+534        """
+535        self.planning_world.set_use_point_cloud(use_point_cloud)
+536        self.planning_world.set_use_attach(use_attach)
+537        n = current_qpos.shape[0]
+538        if fix_joint_limits:
+539            for i in range(n):
+540                if current_qpos[i] < self.joint_limits[i][0]:
+541                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+542                if current_qpos[i] > self.joint_limits[i][1]:
+543                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+544
+545        current_qpos = self.pad_qpos(current_qpos)
+546
+547        self.robot.set_qpos(current_qpos, True)
+548        collisions = self.planning_world.collide_full()
+549        if len(collisions) != 0:
+550            print("Invalid start state!")
+551            for collision in collisions:
+552                print(
+553                    "%s and %s collide!" % (collision.link_name1, collision.link_name2)
+554                )
+555
+556        idx = self.move_group_joint_indices
+557
+558        goal_qpos_ = []
+559        for i in range(len(goal_qposes)):
+560            goal_qpos_.append(goal_qposes[i][idx])
+561
+562        fixed_joints = set()
+563        for joint_idx in fixed_joint_indices:
+564            fixed_joints.add(ompl.FixedJoint(0, joint_idx, current_qpos[joint_idx]))
+565
+566        assert len(current_qpos[idx]) == len(goal_qpos_[0])
+567        status, path = self.planner.plan(
+568            current_qpos[idx],
+569            goal_qpos_,
+570            range=rrt_range,
+571            time=planning_time,
+572            fixed_joints=fixed_joints,
+573            verbose=verbose,
+574            planner_name=planner_name,
+575            no_simplification=no_simplification,
+576            constraint_function=constraint_function,
+577            constraint_jacobian=constraint_jacobian,
+578            constraint_tolerance=constraint_tolerance,
+579        )
+580
+581        if status == "Exact solution":
+582            if verbose:
+583                ta.setup_logging("INFO")
+584            else:
+585                ta.setup_logging("WARNING")
+586            times, pos, vel, acc, duration = self.TOPP(path, time_step)
+587            return {
+588                "status": "Success",
+589                "time": times,
+590                "position": pos,
+591                "velocity": vel,
+592                "acceleration": acc,
+593                "duration": duration,
+594            }
+595        else:
+596            return {"status": "RRT Failed. %s" % status}
 
    
 
 
@@ -2945,102 +3076,104 @@ 
    
 
     
    
     
-            
    553    def plan_qpos_to_pose(
-554        self,
-555        goal_pose,
-556        current_qpos,
-557        mask = [],
-558        time_step=0.1,
-559        rrt_range=0.1,
-560        planning_time=1,
-561        fix_joint_limits=True,
-562        use_point_cloud=False,
-563        use_attach=False,
-564        verbose=False,
-565        wrt_world=False,
-566        planner_name="RRTConnect",
-567        no_simplification=False,
-568        constraint_function=None,
-569        constraint_jacobian=None,
-570        constraint_tolerance=1e-3
-571    ):
-572        """
-573        plan from a start configuration to a goal pose of the end-effector
-574
-575        Args:
-576            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-577            current_qpos: current joint configuration (either full or move_group joints)
-578            mask: if the value at a given index is True, the joint is *not* used in the IK
-579            time_step: time step for TOPPRA (time parameterization of path)
-580            rrt_range: step size for RRT
-581            planning_time: time limit for RRT
-582            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
-583            use_point_cloud: if True, will use the point cloud to avoid collision
-584            use_attach: if True, will consider the attached tool collision when planning
-585            verbose: if True, will print the log of OMPL and TOPPRA
-586            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
-587        """
-588        n = current_qpos.shape[0]
-589        if fix_joint_limits:
-590            for i in range(n):
-591                if current_qpos[i] < self.joint_limits[i][0]:
-592                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
-593                if current_qpos[i] > self.joint_limits[i][1]:
-594                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
-595
-596        if wrt_world:
-597            base_pose = self.robot.get_base_pose()
-598            base_tf = np.eye(4)
-599            base_tf[0:3, 3] = base_pose[:3]
-600            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
-601            goal_tf = np.eye(4)
-602            goal_tf[0:3, 3] = goal_pose[:3]
-603            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
-604            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
-605            goal_pose[:3] = goal_tf[0:3, 3]
-606            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
-607
-608        idx = self.move_group_joint_indices  # we need to take only the move_group joints when planning
-609
-610        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-611        if ik_status != "Success":
-612            return {"status": ik_status}
-613
-614        if verbose:
-615            print("IK results:")
-616            for i in range(len(goal_qpos)):
-617               print(goal_qpos[i])
-618
-619        goal_qpos_ = []
-620        for i in range(len(goal_qpos)):
-621            goal_qpos_.append(goal_qpos[i][idx])
-622        self.robot.set_qpos(current_qpos, True)
-623
-624        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
-625        if ik_status != "Success":
-626            return {"status": ik_status}
-627
-628        if verbose:
-629            print("IK results:")
-630            for i in range(len(goal_qpos)):
-631               print(goal_qpos[i])
-632
-633        return self.plan_qpos_to_qpos(
-634            goal_qpos,
-635            current_qpos,
-636            time_step,
-637            rrt_range,
-638            planning_time,
-639            fix_joint_limits,
-640            use_point_cloud,
-641            use_attach,
-642            verbose,
-643            planner_name,
-644            no_simplification,
-645            constraint_function,
-646            constraint_jacobian,
-647            constraint_tolerance
-648        )
+            
    598    def plan_qpos_to_pose(
+599        self,
+600        goal_pose,
+601        current_qpos,
+602        mask=[],
+603        time_step=0.1,
+604        rrt_range=0.1,
+605        planning_time=1,
+606        fix_joint_limits=True,
+607        use_point_cloud=False,
+608        use_attach=False,
+609        verbose=False,
+610        wrt_world=False,
+611        planner_name="RRTConnect",
+612        no_simplification=False,
+613        constraint_function=None,
+614        constraint_jacobian=None,
+615        constraint_tolerance=1e-3,
+616    ):
+617        """
+618        plan from a start configuration to a goal pose of the end-effector
+619
+620        Args:
+621            goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+622            current_qpos: current joint configuration (either full or move_group joints)
+623            mask: if the value at a given index is True, the joint is *not* used in the IK
+624            time_step: time step for TOPPRA (time parameterization of path)
+625            rrt_range: step size for RRT
+626            planning_time: time limit for RRT
+627            fix_joint_limits: if True, will clip the joint configuration to be within the joint limits
+628            use_point_cloud: if True, will use the point cloud to avoid collision
+629            use_attach: if True, will consider the attached tool collision when planning
+630            verbose: if True, will print the log of OMPL and TOPPRA
+631            wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
+632        """
+633        n = current_qpos.shape[0]
+634        if fix_joint_limits:
+635            for i in range(n):
+636                if current_qpos[i] < self.joint_limits[i][0]:
+637                    current_qpos[i] = self.joint_limits[i][0] + 1e-3
+638                if current_qpos[i] > self.joint_limits[i][1]:
+639                    current_qpos[i] = self.joint_limits[i][1] - 1e-3
+640
+641        if wrt_world:
+642            base_pose = self.robot.get_base_pose()
+643            base_tf = np.eye(4)
+644            base_tf[0:3, 3] = base_pose[:3]
+645            base_tf[0:3, 0:3] = quat2mat(base_pose[3:])
+646            goal_tf = np.eye(4)
+647            goal_tf[0:3, 3] = goal_pose[:3]
+648            goal_tf[0:3, 0:3] = quat2mat(goal_pose[3:])
+649            goal_tf = np.linalg.inv(base_tf).dot(goal_tf)
+650            goal_pose[:3] = goal_tf[0:3, 3]
+651            goal_pose[3:] = mat2quat(goal_tf[0:3, 0:3])
+652
+653        idx = (
+654            self.move_group_joint_indices
+655        )  # we need to take only the move_group joints when planning
+656
+657        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+658        if ik_status != "Success":
+659            return {"status": ik_status}
+660
+661        if verbose:
+662            print("IK results:")
+663            for i in range(len(goal_qpos)):
+664                print(goal_qpos[i])
+665
+666        goal_qpos_ = []
+667        for i in range(len(goal_qpos)):
+668            goal_qpos_.append(goal_qpos[i][idx])
+669        self.robot.set_qpos(current_qpos, True)
+670
+671        ik_status, goal_qpos = self.IK(goal_pose, current_qpos, mask)
+672        if ik_status != "Success":
+673            return {"status": ik_status}
+674
+675        if verbose:
+676            print("IK results:")
+677            for i in range(len(goal_qpos)):
+678                print(goal_qpos[i])
+679
+680        return self.plan_qpos_to_qpos(
+681            goal_qpos,
+682            current_qpos,
+683            time_step,
+684            rrt_range,
+685            planning_time,
+686            fix_joint_limits,
+687            use_point_cloud,
+688            use_attach,
+689            verbose,
+690            planner_name,
+691            no_simplification,
+692            constraint_function,
+693            constraint_jacobian,
+694            constraint_tolerance,
+695        )
 
    
 
 
@@ -3073,153 +3206,147 @@ 
    
 
     
    
     
-            
    650    def plan_screw(
-651        self,
-652        target_pose,
-653        qpos,
-654        qpos_step=0.1,
-655        time_step=0.1,
-656        use_point_cloud=False,
-657        use_attach=False,
-658        verbose=False,
-659    ):
-660        """
-661        plan from a start configuration to a goal pose of the end-effector using screw motion
-662
-663        Args:
-664            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
-665            qpos: current joint configuration (either full or move_group joints)
-666            qpos_step: size of the random step for RRT
-667            time_step: time step for the discretization
-668            use_point_cloud: if True, will use the point cloud to avoid collision
-669            use_attach: if True, will use the attached tool to avoid collision
-670            verbose: if True, will print the log of TOPPRA
-671        """
-672        self.planning_world.set_use_point_cloud(use_point_cloud)
-673        self.planning_world.set_use_attach(use_attach)
-674        qpos = self.pad_qpos(qpos.copy())
-675        self.robot.set_qpos(qpos, True)
-676
-677        def pose7D2mat(pose):
-678            mat = np.eye(4)
-679            mat[0:3, 3] = pose[:3]
-680            mat[0:3, 0:3] = quat2mat(pose[3:])
-681            return mat
-682
-683        def skew(vec):
-684            return np.array(
-685                [
-686                    [0, -vec[2], vec[1]],
-687                    [vec[2], 0, -vec[0]],
-688                    [-vec[1], vec[0], 0],
-689                ]
-690            )
-691
-692        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
-693            def rot2so3(rotation: np.ndarray):
-694                assert rotation.shape == (3, 3)
-695                if np.isclose(rotation.trace(), 3):
-696                    return np.zeros(3), 1
-697                if np.isclose(rotation.trace(), -1):
-698                    return np.zeros(3), -1e6
-699                theta = np.arccos((rotation.trace() - 1) / 2)
-700                omega = (
-701                    1
-702                    / 2
-703                    / np.sin(theta)
-704                    * np.array(
-705                        [
-706                            rotation[2, 1] - rotation[1, 2],
-707                            rotation[0, 2] - rotation[2, 0],
-708                            rotation[1, 0] - rotation[0, 1],
-709                        ]
-710                    ).T
-711                )
-712                return omega, theta
-713
-714            omega, theta = rot2so3(pose[:3, :3])
-715            if theta < -1e5:
-716                return omega, theta
-717            ss = skew(omega)
-718            inv_left_jacobian = (
-719                np.eye(3) / theta
-720                - 0.5 * ss
-721                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
-722            )
-723            v = inv_left_jacobian @ pose[:3, 3]
-724            return np.concatenate([v, omega]), theta
-725
-726        self.pinocchio_model.compute_forward_kinematics(qpos)
-727        ee_index = self.link_name_2_idx[self.move_group]
-728        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
-729        target_p = pose7D2mat(target_pose)
-730        relative_transform = target_p @ np.linalg.inv(current_p)
-731
-732        omega, theta = pose2exp_coordinate(relative_transform)
-733
-734        if theta < -1e4:
-735            return {"status": "screw plan failed."}
-736        omega = omega.reshape((-1, 1)) * theta
-737
-738        index = self.move_group_joint_indices
-739        path = [np.copy(qpos[index])]
-740
-741        while True:
-742            self.pinocchio_model.compute_full_jacobian(qpos)
-743            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
-744            delta_q = np.linalg.pinv(J) @ omega
-745            delta_q *= qpos_step / (np.linalg.norm(delta_q))
-746            delta_twist = J @ delta_q
-747
-748            flag = False
-749            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
-750                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
-751                delta_q = delta_q * ratio
-752                delta_twist = delta_twist * ratio
-753                flag = True
-754
-755            qpos += delta_q.reshape(-1)
-756            omega -= delta_twist
-757
-758            def check_joint_limit(q):
-759                n = len(q)
-760                for i in range(n):
-761                    if (
-762                        q[i] < self.joint_limits[i][0] - 1e-3
-763                        or q[i] > self.joint_limits[i][1] + 1e-3
-764                    ):
-765                        return False
-766                return True
-767
-768            within_joint_limit = check_joint_limit(qpos)
-769            self.planning_world.set_qpos_all(qpos[index])
-770            collide = self.planning_world.collide()
-771
-772            if (
-773                np.linalg.norm(delta_twist) < 1e-4
-774                or collide
-775                or within_joint_limit == False
-776            ):
-777                return {"status": "screw plan failed"}
-778
-779            path.append(np.copy(qpos[index]))
+            
    697    def plan_screw(
+698        self,
+699        target_pose,
+700        qpos,
+701        qpos_step=0.1,
+702        time_step=0.1,
+703        use_point_cloud=False,
+704        use_attach=False,
+705        verbose=False,
+706    ):
+707        """
+708        plan from a start configuration to a goal pose of the end-effector using screw motion
+709
+710        Args:
+711            target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal
+712            qpos: current joint configuration (either full or move_group joints)
+713            qpos_step: size of the random step for RRT
+714            time_step: time step for the discretization
+715            use_point_cloud: if True, will use the point cloud to avoid collision
+716            use_attach: if True, will use the attached tool to avoid collision
+717            verbose: if True, will print the log of TOPPRA
+718        """
+719        self.planning_world.set_use_point_cloud(use_point_cloud)
+720        self.planning_world.set_use_attach(use_attach)
+721        qpos = self.pad_qpos(qpos.copy())
+722        self.robot.set_qpos(qpos, True)
+723
+724        def pose7D2mat(pose):
+725            mat = np.eye(4)
+726            mat[0:3, 3] = pose[:3]
+727            mat[0:3, 0:3] = quat2mat(pose[3:])
+728            return mat
+729
+730        def skew(vec):
+731            return np.array([
+732                [0, -vec[2], vec[1]],
+733                [vec[2], 0, -vec[0]],
+734                [-vec[1], vec[0], 0],
+735            ])
+736
+737        def pose2exp_coordinate(pose: np.ndarray) -> Tuple[np.ndarray, float]:
+738            def rot2so3(rotation: np.ndarray):
+739                assert rotation.shape == (3, 3)
+740                if np.isclose(rotation.trace(), 3):
+741                    return np.zeros(3), 1
+742                if np.isclose(rotation.trace(), -1):
+743                    return np.zeros(3), -1e6
+744                theta = np.arccos((rotation.trace() - 1) / 2)
+745                omega = (
+746                    1
+747                    / 2
+748                    / np.sin(theta)
+749                    * np.array([
+750                        rotation[2, 1] - rotation[1, 2],
+751                        rotation[0, 2] - rotation[2, 0],
+752                        rotation[1, 0] - rotation[0, 1],
+753                    ]).T
+754                )
+755                return omega, theta
+756
+757            omega, theta = rot2so3(pose[:3, :3])
+758            if theta < -1e5:
+759                return omega, theta
+760            ss = skew(omega)
+761            inv_left_jacobian = (
+762                np.eye(3) / theta
+763                - 0.5 * ss
+764                + (1.0 / theta - 0.5 / np.tan(theta / 2)) * ss @ ss
+765            )
+766            v = inv_left_jacobian @ pose[:3, 3]
+767            return np.concatenate([v, omega]), theta
+768
+769        self.pinocchio_model.compute_forward_kinematics(qpos)
+770        ee_index = self.link_name_2_idx[self.move_group]
+771        current_p = pose7D2mat(self.pinocchio_model.get_link_pose(ee_index))
+772        target_p = pose7D2mat(target_pose)
+773        relative_transform = target_p @ np.linalg.inv(current_p)
+774
+775        omega, theta = pose2exp_coordinate(relative_transform)
+776
+777        if theta < -1e4:
+778            return {"status": "screw plan failed."}
+779        omega = omega.reshape((-1, 1)) * theta
 780
-781            if flag:
-782                if verbose:
-783                    ta.setup_logging("INFO")
-784                else:
-785                    ta.setup_logging("WARNING")
-786                times, pos, vel, acc, duration = self.TOPP(
-787                    np.vstack(path), time_step
-788                )
-789                return {
-790                    "status": "Success",
-791                    "time": times,
-792                    "position": pos,
-793                    "velocity": vel,
-794                    "acceleration": acc,
-795                    "duration": duration,
-796                }
+781        index = self.move_group_joint_indices
+782        path = [np.copy(qpos[index])]
+783
+784        while True:
+785            self.pinocchio_model.compute_full_jacobian(qpos)
+786            J = self.pinocchio_model.get_link_jacobian(ee_index, local=False)
+787            delta_q = np.linalg.pinv(J) @ omega
+788            delta_q *= qpos_step / (np.linalg.norm(delta_q))
+789            delta_twist = J @ delta_q
+790
+791            flag = False
+792            if np.linalg.norm(delta_twist) > np.linalg.norm(omega):
+793                ratio = np.linalg.norm(omega) / np.linalg.norm(delta_twist)
+794                delta_q = delta_q * ratio
+795                delta_twist = delta_twist * ratio
+796                flag = True
+797
+798            qpos += delta_q.reshape(-1)
+799            omega -= delta_twist
+800
+801            def check_joint_limit(q):
+802                n = len(q)
+803                for i in range(n):
+804                    if (
+805                        q[i] < self.joint_limits[i][0] - 1e-3
+806                        or q[i] > self.joint_limits[i][1] + 1e-3
+807                    ):
+808                        return False
+809                return True
+810
+811            within_joint_limit = check_joint_limit(qpos)
+812            self.planning_world.set_qpos_all(qpos[index])
+813            collide = self.planning_world.collide()
+814
+815            if (
+816                np.linalg.norm(delta_twist) < 1e-4
+817                or collide
+818                or within_joint_limit == False
+819            ):
+820                return {"status": "screw plan failed"}
+821
+822            path.append(np.copy(qpos[index]))
+823
+824            if flag:
+825                if verbose:
+826                    ta.setup_logging("INFO")
+827                else:
+828                    ta.setup_logging("WARNING")
+829                times, pos, vel, acc, duration = self.TOPP(np.vstack(path), time_step)
+830                return {
+831                    "status": "Success",
+832                    "time": times,
+833                    "position": pos,
+834                    "velocity": vel,
+835                    "acceleration": acc,
+836                    "duration": duration,
+837                }
 
    
 
 
diff --git a/docs/search.js b/docs/search.js
index 0b98a5aa..aee8dd07 100644
--- a/docs/search.js
+++ b/docs/search.js
@@ -1,6 +1,6 @@
 window.pdocSearch = (function(){
 /** elasticlunr - http://weixsong.github.io * Copyright (C) 2017 Oliver Nightingale * Copyright (C) 2017 Wei Song * MIT Licensed */!function(){function e(e){if(null===e||"object"!=typeof e)return e;var t=e.constructor();for(var n in e)e.hasOwnProperty(n)&&(t[n]=e[n]);return t}var t=function(e){var n=new t.Index;return n.pipeline.add(t.trimmer,t.stopWordFilter,t.stemmer),e&&e.call(n,n),n};t.version="0.9.5",lunr=t,t.utils={},t.utils.warn=function(e){return function(t){e.console&&console.warn&&console.warn(t)}}(this),t.utils.toString=function(e){return void 0===e||null===e?"":e.toString()},t.EventEmitter=function(){this.events={}},t.EventEmitter.prototype.addListener=function(){var e=Array.prototype.slice.call(arguments),t=e.pop(),n=e;if("function"!=typeof t)throw new TypeError("last argument must be a function");n.forEach(function(e){this.hasHandler(e)||(this.events[e]=[]),this.events[e].push(t)},this)},t.EventEmitter.prototype.removeListener=function(e,t){if(this.hasHandler(e)){var n=this.events[e].indexOf(t);-1!==n&&(this.events[e].splice(n,1),0==this.events[e].length&&delete this.events[e])}},t.EventEmitter.prototype.emit=function(e){if(this.hasHandler(e)){var t=Array.prototype.slice.call(arguments,1);this.events[e].forEach(function(e){e.apply(void 0,t)},this)}},t.EventEmitter.prototype.hasHandler=function(e){return e in this.events},t.tokenizer=function(e){if(!arguments.length||null===e||void 0===e)return[];if(Array.isArray(e)){var n=e.filter(function(e){return null===e||void 0===e?!1:!0});n=n.map(function(e){return t.utils.toString(e).toLowerCase()});var i=[];return n.forEach(function(e){var n=e.split(t.tokenizer.seperator);i=i.concat(n)},this),i}return e.toString().trim().toLowerCase().split(t.tokenizer.seperator)},t.tokenizer.defaultSeperator=/[\s\-]+/,t.tokenizer.seperator=t.tokenizer.defaultSeperator,t.tokenizer.setSeperator=function(e){null!==e&&void 0!==e&&"object"==typeof e&&(t.tokenizer.seperator=e)},t.tokenizer.resetSeperator=function(){t.tokenizer.seperator=t.tokenizer.defaultSeperator},t.tokenizer.getSeperator=function(){return t.tokenizer.seperator},t.Pipeline=function(){this._queue=[]},t.Pipeline.registeredFunctions={},t.Pipeline.registerFunction=function(e,n){n in t.Pipeline.registeredFunctions&&t.utils.warn("Overwriting existing registered function: "+n),e.label=n,t.Pipeline.registeredFunctions[n]=e},t.Pipeline.getRegisteredFunction=function(e){return e in t.Pipeline.registeredFunctions!=!0?null:t.Pipeline.registeredFunctions[e]},t.Pipeline.warnIfFunctionNotRegistered=function(e){var n=e.label&&e.label in this.registeredFunctions;n||t.utils.warn("Function is not registered with pipeline. This may cause problems when serialising the index.\n",e)},t.Pipeline.load=function(e){var n=new t.Pipeline;return e.forEach(function(e){var i=t.Pipeline.getRegisteredFunction(e);if(!i)throw new Error("Cannot load un-registered function: "+e);n.add(i)}),n},t.Pipeline.prototype.add=function(){var e=Array.prototype.slice.call(arguments);e.forEach(function(e){t.Pipeline.warnIfFunctionNotRegistered(e),this._queue.push(e)},this)},t.Pipeline.prototype.after=function(e,n){t.Pipeline.warnIfFunctionNotRegistered(n);var i=this._queue.indexOf(e);if(-1===i)throw new Error("Cannot find existingFn");this._queue.splice(i+1,0,n)},t.Pipeline.prototype.before=function(e,n){t.Pipeline.warnIfFunctionNotRegistered(n);var i=this._queue.indexOf(e);if(-1===i)throw new Error("Cannot find existingFn");this._queue.splice(i,0,n)},t.Pipeline.prototype.remove=function(e){var t=this._queue.indexOf(e);-1!==t&&this._queue.splice(t,1)},t.Pipeline.prototype.run=function(e){for(var t=[],n=e.length,i=this._queue.length,o=0;n>o;o++){for(var r=e[o],s=0;i>s&&(r=this._queue[s](r,o,e),void 0!==r&&null!==r);s++);void 0!==r&&null!==r&&t.push(r)}return t},t.Pipeline.prototype.reset=function(){this._queue=[]},t.Pipeline.prototype.get=function(){return this._queue},t.Pipeline.prototype.toJSON=function(){return this._queue.map(function(e){return t.Pipeline.warnIfFunctionNotRegistered(e),e.label})},t.Index=function(){this._fields=[],this._ref="id",this.pipeline=new t.Pipeline,this.documentStore=new t.DocumentStore,this.index={},this.eventEmitter=new t.EventEmitter,this._idfCache={},this.on("add","remove","update",function(){this._idfCache={}}.bind(this))},t.Index.prototype.on=function(){var e=Array.prototype.slice.call(arguments);return this.eventEmitter.addListener.apply(this.eventEmitter,e)},t.Index.prototype.off=function(e,t){return this.eventEmitter.removeListener(e,t)},t.Index.load=function(e){e.version!==t.version&&t.utils.warn("version mismatch: current "+t.version+" importing "+e.version);var n=new this;n._fields=e.fields,n._ref=e.ref,n.documentStore=t.DocumentStore.load(e.documentStore),n.pipeline=t.Pipeline.load(e.pipeline),n.index={};for(var i in e.index)n.index[i]=t.InvertedIndex.load(e.index[i]);return n},t.Index.prototype.addField=function(e){return this._fields.push(e),this.index[e]=new t.InvertedIndex,this},t.Index.prototype.setRef=function(e){return this._ref=e,this},t.Index.prototype.saveDocument=function(e){return this.documentStore=new t.DocumentStore(e),this},t.Index.prototype.addDoc=function(e,n){if(e){var n=void 0===n?!0:n,i=e[this._ref];this.documentStore.addDoc(i,e),this._fields.forEach(function(n){var o=this.pipeline.run(t.tokenizer(e[n]));this.documentStore.addFieldLength(i,n,o.length);var r={};o.forEach(function(e){e in r?r[e]+=1:r[e]=1},this);for(var s in r){var u=r[s];u=Math.sqrt(u),this.index[n].addToken(s,{ref:i,tf:u})}},this),n&&this.eventEmitter.emit("add",e,this)}},t.Index.prototype.removeDocByRef=function(e){if(e&&this.documentStore.isDocStored()!==!1&&this.documentStore.hasDoc(e)){var t=this.documentStore.getDoc(e);this.removeDoc(t,!1)}},t.Index.prototype.removeDoc=function(e,n){if(e){var n=void 0===n?!0:n,i=e[this._ref];this.documentStore.hasDoc(i)&&(this.documentStore.removeDoc(i),this._fields.forEach(function(n){var o=this.pipeline.run(t.tokenizer(e[n]));o.forEach(function(e){this.index[n].removeToken(e,i)},this)},this),n&&this.eventEmitter.emit("remove",e,this))}},t.Index.prototype.updateDoc=function(e,t){var t=void 0===t?!0:t;this.removeDocByRef(e[this._ref],!1),this.addDoc(e,!1),t&&this.eventEmitter.emit("update",e,this)},t.Index.prototype.idf=function(e,t){var n="@"+t+"/"+e;if(Object.prototype.hasOwnProperty.call(this._idfCache,n))return this._idfCache[n];var i=this.index[t].getDocFreq(e),o=1+Math.log(this.documentStore.length/(i+1));return this._idfCache[n]=o,o},t.Index.prototype.getFields=function(){return this._fields.slice()},t.Index.prototype.search=function(e,n){if(!e)return[];e="string"==typeof e?{any:e}:JSON.parse(JSON.stringify(e));var i=null;null!=n&&(i=JSON.stringify(n));for(var o=new t.Configuration(i,this.getFields()).get(),r={},s=Object.keys(e),u=0;u0&&t.push(e);for(var i in n)"docs"!==i&&"df"!==i&&this.expandToken(e+i,t,n[i]);return t},t.InvertedIndex.prototype.toJSON=function(){return{root:this.root}},t.Configuration=function(e,n){var e=e||"";if(void 0==n||null==n)throw new Error("fields should not be null");this.config={};var i;try{i=JSON.parse(e),this.buildUserConfig(i,n)}catch(o){t.utils.warn("user configuration parse failed, will use default configuration"),this.buildDefaultConfig(n)}},t.Configuration.prototype.buildDefaultConfig=function(e){this.reset(),e.forEach(function(e){this.config[e]={boost:1,bool:"OR",expand:!1}},this)},t.Configuration.prototype.buildUserConfig=function(e,n){var i="OR",o=!1;if(this.reset(),"bool"in e&&(i=e.bool||i),"expand"in e&&(o=e.expand||o),"fields"in e)for(var r in e.fields)if(n.indexOf(r)>-1){var s=e.fields[r],u=o;void 0!=s.expand&&(u=s.expand),this.config[r]={boost:s.boost||0===s.boost?s.boost:1,bool:s.bool||i,expand:u}}else t.utils.warn("field name in user configuration not found in index instance fields");else this.addAllFields2UserConfig(i,o,n)},t.Configuration.prototype.addAllFields2UserConfig=function(e,t,n){n.forEach(function(n){this.config[n]={boost:1,bool:e,expand:t}},this)},t.Configuration.prototype.get=function(){return this.config},t.Configuration.prototype.reset=function(){this.config={}},lunr.SortedSet=function(){this.length=0,this.elements=[]},lunr.SortedSet.load=function(e){var t=new this;return t.elements=e,t.length=e.length,t},lunr.SortedSet.prototype.add=function(){var e,t;for(e=0;e1;){if(r===e)return o;e>r&&(t=o),r>e&&(n=o),i=n-t,o=t+Math.floor(i/2),r=this.elements[o]}return r===e?o:-1},lunr.SortedSet.prototype.locationFor=function(e){for(var t=0,n=this.elements.length,i=n-t,o=t+Math.floor(i/2),r=this.elements[o];i>1;)e>r&&(t=o),r>e&&(n=o),i=n-t,o=t+Math.floor(i/2),r=this.elements[o];return r>e?o:e>r?o+1:void 0},lunr.SortedSet.prototype.intersect=function(e){for(var t=new lunr.SortedSet,n=0,i=0,o=this.length,r=e.length,s=this.elements,u=e.elements;;){if(n>o-1||i>r-1)break;s[n]!==u[i]?s[n]u[i]&&i++:(t.add(s[n]),n++,i++)}return t},lunr.SortedSet.prototype.clone=function(){var e=new lunr.SortedSet;return e.elements=this.toArray(),e.length=e.elements.length,e},lunr.SortedSet.prototype.union=function(e){var t,n,i;this.length>=e.length?(t=this,n=e):(t=e,n=this),i=t.clone();for(var o=0,r=n.toArray();oMPlib
    \n\n
    MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up. With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.
    \n\n
    Installation
    \n\n
    Pre-built pip packages support Ubuntu 18.04+ with Python 3.6+.
    \n\n
    pip install mplib\n
    \n\n
    Usage
    \n\n
    See our tutorial for detailed usage and examples.
    \n"}, {"fullname": "mplib.examples", "modulename": "mplib.examples", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance", "modulename": "mplib.examples.collision_avoidance", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo", "kind": "class", "doc": "
    The shows the planner's ability to generate a collision free path with the straight path causes collisions
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.__init__", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Same setup as demo.py except the boxes are of difference sizes and different use\nRed box is the target we want to grab\nBlue box is the obstacle we want to avoid\ngreen box is the landing pad on which we want to place the red box
    \n", "signature": "()"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.table", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.red_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.green_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.blue_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "
    we tell the planner about the obstacle through a point cloud
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.demo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    We pick up the red box while avoiding the blue box and place it back down on top of the green box
    \n", "signature": "(self, with_screw=True, use_point_cloud=True, use_attach=True):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning", "modulename": "mplib.examples.constrained_planning", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo", "kind": "class", "doc": "
    This demo shows the planner's ability to plan with constraints.\nFor this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.__init__", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.__init__", "kind": "function", "doc": "
    set up the scene and load the robot
    \n", "signature": "()"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.add_point_cloud", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.add_point_cloud", "kind": "function", "doc": "
    add some random obstacles to make the planning more challenging
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.get_eef_z", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.get_eef_z", "kind": "function", "doc": "
    helper function for constraint
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_f", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_f", "kind": "function", "doc": "
    create a constraint function that takes in a qpos and outputs a scalar\nA valid constraint function should evaluates to 0 when the constraint is satisfied\nSee ompl constrained planning for more details
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_j", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_j", "kind": "function", "doc": "
    create the jacobian of the constraint function w.r.t. qpos\nThis is needed because the planner uses the jacobian to project a random sample to the constraint manifold
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.demo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.demo", "kind": "function", "doc": "
    We first plan with constraints to three poses, then plan without constraints to the same poses\nWhile not always the case, sometimes without constraints, the end effector will tilt almost upside down
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo", "modulename": "mplib.examples.demo", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.demo.PlanningDemo.__init__", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Setting up the scene, the planner, and adding some objects to the scene\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
    \n", "signature": "()"}, {"fullname": "mplib.examples.demo.PlanningDemo.table", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.red_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.green_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.blue_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.demo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    Declare three poses for the robot to move to, each one corresponding to the position of a box\nPick up the box, and set it down 0.1m to the right of its original position
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup", "modulename": "mplib.examples.demo_setup", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.demo_setup.DemoSetup", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup", "kind": "class", "doc": "
    This class is the super class to abstract away some of the setups for the demos\nYou will need to install Sapien via pip install sapien for this to work if you want to use the viewer
    \n"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.__init__", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.__init__", "kind": "function", "doc": "
    Nothing to do
    \n", "signature": "()"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_scene", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_scene", "kind": "function", "doc": "
    This is the Sapien simulator setup and has nothing to do with mplib
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.load_robot", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.load_robot", "kind": "function", "doc": "
    This function loads a robot from a URDF file into the Sapien Scene created above.\nNote that does mean that setup_scene() must be called before this function.
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_planner", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_planner", "kind": "function", "doc": "
    Create an mplib planner using the default robot\nsee planner.py for more details on the arguments
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.follow_path", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.follow_path", "kind": "function", "doc": "
    Helper function to follow a path generated by the planner
    \n", "signature": "(self, result):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.set_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.set_gripper", "kind": "function", "doc": "
    Helper function to activate gripper joints\nArgs:\n    pos: position of the gripper joint in real number
    \n", "signature": "(self, pos):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.open_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.open_gripper", "kind": "function", "doc": "
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.close_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.close_gripper", "kind": "function", "doc": "
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_RRTConnect", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_RRTConnect", "kind": "function", "doc": "
    Plan and follow a path to a pose using RRTConnect
    \n\n
    Args:\n    pose: [x, y, z, qx, qy, qz, qw]\n    use_point_cloud (optional): if to take the point cloud into consideration for collision checking.\n    use_attach (optional): if to take the attach into consideration for collision checking.
    \n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_screw", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_screw", "kind": "function", "doc": "
    Interpolative planning with screw motion.\nWill not avoid collision and will fail if the path contains collision.
    \n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose", "kind": "function", "doc": "
    API to multiplex between the two planning methods
    \n", "signature": "(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision", "modulename": "mplib.examples.detect_collision", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo", "kind": "class", "doc": "
    This demonstrates some of the collision detection functions in the planner.
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.__init__", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.__init__", "kind": "function", "doc": "
    Only the planner is needed this time. No simulation env required
    \n", "signature": "()"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.print_collisions", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.print_collisions", "kind": "function", "doc": "
    Helper function to abstract away the printing of collisions
    \n", "signature": "(self, collisions):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.demo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.demo", "kind": "function", "doc": "
    We test several configurations:
    \n\n
      \n
    1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
      2. \n
    2. Set robot to a self-collision qpos and check for self-collision returns a collision
      3. \n
    3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
      4. \n
    4. Set robot to a env-collision qpos and check for env-collision returns a collision
      5. \n
    5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
      6. \n
    6. Remove the floor and check for env-collision returns no collision
      7. \n
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.moving_robot", "modulename": "mplib.examples.moving_robot", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.__init__", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Setting up the scene, the planner, and adding some objects to the scene\nNote 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]\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation\nCompared to demo.py, all the props are shifted by 1 meter in the x and y direction
    \n", "signature": "()"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.table", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.red_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.green_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.blue_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.demo", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    Same demo as demo.py.\nAlthough we shifted everything, the poses have not changed because these are w.r.t. the robot base\nAlternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner\nthe poses are specified with respect to the world
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion", "modulename": "mplib.examples.two_stage_motion", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This demo is the same as collision_avoidance.py except we added a track for the robot to move along\nWe reach the target in two stages:
    \n\n
      \n
    1. First, we move the base while fixing the arm joints
      2. \n
    2. Then, we move the arm while fixing the base joints\nThis corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
      3. \n
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.__init__", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    We have modified the urdf file to include a track for the robot to move along\nOtherwise, the setup is the same as collision_avoidance.py
    \n", "signature": "()"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.table", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.red_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.green_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.blue_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "
    see collision_avoidance.py for details
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.plan_without_base", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.plan_without_base", "kind": "function", "doc": "
    a subroutine to plan a path without moving the base
    \n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.move_in_two_stage", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.move_in_two_stage", "kind": "function", "doc": "
    first, we do a full IK but only generate motions for the base\nthen, do another partial IK for the arm and generate motions for the arm
    \n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.demo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.planner", "modulename": "mplib.planner", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner", "modulename": "mplib.planner", "qualname": "Planner", "kind": "class", "doc": "
    Motion planner.
    \n"}, {"fullname": "mplib.planner.Planner.__init__", "modulename": "mplib.planner", "qualname": "Planner.__init__", "kind": "function", "doc": "
    Motion planner for robots.
    \n\n
    Args:\n    urdf: Unified Robot Description Format file.\n    user_link_names: names of links, the order. if empty, all links will be used.\n    user_joint_names: names of the joints to plan.  if empty, all active joints will be used.\n    move_group: target link to move, usually the end-effector.\n    joint_vel_limits: maximum joint velocities for time parameterization,\n        which should have the same length as\n    joint_acc_limits: maximum joint accelerations for time parameterization,\n        which should have the same length as\n    srdf: Semantic Robot Description Format file.\nReferences:\n    http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
    \n", "signature": "(\turdf: str,\tmove_group: str,\tsrdf: str = '',\tpackage_keyword_replacement: str = '',\tuser_link_names: Sequence[str] = [],\tuser_joint_names: Sequence[str] = [],\tjoint_vel_limits: Union[Sequence[float], numpy.ndarray] = [],\tjoint_acc_limits: Union[Sequence[float], numpy.ndarray] = [],\t**kwargs)"}, {"fullname": "mplib.planner.Planner.urdf", "modulename": "mplib.planner", "qualname": "Planner.urdf", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.robot", "modulename": "mplib.planner", "qualname": "Planner.robot", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.pinocchio_model", "modulename": "mplib.planner", "qualname": "Planner.pinocchio_model", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.planning_world", "modulename": "mplib.planner", "qualname": "Planner.planning_world", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.user_link_names", "modulename": "mplib.planner", "qualname": "Planner.user_link_names", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.user_joint_names", "modulename": "mplib.planner", "qualname": "Planner.user_joint_names", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.joint_name_2_idx", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.link_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.link_name_2_idx", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group", "modulename": "mplib.planner", "qualname": "Planner.move_group", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group_joint_indices", "modulename": "mplib.planner", "qualname": "Planner.move_group_joint_indices", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_types", "modulename": "mplib.planner", "qualname": "Planner.joint_types", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_vel_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_vel_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_acc_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_acc_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group_link_id", "modulename": "mplib.planner", "qualname": "Planner.move_group_link_id", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.planner", "modulename": "mplib.planner", "qualname": "Planner.planner", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.replace_package_keyword", "modulename": "mplib.planner", "qualname": "Planner.replace_package_keyword", "kind": "function", "doc": "
    some ROS URDF files use package:// keyword to refer the package dir\nreplace it with the given string (default is empty)
    \n\n
    Args:\n    package_keyword_replacement: the string to replace 'package://' keyword
    \n", "signature": "(self, package_keyword_replacement):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.generate_collision_pair", "modulename": "mplib.planner", "qualname": "Planner.generate_collision_pair", "kind": "function", "doc": "
    we read the srdf file to get the link pairs that should not collide.\nif not provided, we need to randomly sample configurations to find the link pairs that will always collide.
    \n", "signature": "(self, sample_time=1000000, echo_freq=100000):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.distance_6D", "modulename": "mplib.planner", "qualname": "Planner.distance_6D", "kind": "function", "doc": "
    compute the distance between two poses
    \n\n
    Args:\n    p1: position of pose 1\n    q1: quaternion of pose 1\n    p2: position of pose 2\n    q2: quaternion of pose 2
    \n", "signature": "(self, p1, q1, p2, q2):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_joint_limit", "modulename": "mplib.planner", "qualname": "Planner.check_joint_limit", "kind": "function", "doc": "
    check if the joint configuration is within the joint limits
    \n\n
    Args:\n    q: joint configuration
    \n\n
    Returns:\n    True if the joint configuration is within the joint limits
    \n", "signature": "(self, q):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.pad_qpos", "modulename": "mplib.planner", "qualname": "Planner.pad_qpos", "kind": "function", "doc": "
    if the user does not provide the full qpos but only the move_group joints,\npad the qpos with the rest of the joints
    \n", "signature": "(self, qpos, articulation=None):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_collision", "kind": "function", "doc": "
    helper function to check for collision
    \n", "signature": "(\tself,\tcollision_function,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_self_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_self_collision", "kind": "function", "doc": "
    Check if the robot is in self-collision.
    \n\n
    Args:\n    articulation: robot model. if none will be self.robot\n    qpos: robot configuration. if none will be the current pose
    \n\n
    Returns:\n    A list of collisions.
    \n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_env_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_env_collision", "kind": "function", "doc": "
    Check if the robot is in collision with the environment
    \n\n
    Args:\n    articulation: robot model. if none will be self.robot\n    qpos: robot configuration. if none will be the current pose\n    with_point_cloud: whether to check collision against point cloud\n    use_attach: whether to include the object attached to the end effector in collision checking\nReturns:\n    A list of collisions.
    \n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None,\twith_point_cloud=False,\tuse_attach=False) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.IK", "modulename": "mplib.planner", "qualname": "Planner.IK", "kind": "function", "doc": "
    Inverse kinematics
    \n\n
    Args:\n    goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    start_qpos: initial configuration\n    mask: if the value at a given index is True, the joint is not used in the IK\n    n_init_qpos: number of random initial configurations\n    threshold: threshold for the distance between the goal pose and the result pose
    \n", "signature": "(\tself,\tgoal_pose,\tstart_qpos,\tmask=[],\tn_init_qpos=20,\tthreshold=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.TOPP", "modulename": "mplib.planner", "qualname": "Planner.TOPP", "kind": "function", "doc": "
    Time-Optimal Path Parameterization
    \n\n
    Args:\n    path: numpy array of shape (n, dof)\n    step: step size for the discretization\n    verbose: if True, will print the log of TOPPRA
    \n", "signature": "(self, path, step=0.1, verbose=False):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_point_cloud", "modulename": "mplib.planner", "qualname": "Planner.update_point_cloud", "kind": "function", "doc": "
    Args:\n    pc: numpy array of shape (n, 3)\n    radius: radius of each point. This gives a buffer around each point that planner will avoid
    \n", "signature": "(self, pc, radius=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_tool", "modulename": "mplib.planner", "qualname": "Planner.update_attached_tool", "kind": "function", "doc": "
    helper function to update the attached tool
    \n", "signature": "(self, fcl_collision_geometry, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_sphere", "modulename": "mplib.planner", "qualname": "Planner.update_attached_sphere", "kind": "function", "doc": "
    attach a sphere to some link
    \n\n
    Args:\n    radius: radius of the sphere\n    pose: [x,y,z,qw,qx,qy,qz] pose of the sphere\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, radius, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_box", "modulename": "mplib.planner", "qualname": "Planner.update_attached_box", "kind": "function", "doc": "
    attach a box to some link
    \n\n
    Args:\n    size: [x,y,z] size of the box\n    pose: [x,y,z,qw,qx,qy,qz] pose of the box\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, size, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_mesh", "modulename": "mplib.planner", "qualname": "Planner.update_attached_mesh", "kind": "function", "doc": "
    attach a mesh to some link
    \n\n
    Args:\n    mesh_path: path to the mesh\n    pose: [x,y,z,qw,qx,qy,qz] pose of the mesh\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, mesh_path, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_base_pose", "modulename": "mplib.planner", "qualname": "Planner.set_base_pose", "kind": "function", "doc": "
    tell the planner where the base of the robot is w.r.t the world frame
    \n\n
    Args:\n    pose: [x,y,z,qw,qx,qy,qz] pose of the base
    \n", "signature": "(self, pose):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_normal_object", "modulename": "mplib.planner", "qualname": "Planner.set_normal_object", "kind": "function", "doc": "
    adds or updates a non-articulated collision object in the scene
    \n", "signature": "(self, name, collision_object):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.remove_normal_object", "modulename": "mplib.planner", "qualname": "Planner.remove_normal_object", "kind": "function", "doc": "
    returns true if the object was removed, false if it was not found
    \n", "signature": "(self, name):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_qpos", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_qpos", "kind": "function", "doc": "
    plan a path from a specified joint position to a goal pose
    \n\n
    Args:\n    goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]\n    current_qpos: current joint configuration (either full or move_group joints)\n    mask: mask for IK. When set, the IK will leave certain joints out of planning\n    time_step: time step for TOPP\n    rrt_range: step size for RRT\n    planning_time: time limit for RRT\n    fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will consider the attached tool collision when planning\n    verbose: if True, will print the log of OMPL and TOPPRA\n    planner_name: planner name pick from {\"RRTConnect\", \"RRT*\"}\n    fixed_joint_indices: list of indices of joints that are fixed during planning\n    constraint_function: evals to 0 when constraint is satisfied\n    constraint_jacobian: jacobian of constraint_function\n    constraint_tolerance: tolerance for constraint_function\n    no_simplification: if true, will not simplify the path. constraint planning does not support simplification
    \n", "signature": "(\tself,\tgoal_qposes: list,\tcurrent_qpos,\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001,\tfixed_joint_indices=[]):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_pose", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_pose", "kind": "function", "doc": "
    plan from a start configuration to a goal pose of the end-effector
    \n\n
    Args:\n    goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    current_qpos: current joint configuration (either full or move_group joints)\n    mask: if the value at a given index is True, the joint is not used in the IK\n    time_step: time step for TOPPRA (time parameterization of path)\n    rrt_range: step size for RRT\n    planning_time: time limit for RRT\n    fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will consider the attached tool collision when planning\n    verbose: if True, will print the log of OMPL and TOPPRA\n    wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
    \n", "signature": "(\tself,\tgoal_pose,\tcurrent_qpos,\tmask=[],\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\twrt_world=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_screw", "modulename": "mplib.planner", "qualname": "Planner.plan_screw", "kind": "function", "doc": "
    plan from a start configuration to a goal pose of the end-effector using screw motion
    \n\n
    Args:\n    target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    qpos: current joint configuration (either full or move_group joints)\n    qpos_step: size of the random step for RRT\n    time_step: time step for the discretization\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will use the attached tool to avoid collision\n    verbose: if True, will print the log of TOPPRA
    \n", "signature": "(\tself,\ttarget_pose,\tqpos,\tqpos_step=0.1,\ttime_step=0.1,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False):", "funcdef": "def"}, {"fullname": "mplib.pymp", "modulename": "mplib.pymp", "kind": "module", "doc": "
    Motion planning python binding. To see its documentation, please see the stub files in your IDE.
    \n"}];
+    /** pdoc search index */const docs = [{"fullname": "mplib", "modulename": "mplib", "kind": "module", "doc": "
    MPlib
    \n\n
    MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up. With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.
    \n\n
    \"documentation\"
    \n\n
    \n  \n
    \n\n
    Installation
    \n\n
    Pre-built pip packages support Ubuntu 18.04+ with Python 3.6+.
    \n\n
    pip install mplib\n
    \n\n
    Usage
    \n\n
    See our tutorial for detailed usage and examples.
    \n"}, {"fullname": "mplib.examples", "modulename": "mplib.examples", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance", "modulename": "mplib.examples.collision_avoidance", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo", "kind": "class", "doc": "
    The shows the planner's ability to generate a collision free path with the straight path causes collisions
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.__init__", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Same setup as demo.py except the boxes are of difference sizes and different use\nRed box is the target we want to grab\nBlue box is the obstacle we want to avoid\ngreen box is the landing pad on which we want to place the red box
    \n", "signature": "()"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.table", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.red_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.green_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.blue_cube", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "
    we tell the planner about the obstacle through a point cloud
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.collision_avoidance.PlanningDemo.demo", "modulename": "mplib.examples.collision_avoidance", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    We pick up the red box while avoiding the blue box and place it back down on top of the green box
    \n", "signature": "(self, with_screw=True, use_point_cloud=True, use_attach=True):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning", "modulename": "mplib.examples.constrained_planning", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo", "kind": "class", "doc": "
    This demo shows the planner's ability to plan with constraints.\nFor this particular demo, we move to several poses while pointing the end effector roughly 15 degrees w.r.t. -z axis
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.__init__", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.__init__", "kind": "function", "doc": "
    set up the scene and load the robot
    \n", "signature": "()"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.add_point_cloud", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.add_point_cloud", "kind": "function", "doc": "
    add some random obstacles to make the planning more challenging
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.get_eef_z", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.get_eef_z", "kind": "function", "doc": "
    helper function for constraint
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_f", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_f", "kind": "function", "doc": "
    create a constraint function that takes in a qpos and outputs a scalar\nA valid constraint function should evaluates to 0 when the constraint is satisfied\nSee ompl constrained planning for more details
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.make_j", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.make_j", "kind": "function", "doc": "
    create the jacobian of the constraint function w.r.t. qpos\nThis is needed because the planner uses the jacobian to project a random sample to the constraint manifold
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.constrained_planning.ConstrainedPlanningDemo.demo", "modulename": "mplib.examples.constrained_planning", "qualname": "ConstrainedPlanningDemo.demo", "kind": "function", "doc": "
    We first plan with constraints to three poses, then plan without constraints to the same poses\nWhile not always the case, sometimes without constraints, the end effector will tilt almost upside down
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo", "modulename": "mplib.examples.demo", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This is the most basic demo of the motion planning library where the robot tries to shuffle three boxes around
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.demo.PlanningDemo.__init__", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Setting up the scene, the planner, and adding some objects to the scene\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation
    \n", "signature": "()"}, {"fullname": "mplib.examples.demo.PlanningDemo.table", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.red_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.green_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.blue_cube", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.demo.PlanningDemo.demo", "modulename": "mplib.examples.demo", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    Declare three poses for the robot to move to, each one corresponding to the position of a box\nPick up the box, and set it down 0.1m to the right of its original position
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup", "modulename": "mplib.examples.demo_setup", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.demo_setup.DemoSetup", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup", "kind": "class", "doc": "
    This class is the super class to abstract away some of the setups for the demos\nYou will need to install Sapien via pip install sapien for this to work if you want to use the viewer
    \n"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.__init__", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.__init__", "kind": "function", "doc": "
    Nothing to do
    \n", "signature": "()"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_scene", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_scene", "kind": "function", "doc": "
    This is the Sapien simulator setup and has nothing to do with mplib
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.load_robot", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.load_robot", "kind": "function", "doc": "
    This function loads a robot from a URDF file into the Sapien Scene created above.\nNote that does mean that setup_scene() must be called before this function.
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.setup_planner", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.setup_planner", "kind": "function", "doc": "
    Create an mplib planner using the default robot\nsee planner.py for more details on the arguments
    \n", "signature": "(self, **kwargs):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.follow_path", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.follow_path", "kind": "function", "doc": "
    Helper function to follow a path generated by the planner
    \n", "signature": "(self, result):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.set_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.set_gripper", "kind": "function", "doc": "
    Helper function to activate gripper joints\nArgs:\n    pos: position of the gripper joint in real number
    \n", "signature": "(self, pos):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.open_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.open_gripper", "kind": "function", "doc": "
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.close_gripper", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.close_gripper", "kind": "function", "doc": "
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_RRTConnect", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_RRTConnect", "kind": "function", "doc": "
    Plan and follow a path to a pose using RRTConnect
    \n\n
    Args:\n    pose: [x, y, z, qx, qy, qz, qw]\n    use_point_cloud (optional): if to take the point cloud into consideration for collision checking.\n    use_attach (optional): if to take the attach into consideration for collision checking.
    \n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose_with_screw", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose_with_screw", "kind": "function", "doc": "
    Interpolative planning with screw motion.\nWill not avoid collision and will fail if the path contains collision.
    \n", "signature": "(self, pose, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.demo_setup.DemoSetup.move_to_pose", "modulename": "mplib.examples.demo_setup", "qualname": "DemoSetup.move_to_pose", "kind": "function", "doc": "
    API to multiplex between the two planning methods
    \n", "signature": "(self, pose, with_screw=True, use_point_cloud=False, use_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision", "modulename": "mplib.examples.detect_collision", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo", "kind": "class", "doc": "
    This demonstrates some of the collision detection functions in the planner.
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.__init__", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.__init__", "kind": "function", "doc": "
    Only the planner is needed this time. No simulation env required
    \n", "signature": "()"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.print_collisions", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.print_collisions", "kind": "function", "doc": "
    Helper function to abstract away the printing of collisions
    \n", "signature": "(self, collisions):", "funcdef": "def"}, {"fullname": "mplib.examples.detect_collision.DetectCollisionDemo.demo", "modulename": "mplib.examples.detect_collision", "qualname": "DetectCollisionDemo.demo", "kind": "function", "doc": "
    We test several configurations:
    \n\n
      \n
    1. Set robot to a self-collision-free qpos and check for self-collision returns no collision
      2. \n
    2. Set robot to a self-collision qpos and check for self-collision returns a collision
      3. \n
    3. Set robot to a env-collision-free qpos and check for env-collision returns no collision
      4. \n
    4. Set robot to a env-collision qpos and check for env-collision returns a collision
      5. \n
    5. Attempts to plan a path to a qpos is in collision with the world. This will cause the planner to timeout
      6. \n
    6. Remove the floor and check for env-collision returns no collision
      7. \n
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.moving_robot", "modulename": "mplib.examples.moving_robot", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This is identical to demo.py except the whole scene is shifted to the bottom right by 1 meter respectively
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.__init__", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    Setting up the scene, the planner, and adding some objects to the scene\nNote 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]\nAfterwards, put down a table and three boxes. For details on how to do this, see the sapien documentation\nCompared to demo.py, all the props are shifted by 1 meter in the x and y direction
    \n", "signature": "()"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.table", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.red_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.green_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.blue_cube", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.moving_robot.PlanningDemo.demo", "modulename": "mplib.examples.moving_robot", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    Same demo as demo.py.\nAlthough we shifted everything, the poses have not changed because these are w.r.t. the robot base\nAlternatively, we can also shift the poses by 1 meter in the x and y direction and tell the planner\nthe poses are specified with respect to the world
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion", "modulename": "mplib.examples.two_stage_motion", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo", "kind": "class", "doc": "
    This demo is the same as collision_avoidance.py except we added a track for the robot to move along\nWe reach the target in two stages:
    \n\n
      \n
    1. First, we move the base while fixing the arm joints
      2. \n
    2. Then, we move the arm while fixing the base joints\nThis corresponds to a mobile robot which can move in the x and y direction with a manipulator on top
      3. \n
    \n", "bases": "mplib.examples.demo_setup.DemoSetup"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.__init__", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.__init__", "kind": "function", "doc": "
    We have modified the urdf file to include a track for the robot to move along\nOtherwise, the setup is the same as collision_avoidance.py
    \n", "signature": "()"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.table", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.table", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.red_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.red_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.green_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.green_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.blue_cube", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.blue_cube", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.add_point_cloud", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.add_point_cloud", "kind": "function", "doc": "
    see collision_avoidance.py for details
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.plan_without_base", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.plan_without_base", "kind": "function", "doc": "
    a subroutine to plan a path without moving the base
    \n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.move_in_two_stage", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.move_in_two_stage", "kind": "function", "doc": "
    first, we do a full IK but only generate motions for the base\nthen, do another partial IK for the arm and generate motions for the arm
    \n", "signature": "(self, pose, has_attach=False):", "funcdef": "def"}, {"fullname": "mplib.examples.two_stage_motion.PlanningDemo.demo", "modulename": "mplib.examples.two_stage_motion", "qualname": "PlanningDemo.demo", "kind": "function", "doc": "
    We reach the pick up and drop off poses in two stages, first by moving the base only and then the arm only
    \n", "signature": "(self):", "funcdef": "def"}, {"fullname": "mplib.planner", "modulename": "mplib.planner", "kind": "module", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner", "modulename": "mplib.planner", "qualname": "Planner", "kind": "class", "doc": "
    Motion planner.
    \n"}, {"fullname": "mplib.planner.Planner.__init__", "modulename": "mplib.planner", "qualname": "Planner.__init__", "kind": "function", "doc": "
    Motion planner for robots.
    \n\n
        Args:\n        urdf: Unified Robot Description Format file.\n        user_link_names: names of links, the order. if empty, all links will be used.\n        user_joint_names: names of the joints to plan.  if empty, all active joints will be used.\n        move_group: target link to move, usually the end-effector.\n        joint_vel_limits: maximum joint velocities for time parameterization,\n            which should have the same length as\n        joint_acc_limits: maximum joint accelerations for time parameterization,\n            which should have the same length as\n        srdf: Semantic Robot Description Format file.\n    References:\n        http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html\n
    \n", "signature": "(\turdf: str,\tmove_group: str,\tsrdf: str = '',\tpackage_keyword_replacement: str = '',\tuser_link_names: Sequence[str] = [],\tuser_joint_names: Sequence[str] = [],\tjoint_vel_limits: Union[Sequence[float], numpy.ndarray] = [],\tjoint_acc_limits: Union[Sequence[float], numpy.ndarray] = [],\t**kwargs)"}, {"fullname": "mplib.planner.Planner.urdf", "modulename": "mplib.planner", "qualname": "Planner.urdf", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.robot", "modulename": "mplib.planner", "qualname": "Planner.robot", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.pinocchio_model", "modulename": "mplib.planner", "qualname": "Planner.pinocchio_model", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.planning_world", "modulename": "mplib.planner", "qualname": "Planner.planning_world", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.user_link_names", "modulename": "mplib.planner", "qualname": "Planner.user_link_names", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.user_joint_names", "modulename": "mplib.planner", "qualname": "Planner.user_joint_names", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.joint_name_2_idx", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.link_name_2_idx", "modulename": "mplib.planner", "qualname": "Planner.link_name_2_idx", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group", "modulename": "mplib.planner", "qualname": "Planner.move_group", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group_joint_indices", "modulename": "mplib.planner", "qualname": "Planner.move_group_joint_indices", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_types", "modulename": "mplib.planner", "qualname": "Planner.joint_types", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_vel_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_vel_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.joint_acc_limits", "modulename": "mplib.planner", "qualname": "Planner.joint_acc_limits", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.move_group_link_id", "modulename": "mplib.planner", "qualname": "Planner.move_group_link_id", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.planner", "modulename": "mplib.planner", "qualname": "Planner.planner", "kind": "variable", "doc": "
    \n"}, {"fullname": "mplib.planner.Planner.replace_package_keyword", "modulename": "mplib.planner", "qualname": "Planner.replace_package_keyword", "kind": "function", "doc": "
    some ROS URDF files use package:// keyword to refer the package dir\nreplace it with the given string (default is empty)
    \n\n
    Args:\n    package_keyword_replacement: the string to replace 'package://' keyword
    \n", "signature": "(self, package_keyword_replacement):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.generate_collision_pair", "modulename": "mplib.planner", "qualname": "Planner.generate_collision_pair", "kind": "function", "doc": "
    we read the srdf file to get the link pairs that should not collide.\nif not provided, we need to randomly sample configurations to find the link pairs that will always collide.
    \n", "signature": "(self, sample_time=1000000, echo_freq=100000):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.distance_6D", "modulename": "mplib.planner", "qualname": "Planner.distance_6D", "kind": "function", "doc": "
    compute the distance between two poses
    \n\n
    Args:\n    p1: position of pose 1\n    q1: quaternion of pose 1\n    p2: position of pose 2\n    q2: quaternion of pose 2
    \n", "signature": "(self, p1, q1, p2, q2):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_joint_limit", "modulename": "mplib.planner", "qualname": "Planner.check_joint_limit", "kind": "function", "doc": "
    check if the joint configuration is within the joint limits
    \n\n
    Args:\n    q: joint configuration
    \n\n
    Returns:\n    True if the joint configuration is within the joint limits
    \n", "signature": "(self, q):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.pad_qpos", "modulename": "mplib.planner", "qualname": "Planner.pad_qpos", "kind": "function", "doc": "
    if the user does not provide the full qpos but only the move_group joints,\npad the qpos with the rest of the joints
    \n", "signature": "(self, qpos, articulation=None):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_collision", "kind": "function", "doc": "
    helper function to check for collision
    \n", "signature": "(\tself,\tcollision_function,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_self_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_self_collision", "kind": "function", "doc": "
    Check if the robot is in self-collision.
    \n\n
        Args:\n        articulation: robot model. if none will be self.robot\n        qpos: robot configuration. if none will be the current pose\n\n    Returns:\n        A list of collisions.\n
    \n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.check_for_env_collision", "modulename": "mplib.planner", "qualname": "Planner.check_for_env_collision", "kind": "function", "doc": "
    Check if the robot is in collision with the environment
    \n\n
        Args:\n        articulation: robot model. if none will be self.robot\n        qpos: robot configuration. if none will be the current pose\n        with_point_cloud: whether to check collision against point cloud\n        use_attach: whether to include the object attached to the end effector in collision checking\n    Returns:\n        A list of collisions.\n
    \n", "signature": "(\tself,\tarticulation: mplib.pymp.articulation.ArticulatedModel = None,\tqpos: numpy.ndarray = None,\twith_point_cloud=False,\tuse_attach=False) -> list:", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.IK", "modulename": "mplib.planner", "qualname": "Planner.IK", "kind": "function", "doc": "
    Inverse kinematics
    \n\n
    Args:\n    goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    start_qpos: initial configuration\n    mask: if the value at a given index is True, the joint is not used in the IK\n    n_init_qpos: number of random initial configurations\n    threshold: threshold for the distance between the goal pose and the result pose
    \n", "signature": "(\tself,\tgoal_pose,\tstart_qpos,\tmask=[],\tn_init_qpos=20,\tthreshold=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.TOPP", "modulename": "mplib.planner", "qualname": "Planner.TOPP", "kind": "function", "doc": "
    Time-Optimal Path Parameterization
    \n\n
    Args:\n    path: numpy array of shape (n, dof)\n    step: step size for the discretization\n    verbose: if True, will print the log of TOPPRA
    \n", "signature": "(self, path, step=0.1, verbose=False):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_point_cloud", "modulename": "mplib.planner", "qualname": "Planner.update_point_cloud", "kind": "function", "doc": "
    Args:\n    pc: numpy array of shape (n, 3)\n    radius: radius of each point. This gives a buffer around each point that planner will avoid
    \n", "signature": "(self, pc, radius=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_tool", "modulename": "mplib.planner", "qualname": "Planner.update_attached_tool", "kind": "function", "doc": "
    helper function to update the attached tool
    \n", "signature": "(self, fcl_collision_geometry, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_sphere", "modulename": "mplib.planner", "qualname": "Planner.update_attached_sphere", "kind": "function", "doc": "
    attach a sphere to some link
    \n\n
    Args:\n    radius: radius of the sphere\n    pose: [x,y,z,qw,qx,qy,qz] pose of the sphere\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, radius, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_box", "modulename": "mplib.planner", "qualname": "Planner.update_attached_box", "kind": "function", "doc": "
    attach a box to some link
    \n\n
    Args:\n    size: [x,y,z] size of the box\n    pose: [x,y,z,qw,qx,qy,qz] pose of the box\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, size, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.update_attached_mesh", "modulename": "mplib.planner", "qualname": "Planner.update_attached_mesh", "kind": "function", "doc": "
    attach a mesh to some link
    \n\n
    Args:\n    mesh_path: path to the mesh\n    pose: [x,y,z,qw,qx,qy,qz] pose of the mesh\n    link_id: if not provided, the end effector will be the target.
    \n", "signature": "(self, mesh_path, pose, link_id=-1):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_base_pose", "modulename": "mplib.planner", "qualname": "Planner.set_base_pose", "kind": "function", "doc": "
    tell the planner where the base of the robot is w.r.t the world frame
    \n\n
    Args:\n    pose: [x,y,z,qw,qx,qy,qz] pose of the base
    \n", "signature": "(self, pose):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.set_normal_object", "modulename": "mplib.planner", "qualname": "Planner.set_normal_object", "kind": "function", "doc": "
    adds or updates a non-articulated collision object in the scene
    \n", "signature": "(self, name, collision_object):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.remove_normal_object", "modulename": "mplib.planner", "qualname": "Planner.remove_normal_object", "kind": "function", "doc": "
    returns true if the object was removed, false if it was not found
    \n", "signature": "(self, name):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_qpos", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_qpos", "kind": "function", "doc": "
    plan a path from a specified joint position to a goal pose
    \n\n
    Args:\n    goal_pose: 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]\n    current_qpos: current joint configuration (either full or move_group joints)\n    mask: mask for IK. When set, the IK will leave certain joints out of planning\n    time_step: time step for TOPP\n    rrt_range: step size for RRT\n    planning_time: time limit for RRT\n    fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will consider the attached tool collision when planning\n    verbose: if True, will print the log of OMPL and TOPPRA\n    planner_name: planner name pick from {\"RRTConnect\", \"RRT*\"}\n    fixed_joint_indices: list of indices of joints that are fixed during planning\n    constraint_function: evals to 0 when constraint is satisfied\n    constraint_jacobian: jacobian of constraint_function\n    constraint_tolerance: tolerance for constraint_function\n    no_simplification: if true, will not simplify the path. constraint planning does not support simplification
    \n", "signature": "(\tself,\tgoal_qposes: list,\tcurrent_qpos,\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001,\tfixed_joint_indices=[]):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_qpos_to_pose", "modulename": "mplib.planner", "qualname": "Planner.plan_qpos_to_pose", "kind": "function", "doc": "
    plan from a start configuration to a goal pose of the end-effector
    \n\n
    Args:\n    goal_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    current_qpos: current joint configuration (either full or move_group joints)\n    mask: if the value at a given index is True, the joint is not used in the IK\n    time_step: time step for TOPPRA (time parameterization of path)\n    rrt_range: step size for RRT\n    planning_time: time limit for RRT\n    fix_joint_limits: if True, will clip the joint configuration to be within the joint limits\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will consider the attached tool collision when planning\n    verbose: if True, will print the log of OMPL and TOPPRA\n    wrt_world: if true, interpret the target pose with respect to the world frame instead of the base frame
    \n", "signature": "(\tself,\tgoal_pose,\tcurrent_qpos,\tmask=[],\ttime_step=0.1,\trrt_range=0.1,\tplanning_time=1,\tfix_joint_limits=True,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False,\twrt_world=False,\tplanner_name='RRTConnect',\tno_simplification=False,\tconstraint_function=None,\tconstraint_jacobian=None,\tconstraint_tolerance=0.001):", "funcdef": "def"}, {"fullname": "mplib.planner.Planner.plan_screw", "modulename": "mplib.planner", "qualname": "Planner.plan_screw", "kind": "function", "doc": "
    plan from a start configuration to a goal pose of the end-effector using screw motion
    \n\n
    Args:\n    target_pose: [x,y,z,qw,qx,qy,qz] pose of the goal\n    qpos: current joint configuration (either full or move_group joints)\n    qpos_step: size of the random step for RRT\n    time_step: time step for the discretization\n    use_point_cloud: if True, will use the point cloud to avoid collision\n    use_attach: if True, will use the attached tool to avoid collision\n    verbose: if True, will print the log of TOPPRA
    \n", "signature": "(\tself,\ttarget_pose,\tqpos,\tqpos_step=0.1,\ttime_step=0.1,\tuse_point_cloud=False,\tuse_attach=False,\tverbose=False):", "funcdef": "def"}, {"fullname": "mplib.pymp", "modulename": "mplib.pymp", "kind": "module", "doc": "
    Motion planning python binding. To see its documentation, please see the stub files in your IDE.
    \n"}];
 
     // mirrored in build-search-index.js (part 1)
     // Also split on html tags. this is a cheap heuristic, but good enough.
diff --git a/mplib/__init__.pyi b/mplib/__init__.pyi
new file mode 100644
index 00000000..4fdf4e3b
--- /dev/null
+++ b/mplib/__init__.pyi
@@ -0,0 +1,11 @@
+"""
+
+.. include:: ./README.md
+"""
+from __future__ import annotations
+from importlib.metadata import version
+from mplib.planner import Planner
+from . import planner
+from . import pymp
+__all__ = ['Planner', 'planner', 'pymp', 'version']
+__version__: str = '0.0.9.dev20240115+git.50741d33.dirty'
diff --git a/mplib/planner.pyi b/mplib/planner.pyi
new file mode 100644
index 00000000..f588bf4c
--- /dev/null
+++ b/mplib/planner.pyi
@@ -0,0 +1,261 @@
+from __future__ import annotations
+from mplib.pymp import articulation
+import mplib.pymp.articulation
+from mplib.pymp import fcl
+from mplib.pymp import kdl
+from mplib.pymp import ompl
+from mplib.pymp import pinocchio
+from mplib.pymp import planning_world
+import numpy
+import numpy as np
+import os as os
+import toppra as ta
+from toppra import algorithm as algo
+from toppra import constraint
+from transforms3d.quaternions import mat2quat
+from transforms3d.quaternions import quat2mat
+__all__ = ['Planner', 'algo', 'articulation', 'constraint', 'fcl', 'kdl', 'mat2quat', 'np', 'ompl', 'os', 'pinocchio', 'planning_world', 'quat2mat', 'ta']
+class Planner:
+    """
+    Motion planner.
+    """
+    def IK(self, goal_pose, start_qpos, mask = list(), n_init_qpos = 20, threshold = 0.001):
+        """
+        
+                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
+                    n_init_qpos: number of random initial configurations
+                    threshold: threshold for the distance between the goal pose and the result pose
+                
+        """
+    def TOPP(self, path, step = 0.1, verbose = False):
+        """
+        
+                Time-Optimal Path Parameterization
+        
+                Args:
+                    path: numpy array of shape (n, dof)
+                    step: step size for the discretization
+                    verbose: if True, will print the log of TOPPRA
+                
+        """
+    def __init__(self, urdf: str, move_group: str, srdf: str = '', package_keyword_replacement: str = '', user_link_names: typing.Sequence[str] = list(), user_joint_names: typing.Sequence[str] = list(), joint_vel_limits: typing.Union[typing.Sequence[float], numpy.ndarray] = list(), joint_acc_limits: typing.Union[typing.Sequence[float], numpy.ndarray] = list(), **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.
+                    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
+                    joint_acc_limits: maximum joint accelerations for time parameterization,
+                        which should have the same length as
+                    srdf: Semantic Robot Description Format file.
+                References:
+                    http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
+        
+                
+        """
+    def check_for_collision(self, collision_function, articulation: mplib.pymp.articulation.ArticulatedModel = None, qpos: numpy.ndarray = None) -> list:
+        """
+        helper function to check for collision
+        """
+    def check_for_env_collision(self, articulation: mplib.pymp.articulation.ArticulatedModel = None, qpos: numpy.ndarray = None, with_point_cloud = False, use_attach = False) -> list:
+        """
+        Check if the robot is in collision with the environment
+        
+                Args:
+                    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
+                Returns:
+                    A list of collisions.
+                
+        """
+    def check_for_self_collision(self, articulation: mplib.pymp.articulation.ArticulatedModel = None, qpos: numpy.ndarray = None) -> list:
+        """
+        Check if the robot is in self-collision.
+        
+                Args:
+                    articulation: robot model. if none will be self.robot
+                    qpos: robot configuration. if none will be the current pose
+        
+                Returns:
+                    A list of collisions.
+                
+        """
+    def check_joint_limit(self, q):
+        """
+        
+                check if the joint configuration is within the joint limits
+        
+                Args:
+                    q: joint configuration
+        
+                Returns:
+                    True if the joint configuration is within the joint limits
+                
+        """
+    def distance_6D(self, p1, q1, p2, q2):
+        """
+        
+                compute the distance between two poses
+        
+                Args:
+                    p1: position of pose 1
+                    q1: quaternion of pose 1
+                    p2: position of pose 2
+                    q2: quaternion of pose 2
+                
+        """
+    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.
+                
+        """
+    def pad_qpos(self, qpos, articulation = None):
+        """
+        
+                if the user does not provide the full qpos but only the move_group joints,
+                pad the qpos with the rest of the joints
+                
+        """
+    def plan_qpos_to_pose(self, goal_pose, current_qpos, mask = list(), time_step = 0.1, rrt_range = 0.1, planning_time = 1, fix_joint_limits = True, use_point_cloud = False, use_attach = False, verbose = False, wrt_world = False, planner_name = 'RRTConnect', no_simplification = False, constraint_function = None, constraint_jacobian = None, constraint_tolerance = 0.001):
+        """
+        
+                plan from a start configuration to a goal pose of the end-effector
+        
+                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
+                    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
+                    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
+                
+        """
+    def plan_qpos_to_qpos(self, goal_qposes: list, current_qpos, time_step = 0.1, rrt_range = 0.1, planning_time = 1, fix_joint_limits = True, use_point_cloud = False, use_attach = False, verbose = False, planner_name = 'RRTConnect', no_simplification = False, constraint_function = None, constraint_jacobian = None, constraint_tolerance = 0.001, fixed_joint_indices = list()):
+        """
+        
+                plan a path from a specified joint position to a goal pose
+        
+                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
+                    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
+                    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
+                    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
+                
+        """
+    def plan_screw(self, target_pose, qpos, qpos_step = 0.1, time_step = 0.1, use_point_cloud = False, use_attach = False, verbose = False):
+        """
+        
+                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
+                    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
+                
+        """
+    def remove_normal_object(self, name):
+        """
+        returns true if the object was removed, false if it was not found
+        """
+    def replace_package_keyword(self, package_keyword_replacement):
+        """
+        
+                some ROS URDF files use package:// keyword to refer the package dir
+                replace it with the given string (default is empty)
+        
+                Args:
+                    package_keyword_replacement: the string to replace 'package://' keyword
+                
+        """
+    def set_base_pose(self, pose):
+        """
+        
+                tell the planner where the base of the robot is w.r.t the world frame
+        
+                Args:
+                    pose: [x,y,z,qw,qx,qy,qz] pose of the base
+                
+        """
+    def set_normal_object(self, name, collision_object):
+        """
+        adds or updates a non-articulated collision object in the scene
+        """
+    def update_attached_box(self, size, pose, link_id = -1):
+        """
+        
+                attach a box to some link
+        
+                Args:
+                    size: [x,y,z] size of the box
+                    pose: [x,y,z,qw,qx,qy,qz] pose of the box
+                    link_id: if not provided, the end effector will be the target.
+                
+        """
+    def update_attached_mesh(self, mesh_path, pose, link_id = -1):
+        """
+        
+                attach a mesh to some link
+        
+                Args:
+                    mesh_path: path to the mesh
+                    pose: [x,y,z,qw,qx,qy,qz] pose of the mesh
+                    link_id: if not provided, the end effector will be the target.
+                
+        """
+    def update_attached_sphere(self, radius, pose, link_id = -1):
+        """
+        
+                attach a sphere to some link
+        
+                Args:
+                    radius: radius of the sphere
+                    pose: [x,y,z,qw,qx,qy,qz] pose of the sphere
+                    link_id: if not provided, the end effector will be the target.
+                
+        """
+    def update_attached_tool(self, fcl_collision_geometry, pose, link_id = -1):
+        """
+        helper function to update the attached tool
+        """
+    def update_point_cloud(self, pc, radius = 0.001):
+        """
+        
+                Args:
+                    pc: numpy array of shape (n, 3)
+                    radius: radius of each point. This gives a buffer around each point that planner will avoid
+                
+        """
diff --git a/mplib/pymp/__init__.pyi b/mplib/pymp/__init__.pyi
index 006d1dab..9518217d 100644
--- a/mplib/pymp/__init__.pyi
+++ b/mplib/pymp/__init__.pyi
@@ -1,23 +1,11 @@
 """
-Motion planning python binding
+Motion planning python binding. To see its documentation, please see the stub files in your IDE.
 """
-import mplib.pymp
-import typing
-
-from mplib.pymp import articulation
-from mplib.pymp import fcl
-from mplib.pymp import kdl
-from mplib.pymp import ompl
-from mplib.pymp import pinocchio
-from mplib.pymp import planning_world
-
-__all__ = [
-    "articulation",
-    "fcl",
-    "kdl",
-    "ompl",
-    "pinocchio",
-    "planning_world"
-]
-
-
+from __future__ import annotations
+from . import articulation
+from . import fcl
+from . import kdl
+from . import ompl
+from . import pinocchio
+from . import planning_world
+__all__ = ['articulation', 'fcl', 'kdl', 'ompl', 'pinocchio', 'planning_world']
diff --git a/mplib/pymp/articulation/__init__.pyi b/mplib/pymp/articulation.pyi
similarity index 72%
rename from mplib/pymp/articulation/__init__.pyi
rename to mplib/pymp/articulation.pyi
index 3cedd8db..2cc001f8 100644
--- a/mplib/pymp/articulation/__init__.pyi
+++ b/mplib/pymp/articulation.pyi
@@ -1,21 +1,18 @@
-"""articulated model submodule, i.e. models with moving parts"""
-import mplib.pymp.articulation
-import typing
+"""
+articulated model submodule, i.e. models with moving parts
+"""
+from __future__ import annotations
 import mplib.pymp.fcl
 import mplib.pymp.pinocchio
 import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "ArticulatedModel"
-]
-
-
-class ArticulatedModel():
+import typing
+__all__ = ['ArticulatedModel']
+M = typing.TypeVar("M", bound=int)
+class ArticulatedModel:
     """
     Supports initialization from URDF and SRDF files, and provides access to underlying Pinocchio and FCL models.
     """
-    def __init__(self, urdf_filename: str, srdf_filename: str, gravity: numpy.ndarray[numpy.float64, _Shape[3, 1]], joint_names: list[str], link_names: list[str], verbose: bool = True, convex: bool = False) -> None: 
+    def __init__(self, urdf_filename: str, srdf_filename: str, gravity: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], joint_names: list[str], link_names: list[str], verbose: bool = True, convex: bool = False) -> None:
         """
             Construct an articulated model from URDF and SRDF files.
             Args:
@@ -27,97 +24,96 @@ class ArticulatedModel():
                 verbose: print debug information
                 convex: use convex decomposition for collision objects
         """
-    def get_base_pose(self) -> numpy.ndarray[numpy.float64, _Shape[7, 1]]: 
+    def get_base_pose(self) -> numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]:
         """
             Get the base pose of the robot.
             Returns:
                 base pose of the robot in [x, y, z, qw, qx, qy, qz] format
         """
-    def get_fcl_model(self) -> mplib.pymp.fcl.FCLModel: 
+    def get_fcl_model(self) -> mplib.pymp.fcl.FCLModel:
         """
             Get the underlying FCL model.
             Returns:
                 FCL model used for collision checking
         """
-    def get_move_group_end_effectors(self) -> list[str]: 
+    def get_move_group_end_effectors(self) -> list[str]:
         """
             Get the end effectors of the move group.
             Returns:
                 list of end effectors of the move group
         """
-    def get_move_group_joint_indices(self) -> list[int]: 
+    def get_move_group_joint_indices(self) -> list[int]:
         """
             Get the joint indices of the move group.
             Returns:
                 list of user joint indices of the move group
         """
-    def get_move_group_joint_names(self) -> list[str]: 
+    def get_move_group_joint_names(self) -> list[str]:
         """
             Get the joint names of the move group.
             Returns:
                 list of joint names of the move group
         """
-    def get_move_group_qpos_dim(self) -> int: 
+    def get_move_group_qpos_dim(self) -> int:
         """
             Get the dimension of the move group qpos.
             Returns:
                 dimension of the move group qpos
         """
-    def get_pinocchio_model(self) -> mplib.pymp.pinocchio.PinocchioModel: 
+    def get_pinocchio_model(self) -> mplib.pymp.pinocchio.PinocchioModel:
         """
             Get the underlying Pinocchio model.
             Returns:
                 Pinocchio model used for kinematics and dynamics computations
         """
-    def get_qpos(self) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
+    def get_qpos(self) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
         """
             Get the current joint position of all active joints inside the URDF.
             Returns:
                 current qpos of all active joints
         """
-    def get_user_joint_names(self) -> list[str]: 
+    def get_user_joint_names(self) -> list[str]:
         """
             Get the joint names that the user has provided for planning.
             Returns:
                 list of joint names of the user
         """
-    def get_user_link_names(self) -> list[str]: 
+    def get_user_link_names(self) -> list[str]:
         """
             Get the link names that the user has provided for planning.
             Returns:
                 list of link names of the user
         """
-    def set_base_pose(self, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: 
+    def set_base_pose(self, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Set the base pose of the robot.
             Args:
                 pose: base pose of the robot in [x, y, z, qw, qx, qy, qz] format
         """
     @typing.overload
-    def set_move_group(self, end_effector: str) -> None: 
+    def set_move_group(self, end_effector: str) -> None:
         """
             Set the move group, i.e. the chain ending in end effector for which to compute the forward kinematics for all subsequent queries.
             Args:
                 chain: list of links extending to the end effector
-
-
+        """
+    @typing.overload
+    def set_move_group(self, end_effectors: list[str]) -> None:
+        """
             Set the move group but we have multiple end effectors in a chain. i.e. Base --> EE1 --> EE2 --> ... --> EEn
             Args:
                 end_effectors: names of the end effector link
         """
-    @typing.overload
-    def set_move_group(self, end_effectors: list[str]) -> None: ...
-    def set_qpos(self, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]], full: bool = False) -> None: 
+    def set_qpos(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], full: bool = False) -> None:
         """
             Let the planner know the current joint positions.
             Args:
                 qpos: current qpos of all active joints or just the move group joints
                 full: whether to set the full qpos or just the move group qpos. if full false, we will pad the missing joints with current known qpos. the default is false
         """
-    def update_SRDF(self, SRDF: str) -> None: 
+    def update_SRDF(self, SRDF: str) -> None:
         """
             Update the SRDF file to disable self-collisions.
             Args:
                 srdf: path to SRDF file, can be relative to the current working directory
         """
-    pass
diff --git a/mplib/pymp/fcl.pyi b/mplib/pymp/fcl.pyi
new file mode 100644
index 00000000..cccf7120
--- /dev/null
+++ b/mplib/pymp/fcl.pyi
@@ -0,0 +1,445 @@
+from __future__ import annotations
+import numpy
+import typing
+__all__ = ['BVHModel', 'Box', 'Capsule', 'CollisionGeometry', 'CollisionObject', 'CollisionRequest', 'CollisionResult', 'Contact', 'ContactPoint', 'Convex', 'CostSource', 'Cylinder', 'DistanceRequest', 'DistanceResult', 'FCLModel', 'GJKSolverType', 'GST_INDEP', 'GST_LIBCCD', 'OcTree', 'Triangle', 'collide', 'distance', 'load_mesh_as_BVH', 'load_mesh_as_Convex']
+M = typing.TypeVar("M", bound=int)
+class BVHModel(CollisionGeometry):
+    """
+    
+        BVHModel collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a mesh geometry represented by a BVH tree.
+    """
+    def __init__(self) -> None:
+        ...
+    @typing.overload
+    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]) -> int:
+        """
+            Add a sub-model to the BVHModel.
+            Args:
+                vertices: vertices of the sub-model
+                faces: faces of the sub-model represented by a list of vertex indices
+        """
+    @typing.overload
+    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]], faces: list[Triangle]) -> int:
+        """
+            Add a sub-model to the BVHModel.
+            Args:
+                vertices: vertices of the sub-model
+                faces: faces of the sub-model represented by a list of vertex indices
+        """
+    @typing.overload
+    def addSubModel(self, vertices: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]], faces: list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.int32]]]) -> None:
+        ...
+    def beginModel(self, num_faces: int = 0, num_vertices: int = 0) -> int:
+        """
+            Begin to construct a BVHModel.
+            Args:
+                num_faces: number of faces of the mesh
+                num_vertices: number of vertices of the mesh
+        """
+    def endModel(self) -> int:
+        """
+            End the construction of a BVHModel.
+        """
+    def get_faces(self) -> list[Triangle]:
+        """
+            Get the faces of the BVHModel.
+            Returns:
+                faces of the BVHModel
+        """
+    def get_vertices(self) -> list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
+        """
+            Get the vertices of the BVHModel.
+            Returns:
+                vertices of the BVHModel
+        """
+    @property
+    def num_faces(self) -> int:
+        ...
+    @property
+    def num_vertices(self) -> int:
+        ...
+class Box(CollisionGeometry):
+    """
+    
+        Box collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a box geometry.
+    """
+    side: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]
+    @typing.overload
+    def __init__(self, side: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+        """
+            Construct a box with given side length.
+            Args:
+                side: side length of the box in an array [x, y, z]
+        """
+    @typing.overload
+    def __init__(self, x: float, y: float, z: float) -> None:
+        """
+            Construct a box with given side length.
+            Args:
+                x: side length of the box in x direction
+                y: side length of the box in y direction
+                z: side length of the box in z direction
+        """
+class Capsule(CollisionGeometry):
+    """
+    
+        Capsule collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a capsule geometry.
+    """
+    lz: float
+    radius: float
+    def __init__(self, radius: float, lz: float) -> None:
+        """
+            Construct a capsule with given radius and height.
+            Args:
+                radius: radius of the capsule
+                lz: height of the capsule
+        """
+class CollisionGeometry:
+    """
+    
+        Collision geometry base class.
+        This is an FCL class so you can refer to the FCL doc here https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html
+    """
+    aabb_center: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]
+    aabb_radius: float
+    cost_density: float
+    def computeCOM(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    def computeLocalAABB(self) -> None:
+        ...
+    def computeMomentofInertia(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
+        ...
+    def computeMomentofInertiaRelatedToCOM(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
+        ...
+    def computeVolume(self) -> float:
+        ...
+    def isFree(self) -> bool:
+        ...
+    def isOccupied(self) -> bool:
+        ...
+    def isUncertain(self) -> bool:
+        ...
+class CollisionObject:
+    """
+    
+        Collision object class.
+        This class contains the collision geometry and the transformation of the geometry.
+    """
+    def __init__(self, collision_geometry: CollisionGeometry, translation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], rotation: numpy.ndarray[tuple[typing.Literal[4], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+        """
+            Construct a collision object with given collision geometry and transformation.
+            Args:
+                collision_geometry: collision geometry of the object
+                translation: translation of the object
+                rotation: rotation of the object
+        """
+    def get_collision_geometry(self) -> CollisionGeometry:
+        ...
+    def get_rotation(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]:
+        ...
+    def get_translation(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    def set_transformation(self, arg0: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+        ...
+class CollisionRequest:
+    def __init__(self, num_max_contacts: int = 1, enable_contact: bool = False, num_max_cost_sources: int = 1, enable_cost: bool = False, use_approximate_cost: bool = True, gjk_solver_type: GJKSolverType = ..., gjk_tolerance: float = 1e-06) -> None:
+        ...
+    def isSatisfied(self, result: ...) -> bool:
+        ...
+class CollisionResult:
+    def __init__(self) -> None:
+        ...
+    def add_contact(self, c: ...) -> None:
+        ...
+    def add_cost_source(self, c: ..., num_max_cost_sources: int) -> None:
+        ...
+    def clear(self) -> None:
+        ...
+    def get_contact(self, i: int) -> ...:
+        ...
+    def get_contacts(self) -> list[...]:
+        ...
+    def get_cost_sources(self) -> list[...]:
+        ...
+    def is_collision(self) -> bool:
+        ...
+    def num_contacts(self) -> int:
+        ...
+    def num_cost_sources(self) -> int:
+        ...
+class Contact:
+    @typing.overload
+    def __init__(self) -> None:
+        ...
+    @typing.overload
+    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int) -> None:
+        ...
+    @typing.overload
+    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int, pos: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], normal: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], depth: float) -> None:
+        ...
+    @property
+    def normal(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    @property
+    def penetration_depth(self) -> float:
+        ...
+    @property
+    def pos(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+class ContactPoint:
+    @typing.overload
+    def __init__(self) -> None:
+        ...
+    @typing.overload
+    def __init__(self, normal: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], pos: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], penetration_depth: float) -> None:
+        ...
+    @property
+    def normal(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    @property
+    def penetration_depth(self) -> float:
+        ...
+    @property
+    def pos(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+class Convex(CollisionGeometry):
+    """
+    
+        Convex collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a convex geometry.
+    """
+    @staticmethod
+    @typing.overload
+    def __init__(*args, **kwargs) -> None:
+        """
+            Construct a convex with given vertices and faces.
+            Args:
+                vertices: vertices of the convex
+                num_faces: number of faces of the convex
+                faces: faces of the convex geometry represented by a list of vertex indices
+                throw_if_invalid: if true, throw an exception if the convex is invalid
+        """
+    @typing.overload
+    def __init__(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], faces: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.int32]], throw_if_invalid: bool = True) -> None:
+        """
+            Construct a convex with given vertices and faces.
+            Args:
+                vertices: vertices of the convex
+                faces: faces of the convex geometry represented by a list of vertex indices
+                throw_if_invalid: if true, throw an exception if the convex is invalid
+        """
+    def compute_volume(self) -> float:
+        """
+            Compute the volume of the convex.
+            Returns:
+                volume of the convex
+        """
+    def get_face_count(self) -> int:
+        """
+            Get the number of faces of the convex.
+            Returns:
+                number of faces of the convex
+        """
+    def get_faces(self) -> list[int]:
+        """
+            Get the faces of the convex.
+            Returns:
+                faces of the convex represented by a list of vertex indices
+        """
+    def get_interior_point(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        """
+            Sample a random interior point of the convex geometry
+            Returns:
+                interior point of the convex
+        """
+    def get_vertices(self) -> list[numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
+        """
+            Get the vertices of the convex.
+            Returns:
+                vertices of the convex
+        """
+class CostSource:
+    @typing.overload
+    def __init__(self) -> None:
+        ...
+    @typing.overload
+    def __init__(self, aabb_min: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], aabb_max: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], cost_density: float) -> None:
+        ...
+    @property
+    def aabb_max(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    @property
+    def aabb_min(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+    @property
+    def cost_density(self) -> float:
+        ...
+    @property
+    def total_cost(self) -> float:
+        ...
+class Cylinder(CollisionGeometry):
+    """
+    
+        Cylinder collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a cylinder geometry.
+    """
+    lz: float
+    radius: float
+    def __init__(self, radius: float, lz: float) -> None:
+        """
+            Construct a cylinder with given radius and height.
+            Args:
+                radius: radius of the cylinder
+                lz: height of the cylinder
+        """
+class DistanceRequest:
+    def __init__(self, enable_nearest_points: bool = False, enable_signed_distance: bool = False, rel_err: float = 0.0, abs_err: float = 0.0, distance_tolerance: float = 1e-06, gjk_solver_type: GJKSolverType = ...) -> None:
+        ...
+    def isSatisfied(self, result: ...) -> bool:
+        ...
+class DistanceResult:
+    def __init__(self, min_distance: float = 1.7976931348623157e+308) -> None:
+        ...
+    def clear(self) -> None:
+        ...
+    @property
+    def min_distance(self) -> float:
+        ...
+    @property
+    def nearest_points(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
+class FCLModel:
+    def __init__(self, urdf_filename: str, verbose: bool = True, convex: bool = False) -> None:
+        """
+            Construct an FCL model from URDF and SRDF files.
+            Args:
+                urdf_filename: path to URDF file, can be relative to the current working directory
+                verbose: print debug information
+                convex: use convex decomposition for collision objects
+        """
+    def collide(self, request: CollisionRequest = ...) -> bool:
+        """
+            Perform collision checking.
+            Args:
+                request: collision request
+            Returns:
+                true if collision happens
+        """
+    def collide_full(self, request: CollisionRequest = ...) -> list[CollisionResult]:
+        ...
+    def get_collision_link_names(self) -> list[str]:
+        ...
+    def get_collision_objects(self) -> list[CollisionObject]:
+        """
+            Get the collision objects of the FCL model.
+            Returns:
+                all collision objects of the FCL model
+        """
+    def get_collision_pairs(self) -> list[tuple[int, int]]:
+        """
+            Get the collision pairs of the FCL model.
+            Returns:
+                collision pairs of the FCL model. if the FCL model has N collision objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs
+        """
+    def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None:
+        """
+            Remove collision pairs from SRDF.
+            Args:
+                srdf_filename: path to SRDF file, can be relative to the current working directory
+        """
+    def set_link_order(self, names: list[str]) -> None:
+        """
+            Set the link order of the FCL model.
+            Args:
+                names: list of link names in the order that you want to set.
+        """
+    def update_collision_objects(self, link_poses: list[numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]]) -> None:
+        """
+            Update the collision objects of the FCL model.
+            Args:
+                link_poses: list of link poses in the order of the link order
+        """
+class GJKSolverType:
+    """
+    Members:
+    
+      GST_LIBCCD
+    
+      GST_INDEP
+    """
+    GST_INDEP: typing.ClassVar[GJKSolverType]  # value = 
+    GST_LIBCCD: typing.ClassVar[GJKSolverType]  # value = 
+    __members__: typing.ClassVar[dict[str, GJKSolverType]]  # value = {'GST_LIBCCD': , 'GST_INDEP': }
+    def __eq__(self, other: typing.Any) -> bool:
+        ...
+    def __getstate__(self) -> int:
+        ...
+    def __hash__(self) -> int:
+        ...
+    def __index__(self) -> int:
+        ...
+    def __init__(self, value: int) -> None:
+        ...
+    def __int__(self) -> int:
+        ...
+    def __ne__(self, other: typing.Any) -> bool:
+        ...
+    def __repr__(self) -> str:
+        ...
+    def __setstate__(self, state: int) -> None:
+        ...
+    def __str__(self) -> str:
+        ...
+    @property
+    def name(self) -> str:
+        ...
+    @property
+    def value(self) -> int:
+        ...
+class OcTree(CollisionGeometry):
+    """
+    
+        OcTree collision geometry.
+        Inheriting from CollisionGeometry, this class specializes to a point cloud geometry represented by an Octree.
+    """
+    @typing.overload
+    def __init__(self, resolution: float) -> None:
+        """
+            Construct an OcTree with given resolution.
+            Args:
+                resolution: resolution of the OcTree (smallest size of a voxel). you can treat this is as the diameter of a point
+        """
+    @typing.overload
+    def __init__(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], resolution: float) -> None:
+        """
+            Construct an OcTree with given vertices and resolution.
+            Args:
+                vertices: vertices of the point cloud
+                resolution: resolution of the OcTree
+        """
+class Triangle:
+    def __getitem__(self, arg0: int) -> int:
+        ...
+    @typing.overload
+    def __init__(self) -> None:
+        ...
+    @typing.overload
+    def __init__(self, arg0: int, arg1: int, arg2: int) -> None:
+        ...
+    def get(self, arg0: int) -> int:
+        ...
+    def set(self, arg0: int, arg1: int, arg2: int) -> None:
+        ...
+def collide(arg0: CollisionObject, arg1: CollisionObject, arg2: CollisionRequest) -> CollisionResult:
+    ...
+def distance(arg0: CollisionObject, arg1: CollisionObject, arg2: DistanceRequest) -> DistanceResult:
+    ...
+def load_mesh_as_BVH(mesh_path: str, scale: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> BVHModel:
+    ...
+def load_mesh_as_Convex(mesh_path: str, scale: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> Convex:
+    ...
+GST_INDEP: GJKSolverType  # value = 
+GST_LIBCCD: GJKSolverType  # value = 
diff --git a/mplib/pymp/fcl/__init__.pyi b/mplib/pymp/fcl/__init__.pyi
deleted file mode 100644
index 9a4ee3a4..00000000
--- a/mplib/pymp/fcl/__init__.pyi
+++ /dev/null
@@ -1,525 +0,0 @@
-import mplib.pymp.fcl
-import typing
-import GJKSolverType
-import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "BVHModel",
-    "Box",
-    "Capsule",
-    "CollisionGeometry",
-    "CollisionObject",
-    "CollisionRequest",
-    "CollisionResult",
-    "Contact",
-    "ContactPoint",
-    "Convex",
-    "CostSource",
-    "Cylinder",
-    "DistanceRequest",
-    "DistanceResult",
-    "FCLModel",
-    "GJKSolverType",
-    "GST_INDEP",
-    "GST_LIBCCD",
-    "OcTree",
-    "Triangle",
-    "collide",
-    "distance",
-    "load_mesh_as_BVH",
-    "load_mesh_as_Convex"
-]
-
-
-class CollisionGeometry():
-    """
-        Collision geometry base class.
-        This is an FCL class so you can refer to the FCL doc here https://flexible-collision-library.github.io/d6/d5d/classfcl_1_1CollisionGeometry.html
-    """
-    def computeCOM(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: ...
-    def computeLocalAABB(self) -> None: ...
-    def computeMomentofInertia(self) -> numpy.ndarray[numpy.float64, _Shape[3, 3]]: ...
-    def computeMomentofInertiaRelatedToCOM(self) -> numpy.ndarray[numpy.float64, _Shape[3, 3]]: ...
-    def computeVolume(self) -> float: ...
-    def isFree(self) -> bool: ...
-    def isOccupied(self) -> bool: ...
-    def isUncertain(self) -> bool: ...
-    @property
-    def aabb_center(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @aabb_center.setter
-    def aabb_center(self, arg0: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> None:
-        pass
-    @property
-    def aabb_radius(self) -> float:
-        """
-        :type: float
-        """
-    @aabb_radius.setter
-    def aabb_radius(self, arg0: float) -> None:
-        pass
-    @property
-    def cost_density(self) -> float:
-        """
-        :type: float
-        """
-    @cost_density.setter
-    def cost_density(self, arg0: float) -> None:
-        pass
-    pass
-class Box(CollisionGeometry):
-    """
-        Box collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a box geometry.
-    """
-    @typing.overload
-    def __init__(self, side: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> None: 
-        """
-            Construct a box with given side length.
-            Args:
-                side: side length of the box in an array [x, y, z]
-
-
-            Construct a box with given side length.
-            Args:
-                x: side length of the box in x direction
-                y: side length of the box in y direction
-                z: side length of the box in z direction
-        """
-    @typing.overload
-    def __init__(self, x: float, y: float, z: float) -> None: ...
-    @property
-    def side(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @side.setter
-    def side(self, arg0: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> None:
-        pass
-    pass
-class Capsule(CollisionGeometry):
-    """
-        Capsule collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a capsule geometry.
-    """
-    def __init__(self, radius: float, lz: float) -> None: 
-        """
-            Construct a capsule with given radius and height.
-            Args:
-                radius: radius of the capsule
-                lz: height of the capsule
-        """
-    @property
-    def lz(self) -> float:
-        """
-        :type: float
-        """
-    @lz.setter
-    def lz(self, arg0: float) -> None:
-        pass
-    @property
-    def radius(self) -> float:
-        """
-        :type: float
-        """
-    @radius.setter
-    def radius(self, arg0: float) -> None:
-        pass
-    pass
-class BVHModel(CollisionGeometry):
-    """
-        BVHModel collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a mesh geometry represented by a BVH tree.
-    """
-    def __init__(self) -> None: ...
-    @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[numpy.float64, _Shape[3, 1]]]) -> int: 
-        """
-            Add a sub-model to the BVHModel.
-            Args:
-                vertices: vertices of the sub-model
-                faces: faces of the sub-model represented by a list of vertex indices
-
-
-            Add a sub-model to the BVHModel.
-            Args:
-                vertices: vertices of the sub-model
-                faces: faces of the sub-model represented by a list of vertex indices
-        """
-    @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[numpy.float64, _Shape[3, 1]]], faces: list[Triangle]) -> int: ...
-    @typing.overload
-    def addSubModel(self, vertices: list[numpy.ndarray[numpy.float64, _Shape[3, 1]]], faces: list[numpy.ndarray[numpy.int32, _Shape[3, 1]]]) -> None: ...
-    def beginModel(self, num_faces: int = 0, num_vertices: int = 0) -> int: 
-        """
-            Begin to construct a BVHModel.
-            Args:
-                num_faces: number of faces of the mesh
-                num_vertices: number of vertices of the mesh
-        """
-    def endModel(self) -> int: 
-        """
-            End the construction of a BVHModel.
-        """
-    def get_faces(self) -> list[Triangle]: 
-        """
-            Get the faces of the BVHModel.
-            Returns:
-                faces of the BVHModel
-        """
-    def get_vertices(self) -> list[numpy.ndarray[numpy.float64, _Shape[3, 1]]]: 
-        """
-            Get the vertices of the BVHModel.
-            Returns:
-                vertices of the BVHModel
-        """
-    @property
-    def num_faces(self) -> int:
-        """
-        :type: int
-        """
-    @property
-    def num_vertices(self) -> int:
-        """
-        :type: int
-        """
-    pass
-class CollisionObject():
-    """
-        Collision object class.
-        This class contains the collision geometry and the transformation of the geometry.
-    """
-    def __init__(self, collision_geometry: CollisionGeometry, translation: numpy.ndarray[numpy.float64, _Shape[3, 1]], rotation: numpy.ndarray[numpy.float64, _Shape[4, 1]]) -> None: 
-        """
-            Construct a collision object with given collision geometry and transformation.
-            Args:
-                collision_geometry: collision geometry of the object
-                translation: translation of the object
-                rotation: rotation of the object
-        """
-    def get_collision_geometry(self) -> CollisionGeometry: ...
-    def get_rotation(self) -> numpy.ndarray[numpy.float64, _Shape[3, 3]]: ...
-    def get_translation(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: ...
-    def set_transformation(self, arg0: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
-    pass
-class CollisionRequest():
-    def __init__(self, num_max_contacts: int = 1, enable_contact: bool = False, num_max_cost_sources: int = 1, enable_cost: bool = False, use_approximate_cost: bool = True, gjk_solver_type: GJKSolverType = GJKSolverType.GST_LIBCCD, gjk_tolerance: float = 1e-06) -> None: ...
-    @staticmethod
-    def isSatisfied(*args, **kwargs) -> typing.Any: ...
-    pass
-class CollisionResult():
-    def __init__(self) -> None: ...
-    @staticmethod
-    def add_contact(*args, **kwargs) -> typing.Any: ...
-    @staticmethod
-    def add_cost_source(*args, **kwargs) -> typing.Any: ...
-    def clear(self) -> None: ...
-    @staticmethod
-    def get_contact(*args, **kwargs) -> typing.Any: ...
-    @staticmethod
-    def get_contacts(*args, **kwargs) -> typing.Any: ...
-    @staticmethod
-    def get_cost_sources(*args, **kwargs) -> typing.Any: ...
-    def is_collision(self) -> bool: ...
-    def num_contacts(self) -> int: ...
-    def num_cost_sources(self) -> int: ...
-    pass
-class Contact():
-    @typing.overload
-    def __init__(self) -> None: ...
-    @typing.overload
-    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int) -> None: ...
-    @typing.overload
-    def __init__(self, o1: CollisionGeometry, o2: CollisionGeometry, b1: int, b2: int, pos: numpy.ndarray[numpy.float64, _Shape[3, 1]], normal: numpy.ndarray[numpy.float64, _Shape[3, 1]], depth: float) -> None: ...
-    @property
-    def normal(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @property
-    def penetration_depth(self) -> float:
-        """
-        :type: float
-        """
-    @property
-    def pos(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    pass
-class ContactPoint():
-    @typing.overload
-    def __init__(self) -> None: ...
-    @typing.overload
-    def __init__(self, normal: numpy.ndarray[numpy.float64, _Shape[3, 1]], pos: numpy.ndarray[numpy.float64, _Shape[3, 1]], penetration_depth: float) -> None: ...
-    @property
-    def normal(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @property
-    def penetration_depth(self) -> float:
-        """
-        :type: float
-        """
-    @property
-    def pos(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    pass
-class Convex(CollisionGeometry):
-    """
-        Convex collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a convex geometry.
-    """
-    @staticmethod
-    @typing.overload
-    def __init__(*args, **kwargs) -> typing.Any: 
-        """
-            Construct a convex with given vertices and faces.
-            Args:
-                vertices: vertices of the convex
-                num_faces: number of faces of the convex
-                faces: faces of the convex geometry represented by a list of vertex indices
-                throw_if_invalid: if true, throw an exception if the convex is invalid
-
-
-            Construct a convex with given vertices and faces.
-            Args:
-                vertices: vertices of the convex
-                faces: faces of the convex geometry represented by a list of vertex indices
-                throw_if_invalid: if true, throw an exception if the convex is invalid
-        """
-    @typing.overload
-    def __init__(self, vertices: numpy.ndarray[numpy.float64, _Shape[m, 3]], faces: numpy.ndarray[numpy.int32, _Shape[m, 3]], throw_if_invalid: bool = True) -> None: ...
-    def compute_volume(self) -> float: 
-        """
-            Compute the volume of the convex.
-            Returns:
-                volume of the convex
-        """
-    def get_face_count(self) -> int: 
-        """
-            Get the number of faces of the convex.
-            Returns:
-                number of faces of the convex
-        """
-    def get_faces(self) -> list[int]: 
-        """
-            Get the faces of the convex.
-            Returns:
-                faces of the convex represented by a list of vertex indices
-        """
-    def get_interior_point(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: 
-        """
-            Sample a random interior point of the convex geometry
-            Returns:
-                interior point of the convex
-        """
-    def get_vertices(self) -> list[numpy.ndarray[numpy.float64, _Shape[3, 1]]]: 
-        """
-            Get the vertices of the convex.
-            Returns:
-                vertices of the convex
-        """
-    pass
-class CostSource():
-    @typing.overload
-    def __init__(self) -> None: ...
-    @typing.overload
-    def __init__(self, aabb_min: numpy.ndarray[numpy.float64, _Shape[3, 1]], aabb_max: numpy.ndarray[numpy.float64, _Shape[3, 1]], cost_density: float) -> None: ...
-    @property
-    def aabb_max(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @property
-    def aabb_min(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    @property
-    def cost_density(self) -> float:
-        """
-        :type: float
-        """
-    @property
-    def total_cost(self) -> float:
-        """
-        :type: float
-        """
-    pass
-class Cylinder(CollisionGeometry):
-    """
-        Cylinder collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a cylinder geometry.
-    """
-    def __init__(self, radius: float, lz: float) -> None: 
-        """
-            Construct a cylinder with given radius and height.
-            Args:
-                radius: radius of the cylinder
-                lz: height of the cylinder
-        """
-    @property
-    def lz(self) -> float:
-        """
-        :type: float
-        """
-    @lz.setter
-    def lz(self, arg0: float) -> None:
-        pass
-    @property
-    def radius(self) -> float:
-        """
-        :type: float
-        """
-    @radius.setter
-    def radius(self, arg0: float) -> None:
-        pass
-    pass
-class DistanceRequest():
-    def __init__(self, enable_nearest_points: bool = False, enable_signed_distance: bool = False, rel_err: float = 0.0, abs_err: float = 0.0, distance_tolerance: float = 1e-06, gjk_solver_type: GJKSolverType = GJKSolverType.GST_LIBCCD) -> None: ...
-    @staticmethod
-    def isSatisfied(*args, **kwargs) -> typing.Any: ...
-    pass
-class DistanceResult():
-    def __init__(self, min_distance: float = 1.7976931348623157e+308) -> None: ...
-    def clear(self) -> None: ...
-    @property
-    def min_distance(self) -> float:
-        """
-        :type: float
-        """
-    @property
-    def nearest_points(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]:
-        """
-        :type: numpy.ndarray[numpy.float64, _Shape[3, 1]]
-        """
-    pass
-class FCLModel():
-    def __init__(self, urdf_filename: str, verbose: bool = True, convex: bool = False) -> None: 
-        """
-            Construct an FCL model from URDF and SRDF files.
-            Args:
-                urdf_filename: path to URDF file, can be relative to the current working directory
-                verbose: print debug information
-                convex: use convex decomposition for collision objects
-        """
-    def collide(self, request: CollisionRequest = ...) -> bool: 
-        """
-            Perform collision checking.
-            Args:
-                request: collision request
-            Returns:
-                true if collision happens
-        """
-    def collide_full(self, request: CollisionRequest = ...) -> list[CollisionResult]: ...
-    def get_collision_link_names(self) -> list[str]: ...
-    def get_collision_objects(self) -> list[CollisionObject]: 
-        """
-            Get the collision objects of the FCL model.
-            Returns:
-                all collision objects of the FCL model
-        """
-    def get_collision_pairs(self) -> list[tuple[int, int]]: 
-        """
-            Get the collision pairs of the FCL model.
-            Returns:
-                collision pairs of the FCL model. if the FCL model has N collision objects, the collision pairs is a list of N*(N-1)/2 pairs minus the disabled collision pairs
-        """
-    def remove_collision_pairs_from_srdf(self, srdf_filename: str) -> None: 
-        """
-            Remove collision pairs from SRDF.
-            Args:
-                srdf_filename: path to SRDF file, can be relative to the current working directory
-        """
-    def set_link_order(self, names: list[str]) -> None: 
-        """
-            Set the link order of the FCL model.
-            Args:
-                names: list of link names in the order that you want to set.
-        """
-    def update_collision_objects(self, link_poses: list[numpy.ndarray[numpy.float64, _Shape[7, 1]]]) -> None: 
-        """
-            Update the collision objects of the FCL model.
-            Args:
-                link_poses: list of link poses in the order of the link order
-        """
-    pass
-class GJKSolverType():
-    """
-    Members:
-
-      GST_LIBCCD
-
-      GST_INDEP
-    """
-    def __eq__(self, other: object) -> bool: ...
-    def __getstate__(self) -> int: ...
-    def __hash__(self) -> int: ...
-    def __index__(self) -> int: ...
-    def __init__(self, value: int) -> None: ...
-    def __int__(self) -> int: ...
-    def __ne__(self, other: object) -> bool: ...
-    def __repr__(self) -> str: ...
-    def __setstate__(self, state: int) -> None: ...
-    def __str__(self) -> str: ...
-    @property
-    def name(self) -> str:
-        """
-        :type: str
-        """
-    @property
-    def value(self) -> int:
-        """
-        :type: int
-        """
-    GST_INDEP: mplib.pymp.fcl.GJKSolverType # value = 
-    GST_LIBCCD: mplib.pymp.fcl.GJKSolverType # value = 
-    __members__: dict # value = {'GST_LIBCCD': , 'GST_INDEP': }
-    pass
-class OcTree(CollisionGeometry):
-    """
-        OcTree collision geometry.
-        Inheriting from CollisionGeometry, this class specializes to a point cloud geometry represented by an Octree.
-    """
-    @typing.overload
-    def __init__(self, resolution: float) -> None: 
-        """
-            Construct an OcTree with given resolution.
-            Args:
-                resolution: resolution of the OcTree (smallest size of a voxel). you can treat this is as the diameter of a point
-
-
-            Construct an OcTree with given vertices and resolution.
-            Args:
-                vertices: vertices of the point cloud
-                resolution: resolution of the OcTree
-        """
-    @typing.overload
-    def __init__(self, vertices: numpy.ndarray[numpy.float64, _Shape[m, 3]], resolution: float) -> None: ...
-    pass
-class Triangle():
-    def __getitem__(self, arg0: int) -> int: ...
-    @typing.overload
-    def __init__(self) -> None: ...
-    @typing.overload
-    def __init__(self, arg0: int, arg1: int, arg2: int) -> None: ...
-    def get(self, arg0: int) -> int: ...
-    def set(self, arg0: int, arg1: int, arg2: int) -> None: ...
-    pass
-def collide(arg0: CollisionObject, arg1: CollisionObject, arg2: CollisionRequest) -> CollisionResult:
-    pass
-def distance(arg0: CollisionObject, arg1: CollisionObject, arg2: DistanceRequest) -> DistanceResult:
-    pass
-def load_mesh_as_BVH(mesh_path: str, scale: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> BVHModel:
-    pass
-def load_mesh_as_Convex(mesh_path: str, scale: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> Convex:
-    pass
-GST_INDEP: mplib.pymp.fcl.GJKSolverType # value = 
-GST_LIBCCD: mplib.pymp.fcl.GJKSolverType # value = 
diff --git a/mplib/pymp/kdl.pyi b/mplib/pymp/kdl.pyi
new file mode 100644
index 00000000..45876a3b
--- /dev/null
+++ b/mplib/pymp/kdl.pyi
@@ -0,0 +1,18 @@
+from __future__ import annotations
+import numpy
+import typing
+__all__ = ['KDLModel']
+M = typing.TypeVar("M", bound=int)
+class KDLModel:
+    def __init__(self, urdf_filename: str, joint_names: list[str], link_names: list[str], verbose: bool) -> None:
+        ...
+    def chain_IK_LMA(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
+        ...
+    def chain_IK_NR(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
+        ...
+    def chain_IK_NR_JL(self, index: int, q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
+        ...
+    def get_tree_root_name(self) -> str:
+        ...
+    def tree_IK_NR_JL(self, endpoints: list[str], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_poses: list[numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], int]:
+        ...
diff --git a/mplib/pymp/kdl/__init__.pyi b/mplib/pymp/kdl/__init__.pyi
deleted file mode 100644
index bbf07f4f..00000000
--- a/mplib/pymp/kdl/__init__.pyi
+++ /dev/null
@@ -1,18 +0,0 @@
-import mplib.pymp.kdl
-import typing
-import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "KDLModel"
-]
-
-
-class KDLModel():
-    def __init__(self, urdf_filename: str, joint_names: list[str], link_names: list[str], verbose: bool) -> None: ...
-    def chain_IK_LMA(self, index: int, q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], goal_pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], int]: ...
-    def chain_IK_NR(self, index: int, q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], goal_pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], int]: ...
-    def chain_IK_NR_JL(self, index: int, q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], goal_pose: numpy.ndarray[numpy.float64, _Shape[7, 1]], q_min: numpy.ndarray[numpy.float64, _Shape[m, 1]], q_max: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], int]: ...
-    def get_tree_root_name(self) -> str: ...
-    def tree_IK_NR_JL(self, endpoints: list[str], q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], goal_poses: list[numpy.ndarray[numpy.float64, _Shape[7, 1]]], q_min: numpy.ndarray[numpy.float64, _Shape[m, 1]], q_max: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], int]: ...
-    pass
diff --git a/mplib/pymp/ompl/__init__.pyi b/mplib/pymp/ompl.pyi
similarity index 52%
rename from mplib/pymp/ompl/__init__.pyi
rename to mplib/pymp/ompl.pyi
index 5127cd2b..5d7f210f 100644
--- a/mplib/pymp/ompl/__init__.pyi
+++ b/mplib/pymp/ompl.pyi
@@ -1,51 +1,25 @@
-import mplib.pymp.ompl
-import typing
+from __future__ import annotations
 import mplib.pymp.planning_world
 import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "FixedJoint",
-    "OMPLPlanner"
-]
-
-
-class FixedJoint():
-    def __init__(self, articulation_idx: int, joint_idx: int, value: float) -> None: ...
-    @property
-    def articulation_idx(self) -> int:
-        """
-        :type: int
-        """
-    @articulation_idx.setter
-    def articulation_idx(self, arg0: int) -> None:
-        pass
-    @property
-    def joint_idx(self) -> int:
-        """
-        :type: int
-        """
-    @joint_idx.setter
-    def joint_idx(self, arg0: int) -> None:
-        pass
-    @property
-    def value(self) -> float:
-        """
-        :type: float
-        """
-    @value.setter
-    def value(self, arg0: float) -> None:
-        pass
-    pass
-class OMPLPlanner():
-    def __init__(self, world: mplib.pymp.planning_world.PlanningWorld, robot_idx: int = 0) -> None: 
+import typing
+__all__ = ['FixedJoint', 'OMPLPlanner']
+M = typing.TypeVar("M", bound=int)
+N = typing.TypeVar("N", bound=int)
+class FixedJoint:
+    articulation_idx: int
+    joint_idx: int
+    value: float
+    def __init__(self, articulation_idx: int, joint_idx: int, value: float) -> None:
+        ...
+class OMPLPlanner:
+    def __init__(self, world: mplib.pymp.planning_world.PlanningWorld, robot_idx: int = 0) -> None:
         """
             Args:
                 world: planning world
             Returns:
                 OMPLPlanner object
         """
-    def plan(self, start_state: numpy.ndarray[numpy.float64, _Shape[m, 1]], goal_states: list[numpy.ndarray[numpy.float64, _Shape[m, 1]]], planner_name: str = 'RRTConnect', time: float = 1.0, range: float = 0.0, verbose: bool = False, fixed_joints: set[FixedJoint] = set(), no_simplification: bool = False, constraint_function: typing.Callable[[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]], None] = None, constraint_jacobian: typing.Callable[[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]], None] = None, constraint_tolerance: float = 0.001) -> tuple[str, numpy.ndarray[numpy.float64, _Shape[m, n]]]: 
+    def plan(self, start_state: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], goal_states: list[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], planner_name: str = 'RRTConnect', time: float = 1.0, range: float = 0.0, verbose: bool = False, fixed_joints: set[FixedJoint] = set(), no_simplification: bool = False, constraint_function: typing.Callable[[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], None] = None, constraint_jacobian: typing.Callable[[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]], None] = None, constraint_tolerance: float = 0.001) -> tuple[str, numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
         """
             Plan a path from start state to goal states.
             Args:
@@ -63,11 +37,10 @@ class OMPLPlanner():
             Returns:
                 pair of planner status and path. If planner succeeds, status is "Exact solution."
         """
-    def simplify_path(self, path: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: 
+    def simplify_path(self, path: numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]) -> numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]:
         """
             Args:
                 path: path to be simplified (numpy array of shape (n, dim))
             Returns:
                 simplified path
         """
-    pass
diff --git a/mplib/pymp/pinocchio/__init__.pyi b/mplib/pymp/pinocchio.pyi
similarity index 69%
rename from mplib/pymp/pinocchio/__init__.pyi
rename to mplib/pymp/pinocchio.pyi
index 62ffb128..5ecee108 100644
--- a/mplib/pymp/pinocchio/__init__.pyi
+++ b/mplib/pymp/pinocchio.pyi
@@ -1,15 +1,11 @@
-import mplib.pymp.pinocchio
-import typing
+from __future__ import annotations
 import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "PinocchioModel"
-]
-
-
-class PinocchioModel():
-    def __init__(self, urdf_filename: str, gravity: numpy.ndarray[numpy.float64, _Shape[3, 1]] = array([ 0. , 0. , -9.81]), verbose: bool = True) -> None: 
+import typing
+__all__ = ['PinocchioModel']
+M = typing.TypeVar("M", bound=int)
+N = typing.TypeVar("N", bound=int)
+class PinocchioModel:
+    def __init__(self, urdf_filename: str, gravity: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]] = ..., verbose: bool = True) -> None:
         """
              Args:
                   urdf_filename: path to the urdf file
@@ -18,7 +14,7 @@ class PinocchioModel():
              Returns:
                   PinocchioModel object
         """
-    def compute_IK_CLIK(self, index: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]], q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], mask: list[bool] = [], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], bool, numpy.ndarray[numpy.float64, _Shape[6, 1]]]: 
+    def compute_IK_CLIK(self, index: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], mask: list[bool] = [], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], bool, numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
         """
              Compute the inverse kinematics using close loop inverse kinematics.
              Args:
@@ -33,7 +29,7 @@ class PinocchioModel():
              Returns:
                   joint configuration
         """
-    def compute_IK_CLIK_JL(self, index: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]], q_init: numpy.ndarray[numpy.float64, _Shape[m, 1]], q_min: numpy.ndarray[numpy.float64, _Shape[m, 1]], q_max: numpy.ndarray[numpy.float64, _Shape[m, 1]], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], bool, numpy.ndarray[numpy.float64, _Shape[6, 1]]]: 
+    def compute_IK_CLIK_JL(self, index: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]], q_init: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_min: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], q_max: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], eps: float = 1e-05, maxIter: int = 1000, dt: float = 0.1, damp: float = 1e-12) -> tuple[numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], bool, numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]]:
         """
              The same as compute_IK_CLIK but with it clamps the joint configuration to the given limits.
              Args:
@@ -49,7 +45,7 @@ class PinocchioModel():
              Returns:
                   joint configuration
         """
-    def compute_forward_kinematics(self, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: 
+    def compute_forward_kinematics(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
              Compute forward kinematics for the given joint configuration.
              Args:
@@ -57,7 +53,7 @@ class PinocchioModel():
              Returns:
                   None. If you want the result you need to call get_link_pose
         """
-    def compute_full_jacobian(self, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: 
+    def compute_full_jacobian(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
              Compute the full jacobian for the given joint configuration.
              Args:
@@ -65,7 +61,7 @@ class PinocchioModel():
              Returns:
                   None. If you want the result you need to call get_link_jacobian
         """
-    def compute_single_link_jacobian(self, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]], index: int, local: bool = False) -> numpy.ndarray[numpy.float64, _Shape[6, n]]: 
+    def compute_single_link_jacobian(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]], index: int, local: bool = False) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
         """
              Compute the jacobian of the given link.
              Args:
@@ -75,7 +71,7 @@ class PinocchioModel():
              Returns:
                   6 x n jacobian of the link
         """
-    def get_chain_joint_index(self, end_effector: str) -> list[int]: 
+    def get_chain_joint_index(self, end_effector: str) -> list[int]:
         """
              Get the joint indices of the joints in the chain from the root to the given link.
              Args:
@@ -83,7 +79,7 @@ class PinocchioModel():
              Returns:
                   joint indices of the joints in the chain
         """
-    def get_chain_joint_name(self, end_effector: str) -> list[str]: 
+    def get_chain_joint_name(self, end_effector: str) -> list[str]:
         """
              Get the joint names of the joints in the chain from the root to the given link.
              Args:
@@ -91,9 +87,11 @@ class PinocchioModel():
              Returns:
                   joint names of the joints in the chain
         """
-    def get_joint_dim(self, index: int, user: bool = True) -> int: ...
-    def get_joint_dims(self, user: bool = True) -> numpy.ndarray[numpy.int32, _Shape[m, 1]]: ...
-    def get_joint_ids(self, user: bool = True) -> numpy.ndarray[numpy.int32, _Shape[m, 1]]: 
+    def get_joint_dim(self, index: int, user: bool = True) -> int:
+        ...
+    def get_joint_dims(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
+        ...
+    def get_joint_ids(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
         """
              Get the id of the all the joints. Again, Pinocchio might split a joint into multiple joints.
              Args:
@@ -101,16 +99,19 @@ class PinocchioModel():
              Returns:
                   ids of the joint
         """
-    def get_joint_limits(self, user: bool = True) -> list[numpy.ndarray[numpy.float64, _Shape[m, n]]]: ...
-    def get_joint_names(self, user: bool = True) -> list[str]: ...
-    def get_joint_types(self, user: bool = True) -> list[str]: ...
-    def get_leaf_links(self) -> list[str]: 
+    def get_joint_limits(self, user: bool = True) -> list[numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]]:
+        ...
+    def get_joint_names(self, user: bool = True) -> list[str]:
+        ...
+    def get_joint_types(self, user: bool = True) -> list[str]:
+        ...
+    def get_leaf_links(self) -> list[str]:
         """
              Get the leaf links (links without child) of the kinematic tree.
              Returns:
                   list of leaf links
         """
-    def get_link_jacobian(self, index: int, local: bool = False) -> numpy.ndarray[numpy.float64, _Shape[6, n]]: 
+    def get_link_jacobian(self, index: int, local: bool = False) -> numpy.ndarray[tuple[typing.Literal[6], N], numpy.dtype[numpy.float64]]:
         """
              Get the jacobian of the given link.
              Args:
@@ -119,8 +120,9 @@ class PinocchioModel():
              Returns:
                   6 x n jacobian of the link
         """
-    def get_link_names(self, user: bool = True) -> list[str]: ...
-    def get_link_pose(self, index: int) -> numpy.ndarray[numpy.float64, _Shape[7, 1]]: 
+    def get_link_names(self, user: bool = True) -> list[str]:
+        ...
+    def get_link_pose(self, index: int) -> numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]:
         """
              Get the pose of the given link.
              Args:
@@ -128,7 +130,7 @@ class PinocchioModel():
              Returns:
                   pose of the link [x,y,z,qw,qx,qy,qz]
         """
-    def get_parents(self, user: bool = True) -> numpy.ndarray[numpy.int32, _Shape[m, 1]]: 
+    def get_parents(self, user: bool = True) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.int32]]:
         """
              Get the parent of the all the joints. Again, Pinocchio might split a joint into multiple joints.
              Args:
@@ -136,25 +138,25 @@ class PinocchioModel():
              Returns:
                   parents of the joints
         """
-    def get_random_configuration(self) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: 
+    def get_random_configuration(self) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
         """
              Get a random configuration.
              Returns:
                   random joint configuration
         """
-    def print_frames(self) -> None: ...
-    def set_joint_order(self, names: list[str]) -> None: 
+    def print_frames(self) -> None:
+        ...
+    def set_joint_order(self, names: list[str]) -> None:
         """
              Pinocchio might have a different joint order or it might add additional joints.
              If you do not pass the the list of joint names, the default order might not be the one you want.
              Args:
                   names: list of joint names in the order you want
         """
-    def set_link_order(self, names: list[str]) -> None: 
+    def set_link_order(self, names: list[str]) -> None:
         """
              Pinocchio might have a different link order or it might add additional links.
              If you do not pass the the list of link names, the default order might not be the one you want.
              Args:
                   names: list of link names in the order you want
         """
-    pass
diff --git a/mplib/pymp/planning_world/__init__.pyi b/mplib/pymp/planning_world.pyi
similarity index 77%
rename from mplib/pymp/planning_world/__init__.pyi
rename to mplib/pymp/planning_world.pyi
index d1be442a..2aa2643e 100644
--- a/mplib/pymp/planning_world/__init__.pyi
+++ b/mplib/pymp/planning_world.pyi
@@ -1,18 +1,12 @@
-import mplib.pymp.planning_world
-import typing
+from __future__ import annotations
 import mplib.pymp.articulation
 import mplib.pymp.fcl
 import numpy
-_Shape = typing.Tuple[int, ...]
-
-__all__ = [
-    "PlanningWorld",
-    "WorldCollisionResult"
-]
-
-
-class PlanningWorld():
-    def __init__(self, articulations: list[mplib.pymp.articulation.ArticulatedModel], articulation_names: list[str], normal_objects: list[mplib.pymp.fcl.CollisionObject], normal_object_names: list[str], plan_articulation_id: int = 0) -> None: 
+import typing
+__all__ = ['PlanningWorld', 'WorldCollisionResult']
+M = typing.TypeVar("M", bound=int)
+class PlanningWorld:
+    def __init__(self, articulations: list[mplib.pymp.articulation.ArticulatedModel], articulation_names: list[str], normal_objects: list[mplib.pymp.fcl.CollisionObject], normal_object_names: list[str], plan_articulation_id: int = 0) -> None:
         """
             Planning world for collision checking.
             Args:
@@ -24,7 +18,7 @@ class PlanningWorld():
             Returns:
                 PlanningWorld object
         """
-    def add_articulation(self, model: mplib.pymp.articulation.ArticulatedModel, name: str) -> None: 
+    def add_articulation(self, model: mplib.pymp.articulation.ArticulatedModel, name: str) -> None:
         """
             Add an articulated model to the planning world.
             Args:
@@ -33,7 +27,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def add_articulations(self, models: list[mplib.pymp.articulation.ArticulatedModel], names: list[str]) -> None: 
+    def add_articulations(self, models: list[mplib.pymp.articulation.ArticulatedModel], names: list[str]) -> None:
         """
             Add a list of articulated models to the planning world.
             Args:
@@ -42,14 +36,13 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def collide(self) -> bool: 
+    def collide(self) -> bool:
         """
             Check collision between all objects.
             Returns:
                 True if collision happens
         """
-    @staticmethod
-    def collide_full(*args, **kwargs) -> typing.Any: 
+    def collide_full(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
         """
             Check collision between the articulated model and all objects.
             Args:
@@ -58,8 +51,7 @@ class PlanningWorld():
             Returns:
                 List of WorldCollisionResult objects
         """
-    @staticmethod
-    def collide_with_others(*args, **kwargs) -> typing.Any: 
+    def collide_with_others(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
         """
             Check collision between the articulated model and other objects.
             Args:
@@ -68,31 +60,31 @@ class PlanningWorld():
             Returns:
                 List of WorldCollisionResult objects
         """
-    def get_articulations(self) -> list[mplib.pymp.articulation.ArticulatedModel]: 
+    def get_articulations(self) -> list[mplib.pymp.articulation.ArticulatedModel]:
         """
             Get the list of articulated models.
             Returns:
                 list of articulated models as pointers
         """
-    def get_normal_objects(self) -> list[mplib.pymp.fcl.CollisionObject]: 
+    def get_normal_objects(self) -> list[mplib.pymp.fcl.CollisionObject]:
         """
             Get the list of non-articulated collision objects.
             Returns:
                 list of non-articulated collision objects
         """
-    def print_attached_tool_pose(self) -> None: 
+    def print_attached_tool_pose(self) -> None:
         """
             Print the pose of the attached tool.
             Returns:
                 None
         """
-    def remove_attach(self) -> None: 
+    def remove_attach(self) -> None:
         """
             Remove the attached tool.
             Returns:
                 None
         """
-    def remove_normal_object(self, name: str) -> bool: 
+    def remove_normal_object(self, name: str) -> bool:
         """
             Remove a non-articulated collision object from the planning world.
             Args:
@@ -100,8 +92,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    @staticmethod
-    def self_collide(*args, **kwargs) -> typing.Any: 
+    def self_collide(self, index: int = 0, request: mplib.pymp.fcl.CollisionRequest = ...) -> list[...]:
         """
             Check collision between the articulated model and itself.
             Args:
@@ -110,7 +101,7 @@ class PlanningWorld():
             Returns:
                 List of WorldCollisionResult objects
         """
-    def set_normal_object(self, collision_object: str, name: mplib.pymp.fcl.CollisionObject) -> None: 
+    def set_normal_object(self, collision_object: str, name: mplib.pymp.fcl.CollisionObject) -> None:
         """
             Add a non-articulated collision object to the planning world.
             Args:
@@ -119,7 +110,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def set_qpos(self, index: int, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: 
+    def set_qpos(self, index: int, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Set the joint qpos of the articulated model.
             Args:
@@ -128,7 +119,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def set_qpos_all(self, qpos: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: 
+    def set_qpos_all(self, qpos: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Set the joint qpos of all articulated models.
             Args:
@@ -136,7 +127,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def set_use_attach(self, use: bool) -> None: 
+    def set_use_attach(self, use: bool) -> None:
         """
             Set whether to use attached tool for collision checking.
             Args:
@@ -144,7 +135,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def set_use_point_cloud(self, use: bool) -> None: 
+    def set_use_point_cloud(self, use: bool) -> None:
         """
             Set whether to use point cloud for collision checking.
             Args:
@@ -152,8 +143,9 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def update_attached_box(self, size: numpy.ndarray[numpy.float64, _Shape[3, 1]], link_id: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
-    def update_attached_mesh(self, mesh_path: str, link_id: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: 
+    def update_attached_box(self, size: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]], link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
+        ...
+    def update_attached_mesh(self, mesh_path: str, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Add mesh as the attached tool.
             Args:
@@ -163,7 +155,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def update_attached_sphere(self, radius: float, link_id: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: 
+    def update_attached_sphere(self, radius: float, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Add sphere as the attached tool.
             Args:
@@ -173,7 +165,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def update_attached_tool(self, p_geom: mplib.pymp.fcl.CollisionGeometry, link_id: int, pose: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: 
+    def update_attached_tool(self, p_geom: mplib.pymp.fcl.CollisionGeometry, link_id: int, pose: numpy.ndarray[tuple[typing.Literal[7], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
         """
             Update the attached tool.
             Args:
@@ -183,7 +175,7 @@ class PlanningWorld():
             Returns:
                 None
         """
-    def update_point_cloud(self, vertices: numpy.ndarray[numpy.float64, _Shape[m, 3]], radius: float) -> None: 
+    def update_point_cloud(self, vertices: numpy.ndarray[tuple[M, typing.Literal[3]], numpy.dtype[numpy.float64]], radius: float) -> None:
         """
             Update the point cloud for collision checking.
             Args:
@@ -194,17 +186,13 @@ class PlanningWorld():
         """
     @property
     def use_attach(self) -> bool:
-        """
-        :type: bool
-        """
+        ...
     @property
     def use_point_cloud(self) -> bool:
-        """
-        :type: bool
-        """
-    pass
-class WorldCollisionResult():
+        ...
+class WorldCollisionResult:
     """
+    
         Result of the collision checking.
         Attributes:
             res: whether collision happens
@@ -216,32 +204,19 @@ class WorldCollisionResult():
     """
     @property
     def collision_type(self) -> str:
-        """
-        :type: str
-        """
+        ...
     @property
     def link_name1(self) -> str:
-        """
-        :type: str
-        """
+        ...
     @property
     def link_name2(self) -> str:
-        """
-        :type: str
-        """
+        ...
     @property
     def object_name1(self) -> str:
-        """
-        :type: str
-        """
+        ...
     @property
     def object_name2(self) -> str:
-        """
-        :type: str
-        """
+        ...
     @property
     def res(self) -> mplib.pymp.fcl.CollisionResult:
-        """
-        :type: mplib.pymp.fcl.CollisionResult
-        """
-    pass
+        ...