### Start Documentation Webserver

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Starts the documentation webserver for the project.

```bash
./dev/start_doc_webserver.sh
```

--------------------------------

### Setup Planner and Collision Printer

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/detect_collision.rst

Sets up the planner using `setup_planner` and defines a function to print detected collisions. This is a prerequisite for collision detection examples.

```python
from mplib.examples.demo_setup import DemoSetup

def print_collision(collision_result):
    if collision_result.is_collision:
        print("Collision detected!")
        for contact in collision_result.contacts:
            print(f"  - Contact between {contact.body1_name} and {contact.body2_name} at distance {contact.distance}")
    else:
        print("No collision detected.")
```

--------------------------------

### Setup Docker Container for Debugging

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Starts a Docker container configured for debugging the project.

```bash
./dev/docker_setup.sh
```

--------------------------------

### Complete Usage Example: OMPLPlanner and FixedJoint

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Demonstrates the end-to-end usage of OMPLPlanner for motion planning, including setting up the world, defining start and goal states, applying fixed joint constraints, and simplifying the resulting path.

```python
from mplib.planning import OMPLPlanner, FixedJoint
from mplib import PlanningWorld, ArticulatedModel, Planner
import numpy as np

# Method 1: Direct OMPL planner usage
robot = ArticulatedModel("robot.urdf", "robot.srdf")
world = PlanningWorld([robot])
ompl_planner = OMPLPlanner(world)

start = np.array([0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79])
goal = np.array([0, 0.3, 0.1, -2.5, 0.1, 2.8, 0.8])

# Fix first joint
fixed = {FixedJoint(0, 0, 0.0)}

status, path = ompl_planner.plan(
    start_state=start,
    goal_states=[goal],
    time=2.0,
    range=0.1,
    fixed_joints=fixed,
    simplify=True
)

if status == "Exact solution":
    print(f"Path has {path.shape[0]} waypoints")
    simplified = ompl_planner.simplify_path(path)
    print(f"Simplified path has {simplified.shape[0]} waypoints")
```

--------------------------------

### Install Boost on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the Boost C++ libraries on Ubuntu systems using apt.

```bash
sudo apt install libboost-all-dev
```

--------------------------------

### Install OMPL on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the Open Motional Planning Library (OMPL) on Ubuntu systems using apt.

```bash
sudo apt install libompl-dev
```

--------------------------------

### Install OctoMap on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the OctoMap point cloud library on Ubuntu systems using apt.

```bash
sudo apt install liboctomap-dev
```

--------------------------------

### Example: Print Leaf Links

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Demonstrates how to get and print the names of leaf links, often used as potential end-effectors.

```python
leaf_links = pinocchio_model.get_leaf_links()
print(f"End-effector candidates: {leaf_links}")
```

--------------------------------

### Complete Kinematics Usage Example

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

A comprehensive example demonstrating the usage of the Kinematics module for forward kinematics, Jacobian computation, and inverse kinematics (IK).

```python
from mplib.kinematics import PinocchioModel
from mplib import Pose
import numpy as np

# Create model
model = PinocchioModel("robot.urdf")

# Get configuration
q_home = np.zeros(9)

# Forward kinematics
model.compute_forward_kinematics(q_home)
ee_pose = model.get_link_pose(8)

# Jacobian
model.compute_full_jacobian(q_home)
J = model.get_link_jacobian(8, local=False)

# IK
goal = Pose(p=[0.4, 0.3, 0.5], q=[1, 0, 0, 0])
q_ik, success, error = model.compute_IK_CLIK(
    index=8,
    pose=goal,
    q_init=q_home
)

if success:
    print("IK succeeded!")
    model.compute_forward_kinematics(q_ik)
    result_pose = model.get_link_pose(8)
    dist = goal.distance(result_pose)
    print(f"Distance to goal: {dist}")
```

--------------------------------

### Install libccd on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the libccd collision detection library on Ubuntu systems using apt.

```bash
sudo apt install libccd-dev
```

--------------------------------

### Install Eigen3 on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the Eigen3 linear algebra library on Ubuntu systems using apt.

```bash
sudo apt install libeigen3-dev
```

--------------------------------

### Install Orocos KDL on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the Orocos KDL (Kinematics Dynamics Library) on Ubuntu systems using apt.

```bash
sudo apt install liborocos-kdl-dev
```

--------------------------------

### Load URDF and Setup Scene

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/planning_with_fixed_joints.rst

Loads a URDF file with linear tracks and sets up the planning scene. This is the initial setup for planning with fixed joints.

```python
self.setup_scene()
        self.robot.set_joint_positions(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]))
        self.scene.add_object('table', self.table)
        self.scene.add_robot(self.robot)
        self.scene.add_object('ankor', self.ankor)
        self.scene.add_object('target', self.target)
        self.scene.add_object('target2', self.target2)
```

--------------------------------

### Planner Initialization Example

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/configuration.md

