MPlib Motion Planning Library

Because MPlib uses a projection-based planning method, you must provide the Jacobian of the constraint function. This Jacobian allows the planner to efficiently project configurations onto the constraint manifold. For performance, the Jacobian calculation should be optimized (e.g., by manipulating the forward kinematics Jacobian).

# Example: Jacobian of the constraint function
def constraint_jacobian(q):
    # ... implementation logic using single link jacobian ...
    return jacobian_vector

To perform constrained planning, you must define a constraint function $R^d \rightarrow R$ that evaluates to zero when a configuration is valid and non-zero otherwise. In MPlib, this function is used by a projection-based planner to map sampled configurations into the valid constraint manifold.

# Example: Constraint function that evaluates to zero when the 
# end-effector z-axis points downwards
def constraint_function(q):
    # ... implementation logic ...
    return error_value

MPlib uses a custom tool to automatically generate pybind11 docstrings from C++ code comments. This allows C++ documentation to flow into the Python interface.

Requirements:

• C++ comments must follow the doxygen format.
• You must install libclang matching your local LLVM version: python3 -m pip install libclang=={clang-version}

Mechanism:

• The tool ./dev/mkdoc.py processes header files.
• The generated docstrings are in Sphinx reStructuredText (reST) format and are stored in ./pybind/docstring/.

MPlib allows you to temporarily fix specific joints during a planning cycle. This is useful for multi-stage motions where you want to move one part of a robot (e.g., a mobile base) while keeping another part (e.g., a manipulator arm) stationary, or vice versa.

To achieve this, you can specify which joints to ignore during the planning process. By ignoring a subset of joints, they are treated as fixed in their current configuration, and the planner only computes trajectories for the remaining 'active' joints.

# Example logic for ignoring specific joints
# To move only joints 0 and 1, ignore joints 2 through 9
# To move joints 2 through 9, ignore joints 0 and 1

# The specific mechanism involves passing the list of joints to ignore 
# to the planning function (as demonstrated in the two-stage motion example).

To automate the generation of Python type stubs and documentation, run the generate_stubs.sh script. This script uses a Docker container to build the wheels and then employs pybind11-stubgen for stubs and pdoc for documentation.

./dev/generate_stubs.sh

Workflow details:

• Builds a Python wheel using ./dev/build_wheels.sh.
• Uses pybind11-stubgen to create stubs, which are then copied into mplib/.
• Uses pdoc to generate documentation, which is then copied into docs/.

Once plan_pose() or plan_screw() returns a successful path, you must drive the robot to follow the generated waypoints. The output dictionary provides the necessary time-parameterized data (position, time, velocity, acceleration) to execute the motion.

In simulation environments like sapien, you use these waypoints to update the robot's state over time. If the robot fails to follow the path despite a successful plan, verify your controller's parameters.

To build Python wheels for specific Python versions, use the provided build script which utilizes the kolinguo/mplib-build Docker image. This ensures a consistent build environment with necessary dependencies pre-installed.

Run the following command to build a wheel for a specific Python version (e.g., 3.10):

./dev/build_wheels.sh --py 310

Generated wheels will be located in the wheelhouse/ directory, and the pybind11 docstrings will be in ./pybind/docstring/.

Install the mplib Python package using pip. This package provides lightweight motion planning functionalities including collision-free trajectory planning, inverse kinematics, and point cloud environment modeling. It is decoupled from ROS and supports Python 3.6+ on Ubuntu 18.04+.

pip install mplib

MPlib is a lightweight Python package for motion planning that is decoupled from ROS. It is designed to provide most robot manipulation motion planning functionalities with minimal code. For detailed usage instructions and comprehensive examples, refer to the official tutorial.

https://motion-planning-lib.readthedocs.io/latest/tutorials/getting_started.html

To execute constrained planning, pass the constraint function and its Jacobian into the planning function. You can optionally specify a tolerance parameter to control the precision of the projection onto the constraint manifold.

# Planning with a constraint
plan = planner.plan(
    start_q, 
    goal_q, 
    constraint_function=constraint_function, 
    constraint_jacobian=constraint_jacobian, 
    tolerance=1e-3
)

# Planning without a constraint
plan = planner.plan(start_q, goal_q)

To improve the development experience in Visual Studio Code, install the C/C++ and CMake extensions.

To enable automatic parsing of include directories from CMake results, add the following to your VS Code settings:

"compileCommands": "${workspaceFolder}/build/compile_commands.json"

If you prefer other editors like Vim, use clangd. You can generate the required compile_commands.json file by running:

CMAKE_EXPORT_COMPILE_COMMANDS=ON cmake ..