### Install Documentation Dependencies

Source: https://github.com/dfki-ric/pytransform3d/blob/main/README.md

Installs the project in editable mode with documentation dependencies. Run this command in the main repository folder.

```bash
pip install -e '.[doc]'
```

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

### Install pytransform3d from source

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Install the package after cloning the repository and installing dependencies. This command should be run from the root directory of the cloned repository.

```bash
python setup.py install
```

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

### Install dependencies from requirements.txt

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Install development dependencies required for building from source by running this command in the cloned repository's root directory.

```bash
pip install -r requirements.txt
```

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

### Install pytransform3d with pip

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Use this command to install the library via pip.

```bash
pip install pytransform3d
```

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

### Install pytransform3d from git repository

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Install the latest version directly from the GitHub repository using pip.

```bash
pip install git+https://github.com/dfki-ric/pytransform3d.git
```

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

### Install pytransform3d with all optional dependencies

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Install pytransform3d with the 'all' extra to include support for loading meshes, the 3D visualizer, and pydot export.

```bash
python -m pip install 'pytransform3d[all]'
```

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

### Test PyPI Release in Docker Container

Source: https://github.com/dfki-ric/pytransform3d/wiki/Home

Instructions to set up a Docker container with Python, pip, and git, then install and test pytransform3d.

```bash
docker run -it ubuntu:24.04
apt update
apt install -y python3 python3-pip git
pip3 install pytransform3d[test]
git clone https://github.com/dfki-ric/pytransform3d.git
cd pytransform3d
MPLBACKEND=Agg pytest
```

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

### Install pytransform3d with all features

Source: https://github.com/dfki-ric/pytransform3d/blob/main/README.md

Use pip to install the package with all optional dependencies. For conda users, use the conda-forge channel.

```bash
pip install 'pytransform3d[all]'
```

```bash
conda install -c conda-forge pytransform3d
```

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

### Install pytransform3d with conda

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/install.rst

Use this command to install the library via conda from the conda-forge channel.

```bash
conda install -c conda-forge pytransform3d
```

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

### Active Transformation Example

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transformation_ambiguities.rst

Demonstrates an active transformation where data is moved with the transformation. The dashed basis represents the original frame.

```python
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.transformations import transform, plot_transform
from pytransform3d.plot_utils import make_3d_axis, Arrow3D


plt.figure()
ax = make_3d_axis(1)
plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05),
            xlabel="X", ylabel="Y", zlabel="Z")
ax.view_init(elev=90, azim=-90)
ax.set_xticks(())
ax.set_yticks(())
ax.set_zticks(())

rng = np.random.default_rng(42)
PA = np.ones((10, 4))
PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3))
PA[:, 0] += 0.3
PA[:, :3] += 0.3

x_translation = -0.1
y_translation = 0.2
z_rotation = np.pi / 4.0
A2B = np.array([
    [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation],
    [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation],
    [0.0, 0.0, 1.0, 0.0],
    [0.0, 0.0, 0.0, 1.0]
])
PB = transform(A2B, PA)

plot_transform(ax=ax, A2B=np.eye(4))
ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c="orange")
plot_transform(ax=ax, A2B=A2B, ls="--", alpha=0.5)
ax.scatter(PB[:, 0], PB[:, 1], PB[:, 2], c="cyan")

axis_arrow = Arrow3D(
    [0.7, 0.3],
    [0.4, 0.9],
    [0.2, 0.2],
    mutation_scale=20, lw=3, arrowstyle="-|>", color="k")
ax.add_artist(axis_arrow)

plt.tight_layout()
plt.show()
```

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

### Build and Publish pytransform3d to PyPI

Source: https://github.com/dfki-ric/pytransform3d/wiki/Home

Steps to build the package and upload it to the Python Package Index (PyPI). Requires 'build' and 'twine' to be installed.

```bash
python -m build
twine upload dist/*
rm -rf dist/
```

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

### Visualize with Open3D

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Uses the Open3D visualizer to display a coordinate frame and a sphere. Requires the `open3d` library to be installed.

```python
from pytransform3d import visualizer as pv

fig3d = pv.figure()
fig3d.plot_transform(T, s=0.3)
fig3d.plot_sphere(radius=0.1, c=[0.5, 0.2, 0.8])
fig3d.show()
```

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

### TransformManager Example in Python

Source: https://github.com/dfki-ric/pytransform3d/blob/main/README.md