Demonstrates how to initialize the Planner class with various configuration options. Ensure URDF and move_group are provided.

```python
from mplib import Planner
import numpy as np

planner = Planner(
    urdf="path/to/robot.urdf",
    move_group="end_effector",
    srdf="path/to/robot.srdf",
    new_package_keyword="~/robot_description",
    use_convex=False,
    user_link_names=["base", "link1", "link2", "end_effector"],
    user_joint_names=["joint1", "joint2", "joint3"],
    joint_vel_limits=np.array([1.0, 1.0, 1.0]),
    joint_acc_limits=np.array([0.5, 0.5, 0.5]),
    verbose=True
)
```

--------------------------------

### Example: Compute Forward Kinematics with Random Configuration

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Shows how to generate a random robot configuration and then compute the forward kinematics for it.

```python
for i in range(10):
    q_random = pinocchio_model.get_random_configuration()
    pinocchio_model.compute_forward_kinematics(q_random)
```

--------------------------------

### Install URDF DOM Parser on Ubuntu

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the URDF DOM parser library on Ubuntu systems using apt.

```bash
sudo apt install liburdfdom-dev
```

--------------------------------

### Install libclang for Docstring Generation

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the libclang Python package, which is required for docstring generation. Ensure the clang-version matches your local LLVM installation.

```bash
python3 -m pip install libclang=={clang-version}
```

--------------------------------

### Install Package with Python Glue Code

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Installs the entire MPlib package, including Python glue code, using pip.

```bash
python3.[version] -m pip install .
```

--------------------------------

### Install MPlib Package

Source: https://github.com/haosulab/mplib/blob/main/README.md

Install the MPlib package using pip. This command is for Ubuntu 20.04+ with Python 3.8+.

```bash
pip install mplib
```

--------------------------------

### Create Poses - Usage Examples

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/Pose.md

Demonstrates various ways to create Pose objects, including from position and quaternion, from a transformation matrix, and using the default constructor.

```python
from mplib import Pose
import numpy as np

# From position and quaternion
pose1 = Pose(
    p=np.array([0.5, 0.0, 0.5]),
    q=np.array([1, 0, 0, 0])  # identity rotation
)

# From transformation matrix
T = np.eye(4)
T[:3, 3] = [0.5, 0.0, 0.5]
pose2 = Pose(T)

# Default pose
pose3 = Pose()
```

--------------------------------

### Basic CMake Project Setup

Source: https://github.com/haosulab/mplib/blob/main/CMakeLists.txt

Sets the minimum CMake version, project name, and C++ standard. Configures release build flags for optimization and warnings.

```cmake
cmake_minimum_required(VERSION 3.12)
project(mp LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)

set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g3 -Wall -Werror -fsized-deallocation -Wno-deprecated-declarations")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
```

--------------------------------

### Example: Iterate Through Joint Limits

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Demonstrates how to iterate through the joint limits obtained from get_joint_limits and print them.

```python
limits = pinocchio_model.get_joint_limits(user=True)
for i, (q_min, q_max) in enumerate(limits):
    print(f"Joint {i}: [{q_min}, {q_max}]")
```

--------------------------------

### Example: Print Adjacent Links

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Demonstrates how to retrieve and print all adjacent link pairs in the robot model.

```python
adjacent = pinocchio_model.get_adjacent_links()
for link1, link2 in adjacent:
    print(f"{link1} - {link2}")
```

--------------------------------

### Example: Specify Joint Order

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Demonstrates how to set a specific order for joints before performing kinematic operations.

```python
# Specify joint order before operations
pinocchio_model.set_joint_order([
    "joint1", "joint2", "joint3", 
    "joint4", "joint5", "joint6"
])
```

--------------------------------

### Planner Initialization and Basic Planning

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Initializes the Planner with robot URDF and SRDF files and demonstrates a basic call to plan a trajectory from a start to a goal configuration.

```APIDOC
## Planner Initialization and Basic Planning

### Description
Initializes the Planner with robot URDF and SRDF files and demonstrates a basic call to plan a trajectory from a start to a goal configuration.

### Method
```python
planner = Planner(
    urdf="robot.urdf",
    move_group="end_effector",
    srdf="robot.srdf"
)

result = planner.plan_qpos(
    goal_qposes=[goal],
    current_qpos=start,
    planning_time=2.0
)
```

### Response
#### Success Response
The result dictionary contains:
- **status** (string): Indicates if the planning was successful ('Success' or other).
- **time** (list): List of time points for the trajectory.
- **position** (list): List of joint positions for each time step.
- **velocity** (list): List of joint velocities for each time step.
- **acceleration** (list): List of joint accelerations for each time step.
- **duration** (float): Total duration of the trajectory.
```

--------------------------------

### Get All Articulation Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve a list of names for all articulations currently in the PlanningWorld.

```python
def get_articulation_names() -> list[str]
```

--------------------------------

### OMPLPlanner.plan

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Plans a path from a start state to one of multiple goal states using OMPL. Supports various planning parameters and constraints.

