piper-planner

piper-planner plans multiple continuous, time-parameterized joint trajectories for the standard AgileX Piper and Piper-X arms. It uses Pinocchio for kinematics, favors keeping a target in view during motion, and returns the best validated trace first while preserving useful trajectory diversity for data collection.

The package plans offline and does not connect to CAN, command a robot, or require LeKit.

Installation

Install the published package:

pip install piper-planner

Install a downloaded wheel:

pip install dist/piper_planner-0.1.3-py3-none-any.whl

Install directly from this monorepo:

pip install "piper-planner @ git+https://github.com/sorelferris/lekit.git@main#subdirectory=packages/piper_planner"

The package requires Python 3.10 or newer. CI currently verifies Python 3.10, 3.11, and 3.12 on Linux x86_64; other Python versions, operating systems, and architectures are unverified.

Basic usage

import numpy as np

from piper_planner import PiperTrajectoryPlanner, PlanningOptions, Pose

planner = PiperTrajectoryPlanner.for_model("piper")

q0 = np.array([
    0.3490658503988659,
    0.7853981633974483,
    -0.8726646259971648,
    0.5235987755982988,
    -0.4363323129985824,
    0.17453292519943295,
])
x1 = Pose.from_xyz_rpy([
    0.14150336082787257,
    0.031721426288498916,
    0.4616535233532061,
    0.866514946550442,
    0.6293760380025386,
    1.2054974044483533,
])

traces = planner.plan(
    q0=q0,
    x1=x1,
    observation_target=x1.position,
    options=PlanningOptions(num_traces=8, random_seed=7),
)

best = traces[0]
print(best.positions_rad.shape, best.timestamps_s[-1], best.score)

q0 contains the six measured joint angles immediately before planning, in SDK joint order and radians. The planner computes x0 = FK(q0); callers do not supply a separate starting pose.

x1 is the desired TCP pose in the Piper base frame. Positions are meters, Euler angles passed to Pose.from_xyz_rpy() are radians, and stored quaternions use normalized xyzw order.

Observation behavior

observation_target is either a three-element XYZ point in the base frame or None.

When a point is supplied, the planner treats visibility as a soft objective during the middle of the motion. Near the end, the TCP orientation smoothly fades back to the exact orientation in x1; the default fade begins at 80% path progress. When observation_target=None, orientation follows the start-to-target pose interpolation without an observation objective.

The default TcpAxisCameraModel treats the TCP local +Z axis as the camera viewing axis. This tcp_axis_proxy approximation is useful when camera extrinsics are unavailable, but it cannot guarantee that the target appears inside the physical image. A calibrated adapter is available from piper_planner.camera.CalibratedCameraModel when camera-to-TCP extrinsics and field of view are known. Pass it with PiperTrajectoryPlanner.for_model(..., camera_model=...).

Set the measured tool transform with the six-element tcp_offset argument. Incorrect TCP or camera calibration makes FK, gaze direction, and terminal pose refer to the wrong tool frame.

Results and diversity

Every returned JointTrace has immutable arrays:

• timestamps_s: seconds, starting at zero;
• positions_rad: shape (N, 6);
• velocities_rad_s: shape (N, 6);
• accelerations_rad_s2: shape (N, 6);
• score, rank, and detailed TraceMetrics.

Only densely validated traces are returned. The list is sorted best first, so traces[0] is the globally best selected trajectory. Remaining entries are selected for spatial, joint-configuration, and timing diversity without displacing that best result.

Use a fixed non-negative random_seed for reproducible planning. Leave it as None to allow different candidate paths across calls.

Timing and errors

The default cooperative planning budget is three seconds. If time expires after at least one feasible trace is found, the planner returns available results and sets trace.metrics.planning_truncated=True. It may therefore return fewer than PlanningOptions.num_traces.

• InvalidPlanningRequestError reports malformed or out-of-range requests.
• KinematicModelError reports URDF or Pinocchio model failures.
• NoFeasibleTrajectoryError reports that no candidate satisfied the hard constraints and includes per-stage failure counts.

Safety scope

The planner validates configured workspace, endpoint tolerance, joint limits, velocity, acceleration, jerk, and kinematic numerical health. It does not perform environment collision or self-collision checking. A returned trace is not a guarantee of collision-free or physically safe execution.

For first execution on hardware, clear and supervise the workspace, keep the emergency stop reachable, use conservative controller limits, execute the best trace first, and compare commanded and measured joints continuously.

Example

The included example requires no robot hardware and prints a ranked JSON summary:

uv run python examples/plan_trajectory.py \
  --robot-model piper_x \
  --seed 7 \
  --num-traces 2

Development and packaging

Run commands from packages/piper_planner:

uv sync --all-groups
uv run ruff check src tests examples scripts
uv run pytest -q
uv build
uv run twine check dist/*

The project uses an independent uv.lock. Downstream pip installations use the dependency constraints in pyproject.toml, not the lock file.

Releases use tags such as piper-planner-v0.1.3. The tag version must exactly match project.version; GitHub Actions then publishes with PyPI Trusted Publishing and no long-lived API token.

Licenses

The Python source is licensed under Apache-2.0; see LICENSE. Packaged URDF assets retain the separate license and provenance files under src/piper_planner/assets/. Apache-2.0 does not relicense those third-party assets.