Demonstrates setting up and using TransformManager to manage and retrieve transformations between different frames. Requires numpy, matplotlib, and pytransform3d. The plot is displayed using plt.show().

```python
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d import rotations as pr
from pytransform3d import transformations as pt
from pytransform3d.transform_manager import TransformManager


rng = np.random.default_rng(0)

ee2robot = pt.transform_from_pq(
    np.hstack((np.array([0.4, -0.3, 0.5]),
               pr.random_quaternion(rng))))
cam2robot = pt.transform_from_pq(
    np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id)))
object2cam = pt.transform_from(
    pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])),
    np.array([0.5, 0.1, 0.1]))

tm = TransformManager()
tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera", "robot", cam2robot)
tm.add_transform("object", "camera", object2cam)

ee2object = tm.get_transform("end-effector", "object")

ax = tm.plot_frames_in("robot", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
plt.show()
```

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

### Prepare Transformation Sequences with NumpyTimeseriesTransform

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transformation_over_time.rst

Prepare transformation sequences using the NumpyTimeseriesTransform class. This example uses screw linear interpolation (ScLERP).

```python
from pytransform3d.transform_manager import NumpyTimeseriesTransform

transforms_a = NumpyTimeseriesTransform()
transforms_a.add_transform(
    0.0, np.eye(4), np.array([0, 0, 0, 1, 0, 0, 0]))
transforms_a.add_transform(
    1.0, np.eye(4), np.array([0, 0, 0, 1, 0, 0, 0]))
transforms_a.add_transform(
    2.0, np.eye(4), np.array([0, 0, 0, 1, 0, 0, 0]))
```

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

### Run Project Tests

Source: https://github.com/dfki-ric/pytransform3d/blob/main/README.md

Executes project tests using pytest. Ensure pytest is installed. For coverage reports, pytest-cov is also required.

```bash
pytest
```

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

### Passive Transformation Example

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transformation_ambiguities.rst

Illustrates a passive transformation where the frame is moved, and the data is interpreted in the new frame. The solid basis represents the new frame.

```python
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import proj3d
from pytransform3d.transformations import transform, plot_transform
from pytransform3d.plot_utils import make_3d_axis, Arrow3D


plt.figure()
ax = make_3d_axis(1)
plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05),
            xlabel="X", ylabel="Y", zlabel="Z")
ax.view_init(elev=90, azim=-90)
ax.set_xticks(())
ax.set_yticks(())
ax.set_zticks(())

rng = np.random.default_rng(42)
PA = np.ones((10, 4))
PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3))
PA[:, 0] += 0.3
PA[:, :3] += 0.3

x_translation = -0.1
y_translation = 0.2
z_rotation = np.pi / 4.0
A2B = np.array([
    [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation],
    [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation],
    [0.0, 0.0, 1.0, 0.0],
    [0.0, 0.0, 0.0, 1.0]
])

plot_transform(ax=ax, A2B=np.eye(4), ls="--", alpha=0.5)
ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c="orange")
plot_transform(ax=ax, A2B=A2B)

axis_arrow = Arrow3D(
    [0.0, -0.1],
    [0.0, 0.2],
    [0.2, 0.2],
    mutation_scale=20, lw=3, arrowstyle="-|>", color="k")
ax.add_artist(axis_arrow)

plt.tight_layout()
plt.show()
```

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

### Build HTML Documentation

Source: https://github.com/dfki-ric/pytransform3d/blob/main/README.md

Commands to build the HTML documentation for the project. Navigate to the 'doc' directory and run 'make html'.

```bash
cd doc
make html
```

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

### Build and Publish pytransform3d to GitHub Pages

Source: https://github.com/dfki-ric/pytransform3d/wiki/Home

Commands to clean, build, and deploy the documentation to GitHub Pages.

```bash
cd doc
make clean
make html
git clone git@github.com:dfki-ric/pytransform3d.git --branch gh-pages
cd pytransform3d
rm -rf *
cp -R ../build/html/* .
git add *

```

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

### Initialize TemporalTransformManager

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transformation_over_time.rst

Pass the prepared transformation sequences to an instance of TemporalTransformManager.

```python
from pytransform3d.transform_manager import TemporalTransformManager

ttm = TemporalTransformManager()
ttm.add_temporal_transform("A", transforms_a)
```

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

### Development and Linting Tools for pytransform3d

Source: https://github.com/dfki-ric/pytransform3d/wiki/Home