```APIDOC
## OMPLPlanner.plan

### Description
Plans a path from start state to one of multiple goal states. Supports various planning parameters and constraints.

### Method
plan

### Parameters
#### Path Parameters
- **start_state** (`np.ndarray`) - Required - Start configuration for move group joints
- **goal_states** (`list[np.ndarray]`) - Required - List of goal configurations; planner stops when one is reached

#### Query Parameters
- **time** (`float`) - Optional - Planning time limit in seconds (default: 1.0)
- **range** (`float`) - Optional - RRT step size / maximum extension (default: 0.0)
- **fixed_joints** (`set[FixedJoint]`) - Optional - Joints to fix at specific values (default: set())
- **simplify** (`bool`) - Optional - Whether to simplify the path (disabled for constrained planning) (default: True)
- **constraint_function** (`Callable` or `None`) - Optional - Constraint function R^d → R that equals 0 when satisfied (default: None)
- **constraint_jacobian** (`Callable` or `None`) - Optional - Jacobian of constraint function w.r.t. joint angles (default: None)
- **constraint_tolerance** (`float`) - Optional - Allowable constraint deviation from 0 (default: 0.001)
- **verbose** (`bool`) - Optional - Print debug logs (default: False)

### Returns
- **status** (`str`) - "Exact solution" if planning succeeded, otherwise description of failure
- **path** (`np.ndarray`) - Array of shape `(n_waypoints, n_dof)` with waypoints (only if succeeded)

### Request Example
```python
start_qpos = np.array([0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79])
goal_qpos = np.array([0, 0.3, 0.1, -2.5, 0.1, 2.8, 0.8])

status, path = planner.plan(
    start_state=start_qpos,
    goal_states=[goal_qpos],
    time=2.0,
    range=0.1,
    simplify=True
)

if status == "Exact solution":
    print(f"Planning succeeded! Path has {len(path)} waypoints")
else:
    print(f"Planning failed: {status}")
```
```

--------------------------------

### Plan with Multi-Constraint Example

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Plan a trajectory satisfying multiple constraints on end-effector position (x and y). Requires defining constraint and Jacobian functions for each constraint.

```python
# Constraints: x position < 0.5 AND y position > -0.3
def constraint_func(qpos, constraint_vec):
    robot.set_qpos(qpos, full=True)
    ee_pose = pinocchio_model.get_link_pose(ee_index)
    
    constraint_vec[0] = ee_pose.p[0] - 0.5      # x < 0.5
    constraint_vec[1] = -0.3 - ee_pose.p[1]     # y > -0.3

def constraint_jac(qpos, jacobian_mat):
    pinocchio_model.compute_forward_kinematics(qpos)
    pinocchio_model.compute_full_jacobian(qpos)
    J = pinocchio_model.get_link_jacobian(ee_index, local=False)
    
    jacobian_mat[:] = J[0:2, :]  # 2 constraints, n_dof columns
```

--------------------------------

### Initialize for Planning Operations

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/module-structure.md

Use this snippet to create an OMPLPlanner for motion planning. It requires a PlanningWorld and can plan paths between a start and goal configuration within a specified time.

```python
from mplib.planning import OMPLPlanner
from mplib import PlanningWorld, ArticulatedModel

robot = ArticulatedModel(urdf_path, srdf_path)
world = PlanningWorld([robot])
planner = OMPLPlanner(world)

status, path = planner.plan(start_qpos, [goal_qpos], time=2.0)
```

--------------------------------

### Pose Transformations - Usage Examples

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/Pose.md

Illustrates common pose transformation operations, such as converting a pose to a transformation matrix and obtaining the inverse of a pose.

```python
# Get transformation matrix
T = pose1.to_transformation_matrix()

# Inverse
pose_inv = pose1.inv()
```

--------------------------------

### Get Planned Articulations

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve a list of all articulated models that are currently marked for planning.

```python
def get_planned_articulations() -> list[ArticulatedModel]
```

--------------------------------

### Python AllowedCollisionMatrix Example Usage

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/collision-detection.md

Demonstrates how to obtain an AllowedCollisionMatrix, disable collisions between specific objects, and check the collision status.

```python
acm = world.get_allowed_collision_matrix()

# Disable collisions between gripper and object
acm.set_entry("gripper", "object", True)

# Check if collision is allowed
is_allowed = acm.get_entry("gripper", "object")
```

--------------------------------

### Plan Trajectory with Goal Poses

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Plan a trajectory from a start configuration to a list of goal configurations. The result includes status, time, position, velocity, acceleration, and duration.

```python
result = planner.plan_qpos(
    goal_qposes=[goal],
    current_qpos=start,
    planning_time=2.0
)

if result["status"] == "Success":
    times = result["time"]
    positions = result["position"]
    velocities = result["velocity"]
    accelerations = result["acceleration"]
    duration = result["duration"]
    
    print(f"Trajectory duration: {duration}s")
    print(f"Number of time steps: {len(times)}")
```

--------------------------------

### Top-Level mplib Imports

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/module-structure.md

Import key classes and functions directly from the top-level mplib package. Used for general planning and robot model setup.

```python
from mplib import (
    Planner,
    ArticulatedModel,
    AttachedBody,
    PlanningWorld,
    Pose,
    set_global_seed,
)
```

--------------------------------

### Plan Path with OMPLPlanner

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Plan a motion path from a start state to one of multiple goal states using the OMPL planner. Allows configuration of planning time, step size, fixed joints, and path simplification.

```python
start_qpos = np.array([0, 0.19, 0.0, -2.62, 0.0, 2.94, 0.79])
goal_qpos = np.array([0, 0.3, 0.1, -2.5, 0.1, 2.8, 0.8])

status, path = planner.plan(
    start_state=start_qpos,
    goal_states=[goal_qpos],
    time=2.0,
    range=0.1,
    simplify=True
)

if status == "Exact solution":
    print(f"Planning succeeded! Path has {len(path)} waypoints")
else:
    print(f"Planning failed: {status}")
```

--------------------------------

### Get Robot Name

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieves the name of the robot. No specific setup is required beyond having an instance of ArticulatedModel.

```python
def get_name() -> str
```

--------------------------------

### Initialize for Kinematics Operations

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/module-structure.md

Use this snippet to create a PinocchioModel for kinematics computations. It supports forward kinematics to get link poses and inverse kinematics (IK) to find joint configurations for a target pose.

```python
from mplib.kinematics import PinocchioModel
from mplib import Pose

model = PinocchioModel(urdf_path)

# Forward kinematics
model.compute_forward_kinematics(qpos)
ee_pose = model.get_link_pose(ee_index)

# Inverse kinematics
qpos_ik, success, error = model.compute_IK_CLIK(
    ee_index, goal_pose, qpos_init
)
```

--------------------------------

### Basic Motion Planning with Planner

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/INDEX.md

Demonstrates how to create a Planner instance and plan a motion to a target pose. Ensure you have URDF and SRDF files available.

```python
from mplib import Planner
import numpy as np

# Create planner
planner = Planner(
    urdf="path/to/robot.urdf",
    move_group="end_effector",
    srdf="path/to/robot.srdf"
)

# Plan motion
result = planner.plan_pose(
    goal_pose=goal,
    current_qpos=np.zeros(7),
    planning_time=2.0
)

# Check result
if result["status"] == "Success":
    times = result["time"]
    positions = result["position"]
    print(f"Planned {len(times)} waypoints")
```

--------------------------------

### Attach and Manipulate Multiple Objects

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Example of creating and attaching multiple objects to the robot's end-effector for simultaneous manipulation.

```python
# Attach multiple objects
object1 = AttachedBody(
    name="obj1",
    object=fcl_obj1,
    attached_articulation=robot,
    attached_link_id=ee_id,
    pose=Pose(p=[0, 0, 0.1])
)

object2 = AttachedBody(
    name="obj2",
    object=fcl_obj2,
    attached_articulation=robot,
    attached_link_id=ee_id,
    pose=Pose(p=[0.05, 0, 0.1])
)

# Plan with both objects attached
result = planner.plan_pose(goal, current_qpos)
```

--------------------------------

### Handle Inverse Kinematics Failures with Retries

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/usage-patterns.md

Manage failures when computing inverse kinematics (IK). This example demonstrates how to retry the IK calculation with increased sampling or a looser threshold if the initial attempt fails.

```python
status, qpos = planner.IK(goal_pose, current_qpos, return_closest=True)

if status != "Success":
    print(f"IK failed: {status}")
    
    # Try with more samples
    status, qpos = planner.IK(
        goal_pose=goal_pose,
        start_qpos=current_qpos,
        n_init_qpos=100,  # More attempts
        threshold=0.01,   # Looser threshold
        return_closest=True
    )
    
    if status != "Success":
        print("Cannot reach goal pose")
```

--------------------------------

### Initialize PlanningWorld

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Create a PlanningWorld instance with initial articulated models and static objects. Use this to set up the collision environment.

```python
from mplib import PlanningWorld, ArticulatedModel
from mplib.collision_detection.fcl import FCLObject

robot = ArticulatedModel("robot.urdf", "robot.srdf")
table = FCLObject("table")

world = PlanningWorld([robot], [table])
```

--------------------------------

### Initialize Planner

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Initialize the Planner with URDF, MoveGroup, and SRDF files.

```python
planner = Planner(
    urdf="robot.urdf",
    move_group="end_effector",
    srdf="robot.srdf"
)
```

--------------------------------

### Create OMPLPlanner Instance

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Instantiate an OMPLPlanner by providing a collision-checked planning world. This is the initial step before planning any motion.

```python
from mplib.planning import OMPLPlanner
from mplib import PlanningWorld, ArticulatedModel

robot = ArticulatedModel("robot.urdf", "robot.srdf")
world = PlanningWorld([robot])
planner = OMPLPlanner(world)
```