A collection of command-line tools for testing, type checking, code formatting, and linting the pytransform3d project.

```bash
pytest
```

```bash
mypy --check-untyped-defs pytransform3d
```

```bash
black . --check
```

```bash
ruff check --output-format=full .
```

```bash
flake8 pytransform3d examples  --show-source --ignore E402,F401,W503,W504,W605,E303,Q000 --statistics --docstring-style numpy
```

```bash
pylint -d C0103,R1725,R0205 --extension-pkg-whitelist=numpy,open3d pytransform3d
```

```bash
darglint --docstring-style numpy pytransform3d
```

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

### Transform Point (Python)

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transformation_modeling.rst

Apply a concatenated transformation to a point using the `transform` function. Ensure the point is correctly represented, for example, using `vector_to_point`.

```python
from pytransform3d.transformations import vector_to_point, transform
p_in_A = vector_to_point(...)  # point in frame A
p_in_C = transform(A2C, p_in_A)
```

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

### Clone the pytransform3d Repository

Source: https://github.com/dfki-ric/pytransform3d/blob/main/CONTRIBUTING.md

Clone your forked repository to your local machine to begin making changes.

```bash
git clone git@github.com:YourLogin/pytransform3d.git
```

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

### Stage and Commit Changes

Source: https://github.com/dfki-ric/pytransform3d/blob/main/CONTRIBUTING.md

Record your modifications using Git's add and commit commands before pushing.

```bash
git add modified_files
git commit
```

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

### Build Transform from Position and Quaternion (PQ)

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Creates a 4x4 homogeneous transformation matrix from a position vector and a quaternion. The input format for PQ is [x, y, z, qw, qx, qy, qz].

```python
# Build from position + quaternion (PQ)
pq = np.array([1.0, 2.0, 0.5, 1.0, 0.0, 0.0, 0.0])  # [x,y,z, qw,qx,qy,qz]
A2B2 = pt.transform_from_pq(pq)
```

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

### Initialize and Use TransformManager

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transform_manager.rst

This snippet shows how to initialize the TransformManager and use it to manage transformations. It is useful for scenarios involving multiple interconnected frames, such as in robotics.

```python
tm = TransformManager()

tm.add_transform("base", "link1", np.eye(4))
tm.add_transform("link1", "link2", np.eye(4))
```

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

### Camera Projection and Coordinate Conversion

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Utilities for pinhole camera models, including projecting 3D points to image coordinates and back-projecting image points to world rays. Requires camera intrinsic and extrinsic parameters.

```python
import numpy as np
from pytransform3d import camera as pc
from pytransform3d import transformations as pt

# Camera intrinsic matrix
sensor_size = (0.00854, 0.00640)  # meters
image_size  = (640, 480)           # pixels
focal_length = 0.012               # meters
M = pc.make_world_grid(n_lines=11, n_points_per_line=51)

# Build camera matrix from focal length and sensor parameters
cam_matrix = pc.estimate_camera_matrix(
    focal_length, sensor_size, image_size)

# Camera extrinsic: world-to-camera transform
cam2world = pt.transform_from(np.eye(3), [0, 0, 1.0])

# Project world grid points to image plane
world_points = pc.make_world_grid()   # shape (N, 4) homogeneous
image_coords = pc.world2image(world_points, cam2world, cam_matrix, image_size)

# Back-project image points to world rays
rays = pc.image2world(image_coords, cam2world, cam_matrix)
```

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

### Initialize and Use TemporalTransformManager

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Manages time-varying transforms, supporting interpolation via ScLERP. Use NumpyTimeseriesTransform for numpy-backed trajectories.

```python
import numpy as np
from pytransform3d.transform_manager import (
    TemporalTransformManager, NumpyTimeseriesTransform, StaticTransform
)
from pytransform3d import transformations as pt

# Build a time-series of poses as PQ arrays [x, y, z, qw, qx, qy, qz]
n_steps = 50
times = np.linspace(0.0, 1.0, n_steps)
pqs = np.zeros((n_steps, 7))
pqs[:, 0] = np.linspace(0, 1, n_steps)   # translate along x
pqs[:, 3] = 1.0                            # unit quaternion (identity rotation)

ttm = TemporalTransformManager()

# Time-varying transform (ScLERP-interpolated)
ttm.add_transform(
    "sensor", "world",
    NumpyTimeseriesTransform(times, pqs, time_clipping=False)
)

# Static transform as a time-varying component
base2world = pt.transform_from(np.eye(3), [0, 0, 0.5])
ttm.add_transform("base", "world", StaticTransform(base2world))
```

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