--------------------------------

### Default Planning Configuration

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/configuration.md

Illustrates the default parameters for the `plan_qpos` method, including time step, RRT range, planning time, and simplification settings.

```python
# Planning defaults
planner.plan_qpos(
    ...,
    time_step=0.1,
    rrt_range=0.1,
    planning_time=1,
    fix_joint_limits=True,
    fixed_joint_indices=None,
    simplify=True,
    constraint_tolerance=1e-3,
    verbose=False,
)
```

--------------------------------

### Get Joint Parents

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the parent joint index for each joint.

```python
def get_joint_parents(user: bool = True) -> np.ndarray:
    pass
```

--------------------------------

### Get Joint IDs

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the Pinocchio internal IDs for all joints.

```python
def get_joint_ids(user: bool = True) -> np.ndarray:
    pass
```

--------------------------------

### Get Single Joint Dimension

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the dimension of a single joint.

```python
def get_joint_dim(index: int, user: bool = True) -> int:
    pass
```

--------------------------------

### Get Object by Name

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve an FCLObject from the PlanningWorld using its name.

```python
def get_object(name: str) -> FCLObject
```

--------------------------------

### Create Planner Instance

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/INDEX.md

Initializes a Planner object. Requires paths to URDF and SRDF files, and the name of the move group.

```python
planner = Planner(urdf_path, move_group_name, srdf=srdf_path)
```

--------------------------------

### Get Articulation by Name

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve an ArticulatedModel from the PlanningWorld using its name.

```python
def get_articulation(name: str) -> ArticulatedModel
```

--------------------------------

### Using Constrained Planner

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/constrained_planning.rst

Demonstrates how to use the constrained planner by passing the constraint function, its Jacobian, and an optional projection tolerance when calling the planning function.

```python
ompl_constraints = [
        ompl_planner.Constraint(constraint_func_ankor, constraint_jacobian_ankor)
    ]
    ompl_planner.plan(start_config, goal_config, constraints=ompl_constraints, projection_tolerance=0.01)

```

--------------------------------

### Get Single Joint Parent

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the parent joint index for a single joint.

```python
def get_joint_parent(index: int, user: bool = True) -> int:
    pass
```

--------------------------------

### Get Single Joint ID

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the Pinocchio internal ID for a single joint.

```python
def get_joint_id(index: int, user: bool = True) -> int:
    pass
```

--------------------------------

### Attach Object via PlanningWorld Methods

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Demonstrates three methods for attaching objects to a robot within the PlanningWorld, including by geometry and explicit touch links.

```python
from mplib import PlanningWorld, ArticulatedModel, Pose
from mplib.collision_detection.fcl import Sphere, CollisionObject, FCLObject

robot = ArticulatedModel("robot.urdf", "robot.srdf")
world = PlanningWorld([robot])

# Method 1: Attach pre-created object
obj = FCLObject("sphere")
world.attach_object("sphere", robot.name, ee_link_id)

# Method 2: Attach by geometry
world.attach_sphere(
    radius=0.05,
    art_name=robot.name,
    link_id=ee_link_id,
    pose=Pose(p=[0, 0, 0.1])
)

# Method 3: Attach by geometry with explicit touch links
from mplib.collision_detection.fcl import Box
geom = Box(0.1, 0.1, 0.1)
world.attach_object(
    name="box",
    obj_geom=geom,
    art_name=robot.name,
    link_id=ee_link_id,
    pose=Pose(),
    touch_links=["panda_hand"]
)

# Get the attached body
attached = world.get_attached_object("box")
print(f"Attached: {attached.get_name()}")

# Detach
world.detach_object("box", also_remove=True)
```

--------------------------------

### Get Joint Dimensions

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the dimensions of all joints. Most joints have a dimension of 1, but some can be multi-DOF.

```python
def get_joint_dims(user: bool = True) -> np.ndarray:
    pass
```

--------------------------------

### Configure mplib Planner

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/getting_started.rst

Instantiate the Planner class with robot URDF, SRDF, and motion planning constraints. This sets up the robot model and planning environment.

```python
planner = Planner(
            urdf_path,
            srdf_path=srdf_path,
            user_link_names=user_link_names,
            user_joint_names=user_joint_names,
            move_group=move_group,
            joint_vel_limits=joint_vel_limits,
            joint_acc_limits=joint_acc_limits,
        )
```

--------------------------------

### Follow a Time-Parameterized Path

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/plan_a_path.rst

This snippet shows how to use the `sapien` library to simulate and drive a robot along a time-parameterized path generated by `planner.plan_pose()`. Ensure your controller parameters are correctly configured.