### Load and Query URDF with UrdfTransformManager

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Load robot kinematic descriptions from URDF files, set joint configurations, and query frame transforms. Visual and collision geometries can also be accessed.

```python
from pytransform3d.urdf import UrdfTransformManager
import numpy as np

utm = UrdfTransformManager()

# Load URDF from file (requires lxml)
with open("robot.urdf", "r") as f:
    utm.load_urdf(f.read(), package_dir=".")

# Set joint angles (in radians)
utm.set_joint("joint_1", 0.5)
utm.set_joint("joint_2", -0.3)

# Query any frame transform
T = utm.get_transform("end_effector", "base_link")

# Iterate visual/collision objects
for visual in utm.visuals:
    print(visual.frame, visual.shape)

# Visualize robot configuration (requires matplotlib)
import matplotlib.pyplot as plt
ax = utm.plot_frames_in("base_link", s=0.05, whitelist=["base_link", "link_2", "end_effector"])
plt.show()
```

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

### Batch SE(3) Operations with trajectories Module

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Perform vectorized batch operations on transforms, position-quaternion sequences, and dual quaternions for significant performance gains over Python loops.

```python
import numpy as np
from pytransform3d import trajectories as ptr
from pytransform3d import rotations as pr

rng = np.random.default_rng(42)
n = 1000

# --- Build a batch of transforms from PQ arrays ---
# pqs: shape (n, 7) — each row [x, y, z, qw, qx, qy, qz]
pqs = np.zeros((n, 7))
pqs[:, :3] = rng.standard_normal((n, 3))
pqs[:, 3:] = pr.random_quaternion(rng) * np.ones((n, 1))  # same orientation
transforms = ptr.transforms_from_pqs(pqs)  # shape (n, 4, 4)

# --- Invert all transforms at once ---
inv_transforms = ptr.invert_transforms(transforms)  # shape (n, 4, 4)

# --- Concatenate one fixed transform with many ---
T_fixed = np.eye(4)
T_fixed[0, 3] = 1.0
result = ptr.concat_one_to_many(T_fixed, transforms)   # shape (n, 4, 4)

# --- Batch dual quaternion operations ---
dqs = ptr.dual_quaternions_from_pqs(pqs)               # shape (n, 8)
pqs_recovered = ptr.pqs_from_dual_quaternions(dqs)

# --- ScLERP interpolation between pose sequences ---
t = np.linspace(0.0, 1.0, n)
dq_start = ptr.dual_quaternions_from_pqs(pqs[:1])
dq_end   = ptr.dual_quaternions_from_pqs(pqs[-1:])
# Interpolate at each t
dq_interp = ptr.dual_quaternions_sclerp(dq_start[0], dq_end[0], t)  # shape (n, 8)

# --- Exponential coordinates batch round-trip ---
Sthetas = ptr.exponential_coordinates_from_transforms(transforms)  # shape (n, 6)
transforms2 = ptr.transforms_from_exponential_coordinates(Sthetas)

# --- Plot a trajectory ---
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ptr.plot_trajectory(ax, pqs, s=0.05, n_frames=10)
plt.show()
```

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

### Position-Quaternion-Screw (PQS) and Dual Quaternion Conversions

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/api.rst

Functions for converting between transformation representations, including PQS and dual quaternions.

```APIDOC
## PQS and Dual Quaternion Conversions

### Functions

- `pqs_from_transforms`
- `pqs_from_dual_quaternions`
- `dual_quaternions_from_pqs`
```

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

### Transformations

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Demonstrates common operations for creating and manipulating 3D transforms.

```APIDOC
## Build a transform from rotation matrix + translation vector
R = pr.matrix_from_axis_angle([0, 0, 1, np.pi / 2])
p = np.array([1.0, 2.0, 0.5])
A2B = pt.transform_from(R, p)
# 4x4 homogeneous matrix [[R, p], [0,0,0,1]]

## Build from position + quaternion (PQ)
```
# [x,y,z, qw,qx,qy,qz]
pq = np.array([1.0, 2.0, 0.5, 1.0, 0.0, 0.0, 0.0])
A2B2 = pt.transform_from_pq(pq)
```

## Invert a transform
```
B2A = pt.invert_transform(A2B)
```

## Concatenate transforms: A->C = (B->C) * (A->B)
```
B2C = pt.transform_from(np.eye(3), [0, 0, 1.0])
A2C = pt.concat(A2B, B2C)
```

## Apply transform to a 3D point
```
point = np.array([1.0, 0.0, 0.0])
point_in_B = pt.transform(A2B, pt.vector_to_point(point))[:3]
```

## Apply transform to a direction (ignores translation)
```
direction_in_B = pt.transform(A2B, pt.vector_to_direction(point))[:3]
```

## Exponential coordinates (Lie algebra se(3), 6-vector)
```
Stheta = pt.exponential_coordinates_from_transform(A2B)
A2B_recovered = pt.transform_from_exponential_coordinates(Stheta)
```

## Screw interpolation (ScLERP) between two transforms
```
T_start = pt.transform_from(np.eye(3), [0, 0, 0])
T_end   = pt.transform_from(R, [1, 0, 0])
T_mid   = pt.transform_sclerp(T_start, T_end, 0.5)
```

## Dual quaternion representation
```
dq = pt.dual_quaternion_from_transform(A2B)
A2B_from_dq = pt.transform_from_dual_quaternion(dq)
```

## Validate a transform matrix
```
try:
    pt.check_transform(A2B)
except ValueError as e:
    print(f"Invalid transform: {e}")
```
```

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

### Rotation Conversions using pytransform3d.rotations

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Perform conversions between various rotation representations like matrices, quaternions, and axis-angle. Ensure rotation matrices are valid using assert_rotation_matrix.

```python
import numpy as np
from pytransform3d import rotations as pr

# Rotation matrix from axis-angle
a = np.array([0.0, 0.0, 1.0, np.pi / 4])  # 45 deg around z-axis
R = pr.matrix_from_axis_angle(a)
# array([[ 0.707, -0.707,  0.   ],
#        [ 0.707,  0.707,  0.   ],
#        [ 0.   ,  0.   ,  1.   ]])

# Quaternion from rotation matrix
q = pr.quaternion_from_matrix(R)
# array([0.924, 0.   , 0.   , 0.383])  -> (w, x, y, z)

# Back to rotation matrix
R2 = pr.matrix_from_quaternion(q)
pr.assert_rotation_matrix(R2)  # validates orthonormality and det=1

# Euler angles (intrinsic ZYX / roll-pitch-yaw) from matrix
e = pr.euler_from_matrix(R, i=2, j=1, k=0, extrinsic=False)

# Axis-angle from two direction vectors
a2 = pr.axis_angle_from_two_directions([1, 0, 0], [0, 1, 0])

# Renormalize a noisy quaternion
q_noisy = q + np.random.randn(4) * 1e-8
if pr.quaternion_requires_renormalization(q_noisy):
    q_clean = pr.check_quaternion(q_noisy)

# Concatenate (compose) two quaternions
q_combined = pr.concatenate_quaternions(q, pr.random_quaternion(np.random.default_rng(0)))

# Apply quaternion rotation to a vector
v = np.array([1.0, 0.0, 0.0])
v_rotated = pr.q_prod_vector(q, v)

# Quaternion SLERP interpolation
q_start = pr.q_id  # identity
q_end   = pr.quaternion_from_axis_angle([0, 0, 1, np.pi / 2])
q_mid   = pr.quaternion_slerp(q_start, q_end, 0.5)

# Rotation matrix SLERP
R_start = np.eye(3)
R_end   = pr.matrix_from_axis_angle([0, 1, 0, np.pi / 3])
R_mid   = pr.matrix_slerp(R_start, R_end, 0.5)
```

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

### Visualization Utilities with Matplotlib and Open3D

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Integrates with Matplotlib for 2D/3D plots and Open3D for interactive rendering. Includes utilities for drawing coordinate frames, geometries, and trajectories.

```python
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d import rotations as pr
from pytransform3d import transformations as pt
from pytransform3d import plot_utils as ppu
```

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

### Initialize and Use TransformManager

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Manages a graph of named coordinate frames and their transformations. Allows registering, retrieving, and checking consistency of transforms.