```python
# Assuming 'path' is the dictionary returned by planner.plan_pose()
# and 'robot' is a sapien.Robot object

# Example of driving the robot (specific implementation depends on your controller)
# This is a conceptual example, actual implementation may vary.
if path["status"] == "Success":
    for i in range(len(path["time"])):
        # Set target joint positions for the robot controller
        # This is a placeholder, actual control might involve setting velocities or torques
        robot.set_joint_positions(path["position"][i])
        # Advance simulation by the time step
        # Note: The actual time step for simulation might need to be adjusted
        # based on the path's time array and the simulation's time step.
        # For simplicity, we might just wait for a fixed duration or use the path's time array.
        # Example: time_to_wait = path["time"][i] - (path["time"][i-1] if i > 0 else 0)
        # scene.step(time_to_wait)
        pass # Replace with actual simulation step and control logic
    print("Robot is following the path.")
else:
    print("Cannot follow path as it was not planned successfully.")
```

--------------------------------

### Get Chain Joint Indices

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the indices of joints in the chain from the root to a specified end-effector.

```python
def get_chain_joint_index(end_effector: str) -> list[int]:
    pass
```

--------------------------------

### Get Chain Joint Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the names of joints in the chain from the root to a specified end-effector.

```python
def get_chain_joint_names(end_effector: str) -> list[str]:
    pass
```

--------------------------------

### Get Leaf Links

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves a list of names of all leaf links, which are links without any children.

```python
def get_leaf_links() -> list[str]:
    pass
```

--------------------------------

### Get Attached Body

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve an AttachedBody object by its name, which represents an object attached to an articulation.

```python
def get_attached_object(name: str) -> AttachedBody
```

--------------------------------

### Get All Object Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieve a list of names for all static collision objects currently in the PlanningWorld.

```python
def get_object_names() -> list[str]
```

--------------------------------

### Get Base Pose (Alias)

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieve the base pose of the robot. This method is an alias for get_pose().

```python
def get_base_pose() -> Pose:
    # Get the base pose (same as get_pose()).
```

--------------------------------

### Initialize Planner with Custom Limits

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/Planner.md

Instantiate the Planner class with specific URDF, SRDF, and joint velocity/acceleration limits. This is useful for setting up a robot model with custom dynamic constraints.

```python
from mplib import Planner
from pathlib import Path

planner = Planner(
    urdf="path/to/robot.urdf",
    move_group="end_effector",
    srdf="path/to/robot.srdf",
    joint_vel_limits=[1.0, 1.0, 1.0, 1.0],
    joint_acc_limits=[0.5, 0.5, 0.5, 0.5]
)
```

--------------------------------

### Initialize ArticulatedModel from Files

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Load a robot model using URDF and SRDF file paths. Specify specific links and joints to load, and optionally set a name and gravity.

```python
from mplib import ArticulatedModel

robot = ArticulatedModel(
    urdf_filename="robot.urdf",
    srdf_filename="robot.srdf",
    link_names=["base", "link1", "link2", "end_effector"],
    joint_names=["joint1", "joint2", "joint3"]
)
```

--------------------------------

### Get Minimum Distance to Self Collision

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieves the minimum distance to self-collisions. Returns a float value.

```python
def distance_to_self_collision() -> float
```

--------------------------------

### Generate Stubs and Documentation

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Executes a script to build Python wheels, generate stubs using pybind11-stubgen, and generate documentation using pdoc.

```bash
./dev/generate_stubs.sh
```

--------------------------------

### Pose Constructors

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/Pose.md

Provides details on how to create Pose objects, including default creation, from position and quaternion, from a transformation matrix, and from other objects.

```APIDOC
## Pose Constructors

### Default Constructor

```python
def __init__() -> None
```

Creates a default pose with position (0, 0, 0) and quaternion (1, 0, 0, 0).

### Constructor with Position and Quaternion

```python
@overload
def __init__(
    p: np.ndarray = None,
    q: np.ndarray = None,
) -> None
```

Creates a pose with the specified position and quaternion.

**Parameters**:

| Parameter | Type | Description |
|-----------|------|-------------|
| p | `np.ndarray` | Position as (3,1) or (3,) array |
| q | `np.ndarray` | Quaternion as (4,1) or (4,) array in format (w, x, y, z) |

**Example**:

```python
from mplib import Pose
import numpy as np

# Quaternion format is (w, x, y, z)
pose = Pose(
    p=np.array([0.5, 0.0, 0.5]),
    q=np.array([1, 0, 0, 0])  # identity quaternion
)
```

### Constructor with Transformation Matrix

```python
@overload
def __init__(
    matrix: np.ndarray,
) -> None
```

Creates a pose from a 4x4 transformation matrix.

**Parameters**:

| Parameter | Type | Description |
|-----------|------|-------------|
| matrix | `np.ndarray` | 4x4 float64 transformation matrix |

**Example**:

```python
import numpy as np
from mplib import Pose

T = np.eye(4)
T[:3, 3] = [0.5, 0.0, 0.5]  # translation

pose = Pose(T)
```

### Constructor from Object

```python
@overload
def __init__(obj: Any) -> None
```

Creates a pose from an object with `p` and `q` attributes (e.g., `sapien.Pose`) or a 4x4 transformation matrix.
```