```python
import numpy as np
from pytransform3d import rotations as pr
from pytransform3d import transformations as pt
from pytransform3d.transform_manager import TransformManager

tm = TransformManager()

# Register known transforms (frame_A → frame_B)
ee2robot = pt.transform_from_pq(
    np.hstack(([0.4, -0.3, 0.5], pr.random_quaternion(np.random.default_rng(0))))
)
cam2robot = pt.transform_from_pq(np.hstack(([0.0, 0.0, 0.8], pr.q_id)))
obj2cam   = pt.transform_from(
    pr.matrix_from_euler([0, 0, -0.5], 0, 1, 2, extrinsic=False),
    [0.5, 0.1, 0.1]
)

tm.add_transform("end-effector", "robot", ee2robot)
tm.add_transform("camera",       "robot", cam2robot)
tm.add_transform("object",       "camera", obj2cam)

# Request any derived transform — multi-hop paths resolved automatically
ee2object = tm.get_transform("end-effector", "object")
obj2robot = tm.get_transform("object", "robot")

# Check consistency (detect contradictory loops)
consistent = tm.check_consistency()

# Remove a transform
tm.remove_transform("camera", "robot")

# Serialize / deserialize to a JSON-compatible dict
data = tm.to_dict()
tm2 = TransformManager()
tm2 = tm2.from_dict(data)
```

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

### Camera Utilities

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/api.rst

Functions for camera-related operations, including creating world grids and lines, and converting between different camera coordinate systems.

```APIDOC
## Camera Utilities

### Functions

- `make_world_grid`
- `make_world_line`
- `cam2sensor`
- `sensor2img`
- `world2image`
- `plot_camera`
```

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

### Export Transform Graph as PNG

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/user_guide/transform_manager.rst

This code demonstrates how to export the current transformation graph managed by the TransformManager to a PNG file. This is useful for visualizing the structure of transformations.

```python
tm.write_png(filename)
```

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

### Euler Angle Conversions using pytransform3d.rotations

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Use unified functions for all 12 intrinsic and 12 extrinsic Euler/Tait-Bryan conventions by specifying axis indices and the extrinsic flag. Check for gimbal lock and normalize angles.

```python
import numpy as np
from pytransform3d import rotations as pr

# Intrinsic XYZ (aerospace: roll-pitch-yaw body frame)
e_xyz = np.array([0.1, 0.2, 0.3])   # radians
R = pr.matrix_from_euler(e_xyz, i=0, j=1, k=2, extrinsic=False)

# Recover Euler angles from matrix
e_recovered = pr.euler_from_matrix(R, i=0, j=1, k=2, extrinsic=False)

# Extrinsic ZYX (common robotics convention)
R2 = pr.matrix_from_euler(e_xyz, i=2, j=1, k=0, extrinsic=True)

# Check for gimbal lock
if pr.euler_near_gimbal_lock(e_xyz, i=0, j=1, k=2):
    print("Near gimbal lock — consider switching representation")

# Normalize Euler angle to canonical range
e_norm = pr.norm_euler(e_xyz, i=0, j=1, k=2)

# Quaternion directly from Euler angles
q = pr.quaternion_from_euler(e_xyz, i=0, j=1, k=2, extrinsic=False)
```

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

### Build Transforms from Rotation Matrix and Translation

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Constructs a 4x4 homogeneous transformation matrix from a rotation matrix and a translation vector. Requires numpy and pytransform3d.transformations.

```python
import numpy as np
from pytransform3d import rotations as pr
from pytransform3d import transformations as pt

R = pr.matrix_from_axis_angle([0, 0, 1, np.pi / 2])
p = np.array([1.0, 2.0, 0.5])
A2B = pt.transform_from(R, p)
# 4x4 homogeneous matrix [[R, p], [0,0,0,1]]
```

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

### Export TransformManager Graph to PNG

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Saves a visualization of the transform graph as a PNG image. Requires pydot.

```python
# Export graph as PNG (requires pydot)
tm.write_png("/tmp/transform_graph.png")
```

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

### Screw Parameters Operations

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/api.rst

Functions for checking, plotting, asserting equality, and converting screw parameters.

```APIDOC
## check_screw_parameters

### Description
Checks if the given parameters represent valid screw parameters.

### Method
`check_screw_parameters(screw_params)`

### Parameters
- **screw_params** (numpy.ndarray) - The screw parameters.

### Response
- **bool** - True if valid, False otherwise.
```

```APIDOC
## plot_screw

### Description
Plots the screw axis and pitch.

### Method
`plot_screw(screw_params, ax)`

### Parameters
- **screw_params** (numpy.ndarray) - The screw parameters.
- **ax** (matplotlib.axes.Axes) - The matplotlib axes to plot on.
```

```APIDOC
## assert_screw_parameters_equal

### Description
Asserts that two sets of screw parameters are equal.

### Method
`assert_screw_parameters_equal(screw_params1, screw_params2)`

### Parameters
- **screw_params1** (numpy.ndarray) - The first set of screw parameters.
- **screw_params2** (numpy.ndarray) - The second set of screw parameters.

### Raises
- **AssertionError** - If the screw parameters are not equal.
```

```APIDOC
## screw_parameters_from_screw_axis

### Description
Computes screw parameters from a screw axis.

### Method
`screw_parameters_from_screw_axis(screw_axis)`

### Parameters
- **screw_axis** (numpy.ndarray) - The screw axis.

### Response
- **numpy.ndarray** - The screw parameters.
```

```APIDOC
## screw_parameters_from_dual_quaternion

### Description
Computes screw parameters from a dual quaternion.

### Method
`screw_parameters_from_dual_quaternion(dual_quaternion)`

### Parameters
- **dual_quaternion** (numpy.ndarray) - The dual quaternion.

### Response
- **numpy.ndarray** - The screw parameters.
```

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

### Visualizer Functions

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/api.rst

Functions for creating and managing visualizations.

```APIDOC
## Visualizer Functions

### Functions

- `figure`
```

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

### Screw Parameters from Dual Quaternions

Source: https://github.com/dfki-ric/pytransform3d/blob/main/doc/source/api.rst

Function to derive screw parameters from dual quaternions.

```APIDOC
## Screw Parameters from Dual Quaternions

### Functions

- `screw_parameters_from_dual_quaternions`
```

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

### Add Upstream Remote Repository

Source: https://github.com/dfki-ric/pytransform3d/blob/main/CONTRIBUTING.md

Add the main pytransform3d repository as an 'upstream' remote to fetch changes from the original project.

```bash
git remote add upstream https://github.com/dfki-ric/pytransform3d.git
```

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

### trajectories Module

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Provides vectorized batch operations on arrays of transforms, position-quaternion sequences, and dual quaternion arrays for efficient SE(3) manipulation.

```APIDOC
## trajectories Module

Provides vectorized batch operations on arrays of transforms, position-quaternion sequences, and dual quaternion arrays for efficient SE(3) manipulation.

### Functions

- **transforms_from_pqs(pqs)**: Converts an array of position-quaternion (PQ) sequences to an array of 4x4 homogeneous transformation matrices.
- **invert_transforms(transforms)**: Inverts a batch of homogeneous transformation matrices.
- **concat_one_to_many(T_fixed, transforms)**: Concatenates a single fixed transform with each transform in a batch.
- **dual_quaternions_from_pqs(pqs)**: Converts an array of PQ sequences to an array of dual quaternions.
- **pqs_from_dual_quaternions(dqs)**: Converts an array of dual quaternions back to PQ sequences.
- **dual_quaternions_sclerp(dq_start, dq_end, t)**: Performs spherical linear interpolation (SLERP) between two dual quaternions over a given time parameter.
- **exponential_coordinates_from_transforms(transforms)**: Converts an array of homogeneous transformation matrices to their exponential coordinate representation (6D vector).
- **transforms_from_exponential_coordinates(Sthetas)**: Converts an array of exponential coordinates back to homogeneous transformation matrices.
- **plot_trajectory(ax, pqs, s, n_frames)**: Plots a trajectory represented by a sequence of poses on a given matplotlib axes.

### Example Usage
```python
import numpy as np
from pytransform3d import trajectories as ptr
from pytransform3d import rotations as pr

rng = np.random.default_rng(42)
n = 1000

# --- Build a batch of transforms from PQ arrays ---
# pqs: shape (n, 7) — each row [x, y, z, qw, qx, qy, qz]
pqs = np.zeros((n, 7))
pqs[:, :3] = rng.standard_normal((n, 3))
pqs[:, 3:] = pr.random_quaternion(rng) * np.ones((n, 1))  # same orientation
transforms = ptr.transforms_from_pqs(pqs)  # shape (n, 4, 4)

# --- Invert all transforms at once ---
inv_transforms = ptr.invert_transforms(transforms)  # shape (n, 4, 4)

# --- Concatenate one fixed transform with many ---
T_fixed = np.eye(4)
T_fixed[0, 3] = 1.0
result = ptr.concat_one_to_many(T_fixed, transforms)   # shape (n, 4, 4)