--------------------------------

### Get Minimum Distance to Collision

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieves the minimum distance to any collision in the world. Returns a float value.

```python
def distance_to_collision() -> float
```

--------------------------------

### Grasp and Manipulate an Object with AttachedBody

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Demonstrates creating an AttachedBody, planning to grasp it, and then planning to lift it while it's attached.

```python
from mplib import Planner, Pose, AttachedBody
from mplib.collision_detection.fcl import Sphere, FCLObject, CollisionObject
import numpy as np

planner = Planner("panda.urdf", "panda_hand", srdf="panda.srdf")

# Grasp an object
object_geom = Sphere(radius=0.05)
object_col = CollisionObject(object_geom)
obj = FCLObject("sphere", shapes=[object_col])

# Create attached body
attached = AttachedBody(
    name="held_sphere",
    object=obj,
    attached_articulation=planner.robot,
    attached_link_id=planner.move_group_link_id,
    pose=Pose(p=[0, 0, 0.1]),
    touch_links=["panda_hand"]
)

# Plan to grasp position
grasp_pose = Pose(p=[0.4, 0.3, 0.2])
result = planner.plan_pose(grasp_pose, np.zeros(7))

if result["status"] == "Success":
    print("Grasped object")
    
    # Plan while holding object
    lift_pose = Pose(p=[0.4, 0.3, 0.5])
    result = planner.plan_pose(lift_pose, result["position"][-1])
    
    if result["status"] == "Success":
        print("Lifted object")
```

--------------------------------

### Get FCL Model

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieves the underlying FCL collision model. This is used for collision detection operations.

```python
def get_fcl_model() -> FCLModel
```

--------------------------------

### Get Attached Link ID

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Retrieve the integer index of the robot link to which the attached body is connected.

```python
print(attached.get_attached_link_id())
```

--------------------------------

### Get Base Pose

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieve the current base (root) pose of the robot. This is equivalent to calling get_base_pose().

```python
def get_pose() -> Pose:
    # Get the base (root) pose of the robot.
```

--------------------------------

### Combine Screw and Sampling-Based Planning

Source: https://github.com/haosulab/mplib/blob/main/docs/source/tutorials/plan_a_path.rst

Demonstrates a strategy to first attempt planning with `plan_screw()` and fall back to sampling-based methods if it fails due to collisions. This ensures a path is found even for complex scenarios.

```python
# plan_screw ankor
        if self.planner.plan_screw(target_pose, current_q):
            path = self.planner.get_path()
        else:
            path = self.planner.plan_pose(target_pose, current_q)

        # plan_screw ankor end
```

--------------------------------

### Get AttachedBody Name

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Retrieve the unique name assigned to the attached body. This is useful for identification and referencing.

```python
print(attached.get_name())  # "held_object"
```

--------------------------------

### Get Minimum Distance to Robot Collision

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/PlanningWorld.md

Retrieves the minimum distance to robot-world collisions. Returns a float value.

```python
def distance_to_robot_collision() -> float
```

--------------------------------

### RRT-Connect Planning Strategy

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Shows how to use the default RRT-Connect planning strategy and adjust its parameters like planning time and step size.

```APIDOC
## RRT-Connect (Bidirectional RRT)

### Description
Utilizes the RRT-Connect planning strategy, which is the default in MPlib, and shows how to adjust parameters like `planning_time` and `rrt_range`.

### Method
```python
# Basic RRT-Connect
result = planner.plan_qpos(
    goal_qposes=[goal],
    current_qpos=start,
    planning_time=2.0,  # Increase for complex scenes
    rrt_range=0.1       # Step size for RRT tree growth
)
```
```

--------------------------------

### Get Adjacent Links

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves a set of tuples, where each tuple represents a pair of adjacent links connected by a joint.

```python
def get_adjacent_links() -> set[tuple[str, str]]:
    pass
```

--------------------------------

### OMPLPlanner Constructor

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/planning.md

Initializes the OMPLPlanner with a given planning world. This planner utilizes OMPL for motion planning tasks.

```APIDOC
## OMPLPlanner Constructor

### Description
Initializes the OMPLPlanner with a given planning world. This planner utilizes OMPL for motion planning tasks.

### Method
__init__

### Parameters
#### Path Parameters
- **world** (`PlanningWorld`) - Required - The collision-checked planning world

### Request Example
```python
from mplib.planning import OMPLPlanner
from mplib import PlanningWorld, ArticulatedModel

robot = ArticulatedModel("robot.urdf", "robot.srdf")
world = PlanningWorld([robot])
planner = OMPLPlanner(world)
```
```

--------------------------------

### Get Joint Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves a list of all joint names. If user is True, the names are returned in the user-specified order.

```python
def get_joint_names(user: bool = True) -> list[str]:
    pass
```

--------------------------------