# --- Batch dual quaternion operations ---
dqs = ptr.dual_quaternions_from_pqs(pqs)               # shape (n, 8)
pqs_recovered = ptr.pqs_from_dual_quaternions(dqs)

# --- ScLERP interpolation between pose sequences ---
t = np.linspace(0.0, 1.0, n)
dq_start = ptr.dual_quaternions_from_pqs(pqs[:1])
dq_end   = ptr.dual_quaternions_from_pqs(pqs[-1:])
# Interpolate at each t
dq_interp = ptr.dual_quaternions_sclerp(dq_start[0], dq_end[0], t)  # shape (n, 8)

# --- Exponential coordinates batch round-trip ---
Sthetas = ptr.exponential_coordinates_from_transforms(transforms)  # shape (n, 6)
transforms2 = ptr.transforms_from_exponential_coordinates(Sthetas)

# --- Plot a trajectory ---
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ptr.plot_trajectory(ax, pqs, s=0.05, n_frames=10)
plt.show()
```
```

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

### batch_rotations Module

Source: https://context7.com/dfki-ric/pytransform3d/llms.txt

Performs vectorized batch operations on arrays of rotations, supporting conversions between quaternions, rotation matrices, and axis-angles.

```APIDOC
## batch_rotations Module

Performs vectorized batch operations on arrays of rotations, supporting conversions between quaternions, rotation matrices, and axis-angles.

### Functions

- **matrices_from_quaternions(Q)**: Converts an array of quaternions to an array of 3x3 rotation matrices.
- **quaternions_from_matrices(Rs)**: Converts an array of 3x3 rotation matrices to an array of quaternions.
- **axis_angles_from_matrices(Rs)**: Converts an array of 3x3 rotation matrices to an array of axis-angle representations.
- **active_matrices_from_intrinsic_euler_angles(i, j, k, e)**: Converts intrinsic Euler angles to active rotation matrices.
- **matrices_from_compact_axis_angles(compact_as)**: Converts compact axis-angle representations to 3x3 rotation matrices.
- **smooth_quaternion_trajectory(Q_raw)**: Smooths a quaternion trajectory by minimizing arc length between consecutive quaternions.
- **quaternion_slerp_batch(Q_starts, Q_ends, t_vals)**: Performs batch spherical linear interpolation (SLERP) between pairs of quaternions.

### Example Usage
```python
import numpy as np
from pytransform3d import batch_rotations as pbr

rng = np.random.default_rng(0)
n = 500

# --- Batch quaternions → rotation matrices ---
# Q: shape (n, 4), each row (w, x, y, z)
Q = np.tile([1.0, 0, 0, 0], (n, 1))  # all identity
Q += rng.standard_normal((n, 4)) * 0.01
Rs = pbr.matrices_from_quaternions(Q)   # shape (n, 3, 3)

# --- Batch rotation matrices → quaternions ---
Q2 = pbr.quaternions_from_matrices(Rs)  # shape (n, 4)

# --- Batch rotation matrices → axis-angles ---
As = pbr.axis_angles_from_matrices(Rs)  # shape (n, 4)

# --- Batch Euler angles → rotation matrices ---
euler_angles = rng.standard_normal((n, 3)) * 0.3
Rs_from_euler = pbr.active_matrices_from_intrinsic_euler_angles(
    i=0, j=1, k=2, e=euler_angles)  # shape (n, 3, 3)

# --- Batch compact axis-angles → rotation matrices ---
compact_as = rng.standard_normal((n, 3)) * 0.1
Rs_from_ca = pbr.matrices_from_compact_axis_angles(compact_as)

# --- Smooth a quaternion trajectory (flip signs to minimize arc length) ---
Q_raw = rng.standard_normal((100, 4))
Q_raw /= np.linalg.norm(Q_raw, axis=1, keepdims=True)
Q_smooth = pbr.smooth_quaternion_trajectory(Q_raw)

# --- Batch SLERP ---
Q_starts = np.tile([1, 0, 0, 0], (n, 1)).astype(float)
Q_ends   = pbr.quaternions_from_matrices(pbr.matrices_from_compact_axis_angles(
    rng.standard_normal((n, 3)) * 0.5))
t_vals   = np.linspace(0, 1, n)
Q_interp = pbr.quaternion_slerp_batch(Q_starts, Q_ends, t_vals)  # shape (n, 4)
```
```