### Get Move Group End-Effectors

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieve the names of the end-effector links associated with the current move group configuration.

```python
def get_move_group_end_effectors() -> list[str]:
    # Get the end-effector link names of the move group.
```

--------------------------------

### Get Move Group Joint Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Retrieve the names of the joints that are part of the currently configured move group.

```python
def get_move_group_joint_names() -> list[str]:
    # Get the names of joints in the move group.
```

--------------------------------

### Compile Dynamic Library

Source: https://github.com/haosulab/mplib/blob/main/dev/README.md

Steps to compile the dynamic library from source. This process generates a Python-importable .so file.

```bash
mkdir build && cd build
cmake .. && make -j8
```

--------------------------------

### Create PinocchioModel from URDF file

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Instantiate a PinocchioModel by providing the path to a URDF file. Optionally configure gravity and verbosity.

```python
from mplib.kinematics import PinocchioModel

pinocchio_model = PinocchioModel(
    urdf_filename="robot.urdf",
    gravity=np.array([0, 0, -9.81])
)
```

--------------------------------

### Create Pose from Position and Quaternion

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/Pose.md

Initializes a Pose object with a specified position and quaternion. The quaternion should be in (w, x, y, z) format. This is useful for defining a pose when translation and rotation are known separately.

```python
from mplib import Pose
import numpy as np

# Quaternion format is (w, x, y, z)
pose = Pose(
    p=np.array([0.5, 0.0, 0.5]),
    q=np.array([1, 0, 0, 0])  # identity quaternion
)
```

--------------------------------

### Get Link Names

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves a list of names for all links in the model. If user is True, names are returned in user-specified order.

```python
def get_link_names(user: bool = True) -> list[str]:
    pass
```

--------------------------------

### Time Parameterization with TOPP

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/usage-patterns.md

Applies time parameterization to a raw motion path obtained from the planner, generating smooth velocity and acceleration profiles. Can specify a target duration or let TOPP determine it.

```python
# Get raw path from planner
status, path = planner.planner.plan(
    start_state=start_qpos[planner.move_group_joint_indices],
    goal_states=[goal_qpos[planner.move_group_joint_indices]],
    time=2.0
)

if status == "Exact solution":
    # Apply time parameterization
    times, pos, vel, acc, duration = planner.TOPP(
        path,
        step=0.01,
        verbose=True
    )
    
    # Or with desired duration
    times, pos, vel, acc, duration = planner.TOPP(
        path,
        step=0.01,
        duration=5.0  # 5 second trajectory
    )
```

--------------------------------

### Get Link Jacobian

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves the Jacobian of a specific link in the world frame. The shape of the returned Jacobian is (6, n_dof).

```python
J = pinocchio_model.get_link_jacobian(index=8, local=False)
print(J.shape)  # (6, 9)
```

--------------------------------

### Initialize Empty OcTree

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/collision-detection.md

Creates an empty OcTree for point clouds. Specify the resolution for voxel size.

```python
class OcTree(CollisionGeometry):
    @overload
    def __init__(self, resolution: float = 0.01) -> None
```

--------------------------------

### Get Move Group DoF Dimension

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/ArticulatedModel.md

Determine the degrees of freedom (DoF) dimension for the currently active move group.

```python
def get_move_group_qpos_dim() -> int:
    # Get the dimension (DoF) of the move group.
```

--------------------------------

### Get Joint Limits

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/kinematics.md

Retrieves a list of limits for all joints. Each limit is a numpy array of shape (2,) representing [q_min, q_max].

```python
def get_joint_limits(
    user: bool = True,
) -> list[np.ndarray]:
    pass
```

--------------------------------

### Initialize OcTree from Point Cloud

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/collision-detection.md

Creates an OcTree from a given set of vertices. The resolution parameter defines the voxel size.

```python
    @overload
    def __init__(
        self,
        vertices: np.ndarray,
        resolution: float = 0.01,
    ) -> None
```

--------------------------------

### Get Attached Articulation Model

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/api-reference/AttachedBody.md

Access the ArticulatedModel instance representing the robot to which this object is attached. This allows for further manipulation of the robot model.

```python
robot = attached.get_attached_articulation()
robot.set_qpos(new_qpos, full=True)
```

--------------------------------

### Add and Update Point Cloud

Source: https://github.com/haosulab/mplib/blob/main/_autodocs/usage-patterns.md

Demonstrates adding a point cloud from sensor data to the planning world, updating it with new readings, and removing it. Specify resolution for voxelization.

```python
import numpy as np

# Point cloud from sensor
points = np.random.rand(1000, 3) * [0.5, 0.5, 1.0] + [0.2, -0.25, 0]

# Add to world
planner.update_point_cloud(
    points=points,
    resolution=0.01,  # 1cm voxel size
    name="sensor_cloud"
)

# Update with new sensor reading
new_points = ...
planner.update_point_cloud(points=new_points, name="sensor_cloud")

# Remove when done
planner.remove_point_cloud("sensor_cloud")
```