Trajectory Generation¶

Functions and classes for generating robot trajectories.

Overview¶

Trajectory generation converts 2D sketches to 3D robot joint positions using inverse kinematics.

graph LR
    A[Sketch] --> B[Map to 3D]
    B --> C[Solve IK]
    C --> D[Interpolate]
    D --> E[Trajectory]

Main Function¶

generate_trajectory¶

generate_trajectory ¶

generate_trajectory(
    image, output_path=None, config=None, initial_q=None
)

Convenience function: Convert image directly to robot trajectory.

This is a one-shot function that combines image_to_sketch() and sketch_to_trajectory() for simple use cases.

Parameters:

Name	Type	Description	Default
`image`	`Union[str, Path, ndarray, Image]`	Input image (file path, numpy array, or PIL Image).	required
`output_path`	`Optional[Union[str, Path]]`	If provided, save trajectory JSON to this path.	`None`
`config`	`Optional[TrajectoryConfig]`	Full configuration (uses sensible defaults if None).	`None`
`initial_q`	`Optional[Union[ndarray, dict, Trajectory]]`	Initial joint configuration to start IK solving from. Can be one of: - numpy array of shape (n_joints,) with joint positions in radians - dict mapping joint names to positions in radians - Trajectory object (uses last waypoint) - None (default): use fixed initial pose for drawing This enables sequential trajectory execution where each new trajectory starts from the robot's current position.	`None`

Returns:

Type	Description
`Trajectory`	Trajectory object ready for execution.

Example

import pib3 trajectory = pib3.generate_trajectory("my_drawing.png") trajectory.to_json("output.json")

With custom config¶

from pib3 import TrajectoryConfig, PaperConfig config = TrajectoryConfig( ... paper=PaperConfig(size=0.20, drawing_scale=0.9), ... ) trajectory = pib3.generate_trajectory("drawing.png", config=config)

Sequential trajectories (robot draws multiple images)¶

traj1 = pib3.generate_trajectory("image1.png") traj2 = pib3.generate_trajectory("image2.png", initial_q=traj1) traj3 = pib3.generate_trajectory("image3.png", initial_q=traj2)

Source code in pib3/__init__.py

def generate_trajectory(
    image: Union[str, Path, "np.ndarray", "PIL.Image.Image"],
    output_path: Optional[Union[str, Path]] = None,
    config: Optional[TrajectoryConfig] = None,
    initial_q: Optional[Union["np.ndarray", dict, Trajectory]] = None,
) -> Trajectory:
    """
    Convenience function: Convert image directly to robot trajectory.

    This is a one-shot function that combines image_to_sketch() and
    sketch_to_trajectory() for simple use cases.

    Args:
        image: Input image (file path, numpy array, or PIL Image).
        output_path: If provided, save trajectory JSON to this path.
        config: Full configuration (uses sensible defaults if None).
        initial_q: Initial joint configuration to start IK solving from.
            Can be one of:
            - numpy array of shape (n_joints,) with joint positions in radians
            - dict mapping joint names to positions in radians
            - Trajectory object (uses last waypoint)
            - None (default): use fixed initial pose for drawing

            This enables sequential trajectory execution where each new
            trajectory starts from the robot's current position.

    Returns:
        Trajectory object ready for execution.

    Example:
        >>> import pib3
        >>> trajectory = pib3.generate_trajectory("my_drawing.png")
        >>> trajectory.to_json("output.json")

        >>> # With custom config
        >>> from pib3 import TrajectoryConfig, PaperConfig
        >>> config = TrajectoryConfig(
        ...     paper=PaperConfig(size=0.20, drawing_scale=0.9),
        ... )
        >>> trajectory = pib3.generate_trajectory("drawing.png", config=config)

        >>> # Sequential trajectories (robot draws multiple images)
        >>> traj1 = pib3.generate_trajectory("image1.png")
        >>> traj2 = pib3.generate_trajectory("image2.png", initial_q=traj1)
        >>> traj3 = pib3.generate_trajectory("image3.png", initial_q=traj2)
    """
    if config is None:
        config = TrajectoryConfig()

    # Step 1: Image to sketch
    sketch = image_to_sketch(image, config.image)

    # Step 2: Sketch to trajectory
    trajectory = sketch_to_trajectory(
        sketch, config, initial_q=initial_q
    )

    # Optional: Save to file
    if output_path is not None:
        trajectory.to_json(output_path)

    return trajectory

Usage¶

import pib3

# Basic usage
trajectory = pib3.generate_trajectory("drawing.png")
trajectory.to_json("output.json")

# With configuration
from pib3 import TrajectoryConfig, PaperConfig
config = TrajectoryConfig(
    paper=PaperConfig(size=0.15, drawing_scale=0.9)
)
trajectory = pib3.generate_trajectory("drawing.png", config=config)

# Save directly while generating
trajectory = pib3.generate_trajectory("drawing.png", output_path="output.json")

# Sequential trajectories: start each IK from the previous trajectory's end pose
traj1 = pib3.generate_trajectory("image1.png")
traj2 = pib3.generate_trajectory("image2.png", initial_q=traj1)

sketch_to_trajectory¶

sketch_to_trajectory ¶

sketch_to_trajectory(
    sketch,
    config=None,
    progress_callback=None,
    initial_q=None,
)

Convert a Sketch to a robot Trajectory using inverse kinematics.

Parameters:

Name	Type	Description	Default
`sketch`	`Sketch`	Sketch object containing strokes to draw.	required
`config`	`Optional[TrajectoryConfig]`	Trajectory configuration. Uses defaults if None.	`None`
`progress_callback`	`Optional[Callable[[int, int, bool], None]]`	Optional callback(current_point, total_points, success).	`None`
`initial_q`	`Optional[Union[ndarray, Dict[str, float], Trajectory]]`	Initial joint configuration to start IK solving from. Can be one of: - numpy array of shape (n_joints,) with joint positions in radians - dict mapping joint names to positions in radians - Trajectory object (uses last waypoint) - None (default): use fixed initial pose for drawing This allows sequential trajectory execution where each new trajectory starts from the end position of the previous one.	`None`

Returns:

Type	Description
`Trajectory`	Trajectory object with joint positions for each waypoint.

Raises:

Type	Description
`ImportError`	If roboticstoolbox is not installed.
`RuntimeError`	If no IK solutions are found.

Example

from pib3 import image_to_sketch, sketch_to_trajectory sketch = image_to_sketch("drawing.png") trajectory = sketch_to_trajectory(sketch) trajectory.to_json("output.json")

Sequential trajectories¶

traj1 = sketch_to_trajectory(sketch1) traj2 = sketch_to_trajectory(sketch2, initial_q=traj1) # Start from traj1 end

Source code in pib3/trajectory.py

def sketch_to_trajectory(
    sketch: Sketch,
    config: Optional[TrajectoryConfig] = None,
    progress_callback: Optional[Callable[[int, int, bool], None]] = None,
    initial_q: Optional[Union[np.ndarray, Dict[str, float], "Trajectory"]] = None,
) -> Trajectory:
    """
    Convert a Sketch to a robot Trajectory using inverse kinematics.

    Args:
        sketch: Sketch object containing strokes to draw.
        config: Trajectory configuration. Uses defaults if None.
        progress_callback: Optional callback(current_point, total_points, success).
        initial_q: Initial joint configuration to start IK solving from.
            Can be one of:
            - numpy array of shape (n_joints,) with joint positions in radians
            - dict mapping joint names to positions in radians
            - Trajectory object (uses last waypoint)
            - None (default): use fixed initial pose for drawing

            This allows sequential trajectory execution where each new
            trajectory starts from the end position of the previous one.

    Returns:
        Trajectory object with joint positions for each waypoint.

    Raises:
        ImportError: If roboticstoolbox is not installed.
        RuntimeError: If no IK solutions are found.

    Example:
        >>> from pib3 import image_to_sketch, sketch_to_trajectory
        >>> sketch = image_to_sketch("drawing.png")
        >>> trajectory = sketch_to_trajectory(sketch)
        >>> trajectory.to_json("output.json")

        >>> # Sequential trajectories
        >>> traj1 = sketch_to_trajectory(sketch1)
        >>> traj2 = sketch_to_trajectory(sketch2, initial_q=traj1)  # Start from traj1 end
    """
    import spatialmath as sm

    from .dh_model import get_dh_robot

    if config is None:
        config = TrajectoryConfig()

    # Determine arm configuration
    arm = config.ik.arm
    grip_style = config.ik.grip_style

    if arm == "left":
        arm_joints = LEFT_ARM_JOINTS
        tcp_link_finger = LEFT_TCP_LINK
        tcp_link_pencil = LEFT_TCP_LINK_PENCIL
    else:
        arm_joints = RIGHT_ARM_JOINTS
        tcp_link_finger = RIGHT_TCP_LINK
        tcp_link_pencil = RIGHT_TCP_LINK_PENCIL

    arm_joint_indices = list(arm_joints.values())
    n_joints = max(URDF_TO_MOTOR_NAME) + 1

    # Drawing tool offset (extra finger/pencil tip offset from the standard
    # tooltip). Only the 6 arm joints are IK degrees of freedom; finger joints
    # stay fixed at the grip pose set by _set_initial_arm_pose. The DH model
    # carries the calibrated base + tool; tool_offset is composed onto the tool.
    if grip_style == "pencil_grip":
        tcp_link = tcp_link_pencil
        tool_offset = sm.SE3(0.04, -0.06, 0)  # 40mm forward, 60mm toward pinky
    else:
        # Index finger drawing: use finger tip as TCP
        tcp_link = tcp_link_finger
        tool_offset = sm.SE3(0, 0.027, 0)  # 27mm in Y (finger tip direction)

    # Calibrated DH model (base + tool) — used for IK and the paper anchor.
    dh_robot = get_dh_robot(arm, tool_offset)

    # Initialize full joint configuration: arm slots are the IK warm start,
    # finger slots hold the fixed grip pose.
    if initial_q is not None:
        # Use provided initial configuration
        if isinstance(initial_q, Trajectory):
            # Expand trajectory back to full robot configuration by mapping
            # each motor name to all URDF indices that share that name
            # (e.g. 'index_left_stretch' drives URDF indices 11 and 12).
            q_current = _set_initial_arm_pose(np.zeros(n_joints), arm, grip_style)
            last_wp = initial_q.waypoints[-1]
            motor_to_indices: Dict[str, List[int]] = {}
            for idx, name in URDF_TO_MOTOR_NAME.items():
                motor_to_indices.setdefault(name, []).append(idx)
            for j, name in enumerate(initial_q.joint_names):
                if j >= len(last_wp):
                    break
                for urdf_idx in motor_to_indices.get(name, []):
                    q_current[urdf_idx] = last_wp[j]
        elif isinstance(initial_q, dict):
            # Convert dict of joint names to array.
            # Build a mapping from motor name -> list of URDF indices, because
            # coupled finger joints share the same motor name for both proximal
            # and distal URDF joints (e.g. indices 11 & 12 are both
            # "index_left_stretch") and both must be set to the same value.
            q_current = _set_initial_arm_pose(np.zeros(n_joints), arm, grip_style)
            joint_name_to_indices: Dict[str, List[int]] = {}
            for idx, name in URDF_TO_MOTOR_NAME.items():
                joint_name_to_indices.setdefault(name, []).append(idx)
            for name, value in initial_q.items():
                if name in joint_name_to_indices:
                    for idx in joint_name_to_indices[name]:
                        q_current[idx] = value
        else:
            # Assume numpy array (full robot configuration)
            q_current = np.asarray(initial_q, dtype=np.float64).copy()
            if q_current.shape[0] != n_joints:
                raise ValueError(
                    f"initial_q array has {q_current.shape[0]} elements, "
                    f"expected {n_joints}"
                )
    else:
        # Default: start from fixed initial pose
        q_current = _set_initial_arm_pose(np.zeros(n_joints), arm, grip_style)

    # Paper geometry is expressed in the torso frame (meters), matching the DH
    # base. Auto-place any unspecified field around a reachable anchor — the FK
    # of the workspace-middle joint configuration — so the paper centre is
    # reachable by construction. Tune PaperConfig on hardware for the real
    # drawing surface.
    qmid = np.array([
        0.5 * (lk.qlim[0] + lk.qlim[1]) if lk.qlim is not None else 0.0
        for lk in dh_robot.links
    ])
    T_anchor = dh_robot.fkine(qmid)
    if isinstance(T_anchor, np.ndarray):
        T_anchor = sm.SE3(T_anchor)
    anchor_m = np.array(T_anchor.t).flatten() / 1000.0  # mm -> m (torso frame)

    # Work on a copy of the paper config to avoid mutating the caller's config
    paper = deepcopy(config.paper)
    if paper.height_z is None:
        paper.height_z = float(anchor_m[2])
    if paper.center_y is None:
        paper.center_y = float(anchor_m[1])
    if paper.start_x is None:
        paper.start_x = float(anchor_m[0] - paper.size / 2.0)

    # Generate 3D trajectory points
    trajectory_3d = []
    for stroke in sketch.strokes:
        dense_points = _interpolate_stroke_points(stroke, config.point_density)
        if not dense_points:
            continue

        # Lift before stroke
        u0, v0 = dense_points[0]
        x, y, z = _map_to_3d(u0, v0, paper)
        trajectory_3d.append((x, y, z + paper.lift_height, True))

        # Draw stroke
        for u, v in dense_points:
            x, y, z = _map_to_3d(u, v, paper)
            trajectory_3d.append((x, y, z, False))

        # Lift after stroke
        u_end, v_end = dense_points[-1]
        x, y, z = _map_to_3d(u_end, v_end, paper)
        trajectory_3d.append((x, y, z + paper.lift_height, True))

    # Solve IK
    q_traj = []
    success_flags = []
    success_count = 0
    fail_count = 0
    total_points = len(trajectory_3d)

    # Orientation constraints
    # For drawing on paper, position accuracy is most important
    # Both grip styles use position-only IK for maximum robustness
    target_orientation = None
    orientation_weight = 0.0

    for i, (x, y, z, is_lift) in enumerate(trajectory_3d):
        q_sol, success = _solve_ik_point(
            np.array([x, y, z]),
            q_current,
            tool_offset,
            arm_joint_indices,
            config.ik,
            target_orientation=target_orientation,
            orientation_weight=orientation_weight,
        )

        if success:
            q_current = q_sol
            success_count += 1
        else:
            fail_count += 1

        q_traj.append(q_current.copy())
        success_flags.append(success)

        if progress_callback:
            progress_callback(i + 1, total_points, success)

    if success_count == 0:
        raise RuntimeError("No successful IK solutions found. Check paper position and robot reach.")

    # Interpolate failed points
    if fail_count > 0:
        q_array = _interpolate_failed_points(q_traj, success_flags)
    else:
        q_array = np.array(q_traj)

    # Filter trajectory to only include the drawing arm + hand joints and
    # deduplicate coupled finger joints. Real-robot fingers have a single
    # motor driving both proximal and distal URDF joints via a mechanical
    # linkage; the Webots digital twin mirrors this coupling in
    # FINGER_PROXIMAL_MOTORS. IK never actuates finger joints (they stay at
    # the pose set by _set_initial_arm_pose), so the proximal and distal
    # columns are guaranteed to be identical — we emit only one column per
    # unique motor name.
    if arm == "left":
        candidate_indices = list(range(2, 19))   # left arm (2-7) + hand (8-18)
    else:
        candidate_indices = list(range(19, 36))  # right arm (19-24) + hand (25-35)

    drawing_joint_indices: List[int] = []
    joint_names: List[str] = []
    seen_motors = set()
    for urdf_idx in candidate_indices:
        motor_name = URDF_TO_MOTOR_NAME.get(urdf_idx, f"joint_{urdf_idx}")
        if motor_name in seen_motors:
            continue
        seen_motors.add(motor_name)
        drawing_joint_indices.append(urdf_idx)
        joint_names.append(motor_name)

    q_array = q_array[:, drawing_joint_indices]

    # Create trajectory
    trajectory = Trajectory(
        joint_names=joint_names,
        waypoints=q_array,
        metadata={
            "source": "pib3",
            "robot_model": "pib",
            "tcp_link": tcp_link,
            "arm": arm,
            "grip_style": grip_style,
            "total_points": len(q_array),
            "success_count": success_count,
            "fail_count": fail_count,
            "success_rate": success_count / total_points if total_points > 0 else 0,
            "paper_config": {
                "start_x": paper.start_x,
                "size": paper.size,
                "center_y": paper.center_y,
                "height_z": paper.height_z,
                "drawing_scale": paper.drawing_scale,
            },
        },
    )

    return trajectory

Usage¶

import pib3

# Step-by-step approach
sketch = pib3.image_to_sketch("drawing.png")
trajectory = pib3.sketch_to_trajectory(sketch)

# With progress callback
def on_progress(current, total, success):
    print(f"Point {current}/{total}: {'OK' if success else 'FAIL'}")

trajectory = pib3.sketch_to_trajectory(
    sketch,
    progress_callback=on_progress
)

# With custom config
from pib3 import TrajectoryConfig
config = TrajectoryConfig(...)
trajectory = pib3.sketch_to_trajectory(sketch, config)

Trajectory Class¶

Trajectory `dataclass` ¶

Trajectory(
    joint_names,
    waypoints,
    metadata=dict(),
    coordinate_frame="webots",
)

Robot joint trajectory for executing a drawing.

Stores joint positions in canonical Webots motor radians.

Attributes:

Name	Type	Description
`joint_names`	`List[str]`	Names of joints in order (36 for PIB).
`waypoints`	`ndarray`	Array of shape (N, num_joints) with joint positions in radians.
`metadata`	`dict`	Additional info (paper position, IK stats, etc.)
`coordinate_frame`	`str`	Frame the waypoints are stored in. Canonical is "webots" (Webots motor radians); backends remap from this frame at playback time (see RobotBackend._trajectory_to_backend_radians).

len ¶

__len__()

Return number of waypoints.

Source code in pib3/trajectory.py

def __len__(self) -> int:
    """Return number of waypoints."""
    return len(self.waypoints)

to_json ¶

to_json(path)

Save trajectory to JSON file with unit metadata.

Source code in pib3/trajectory.py

def to_json(self, path: Union[str, Path]) -> None:
    """Save trajectory to JSON file with unit metadata."""
    data = {
        "format_version": self.FORMAT_VERSION,
        "unit": self.UNIT,
        "coordinate_frame": self.coordinate_frame,
        "joint_names": self.joint_names,
        "waypoints": self.waypoints.tolist(),
        "metadata": {
            **self.metadata,
            "created_at": datetime.now(timezone.utc).isoformat(),
            "offsets": {
                "description": "Canonical format is absolute radians. "
                "Per-joint offsets from Webots starting position are "
                "applied by the backend at playback time.",
            },
        },
    }
    with open(path, 'w') as f:
        json.dump(data, f, indent=2)

from_json `classmethod` ¶

from_json(path)

Load trajectory from JSON file.

Validates the file shape up front so a slightly malformed file fails loudly (with a pointer to what's wrong) instead of silently degrading downstream — e.g. a missing arm metadata field misrouting joints in sequential-trajectory code.

Source code in pib3/trajectory.py

@classmethod
def from_json(cls, path: Union[str, Path]) -> "Trajectory":
    """Load trajectory from JSON file.

    Validates the file shape up front so a slightly malformed file fails
    loudly (with a pointer to what's wrong) instead of silently degrading
    downstream — e.g. a missing ``arm`` metadata field misrouting joints
    in sequential-trajectory code.
    """
    with open(path) as f:
        data = json.load(f)

    if not isinstance(data, dict):
        raise ValueError(
            f"Trajectory file {path} must contain a JSON object at the top level, "
            f"got {type(data).__name__}."
        )

    # Version guard — unknown future versions may carry incompatible fields.
    version = data.get("format_version")
    if version is not None and str(version) != cls.FORMAT_VERSION:
        logger.warning(
            f"Trajectory {path} was written with format_version={version!r}; "
            f"this package expects {cls.FORMAT_VERSION!r}. Proceeding, but "
            f"field semantics may not match."
        )

    unit = data.get("unit")
    if unit is not None and unit != cls.UNIT:
        raise ValueError(
            f"Unsupported trajectory unit {unit!r} (expected {cls.UNIT!r})."
        )

    # Accept legacy "points"/"link_names" aliases but require one to exist.
    joint_names = data.get("joint_names", data.get("link_names"))
    if joint_names is None:
        raise ValueError(
            f"Trajectory file {path} is missing 'joint_names' (or legacy 'link_names')."
        )
    if not isinstance(joint_names, list) or not all(isinstance(n, str) for n in joint_names):
        raise ValueError(
            f"Trajectory file {path}: 'joint_names' must be a list of strings."
        )

    raw_waypoints = data.get("waypoints", data.get("points"))
    if raw_waypoints is None:
        raise ValueError(
            f"Trajectory file {path} is missing 'waypoints' (or legacy 'points')."
        )

    try:
        waypoints = np.asarray(raw_waypoints, dtype=np.float64)
    except (TypeError, ValueError) as e:
        raise ValueError(
            f"Trajectory file {path}: 'waypoints' is not a numeric 2-D array ({e})."
        ) from e

    if waypoints.size == 0:
        waypoints = waypoints.reshape(0, len(joint_names))
    elif waypoints.ndim != 2:
        raise ValueError(
            f"Trajectory file {path}: 'waypoints' must be 2-D "
            f"(N waypoints × M joints); got shape {waypoints.shape}."
        )
    elif waypoints.shape[1] != len(joint_names):
        raise ValueError(
            f"Trajectory file {path}: waypoint width {waypoints.shape[1]} "
            f"does not match joint_names length {len(joint_names)}."
        )

    metadata = data.get("metadata", {})
    if not isinstance(metadata, dict):
        raise ValueError(
            f"Trajectory file {path}: 'metadata' must be an object, "
            f"got {type(metadata).__name__}."
        )

    coordinate_frame = data.get("coordinate_frame", cls.COORDINATE_FRAME)

    return cls(
        joint_names=joint_names,
        waypoints=waypoints,
        metadata=metadata,
        coordinate_frame=coordinate_frame,
    )

to_webots_format ¶

to_webots_format()

Convert waypoints to Webots motor positions (identity).

Waypoints are already in absolute radians. Per-joint offsets from the Webots starting position are applied by the backend, not here.

Source code in pib3/trajectory.py

def to_webots_format(self) -> np.ndarray:
    """Convert waypoints to Webots motor positions (identity).

    Waypoints are already in absolute radians.  Per-joint offsets from
    the Webots starting position are applied by the backend, not here.
    """
    return self.waypoints

to_robot_format ¶

to_robot_format()

Convert waypoints to real robot format (centidegrees).

Raw unit conversion only (radians → centidegrees). It does not apply the per-backend joint-convention remap that finger joints need (Webots vs real-robot finger sign/range differ). For correct playback on the real robot use RealRobotBackend.run_trajectory(traj), which remaps via _trajectory_to_backend_radians before sending.

Source code in pib3/trajectory.py

def to_robot_format(self) -> np.ndarray:
    """Convert waypoints to real robot format (centidegrees).

    Raw unit conversion only (radians → centidegrees). It does **not**
    apply the per-backend joint-convention remap that finger joints need
    (Webots vs real-robot finger sign/range differ). For correct playback
    on the real robot use ``RealRobotBackend.run_trajectory(traj)``, which
    remaps via ``_trajectory_to_backend_radians`` before sending.
    """
    return np.round(np.degrees(self.waypoints) * 100).astype(int)

Creating Trajectories¶

import numpy as np
from pib3 import Trajectory

# From arrays
joint_names = ["joint_0", "joint_1", "joint_2"]
waypoints = np.array([
    [0.0, 0.0, 0.0],
    [0.1, 0.2, 0.3],
    [0.2, 0.4, 0.6],
])

trajectory = Trajectory(
    joint_names=joint_names,
    waypoints=waypoints,
    metadata={"source": "custom"}
)

Saving and Loading¶

from pib3 import Trajectory

# Save to JSON
trajectory.to_json("my_trajectory.json")

# Load from JSON
loaded = Trajectory.from_json("my_trajectory.json")

# Access data
print(f"Waypoints: {len(loaded)}")
print(f"Joints: {loaded.joint_names}")
print(f"Metadata: {loaded.metadata}")

Format Conversion¶

# Get waypoints in Webots format (no offset, canonical format)
webots_waypoints = trajectory.to_webots_format()



# Get waypoints in robot format (centidegrees)
robot_waypoints = trajectory.to_robot_format()

to_robot_format() is a raw unit conversion

It only converts radians → centidegrees; it does not apply the per-backend finger-convention remap. For correct playback on the real robot use RealRobotBackend.run_trajectory(traj), which remaps finger joints before sending.

JSON Format¶

{
  "format_version": "1.0",
  "unit": "radians",
  "coordinate_frame": "webots",
  "joint_names": ["turn_head_motor", "tilt_forward_motor", ...],
  "waypoints": [
    [0.1, 0.2, 0.3, ...],
    [0.15, 0.25, 0.35, ...]
  ],
  "metadata": {
    "source": "pib3",
    "robot_model": "pib",
    "success_rate": 0.95,
    "created_at": "2024-01-01T12:00:00Z"
  }
}

Schema Validation¶

Trajectory.from_json() validates the file and raises a clear error instead of silently accepting malformed data:

Check	Failure mode
Top level is a `dict`	`ValueError`
`format_version` matches supported	warning logged
`unit` equals the expected value	`ValueError` on mismatch
`joint_names` is a `list[str]`	`ValueError`
`waypoints` is a 2D numeric array with columns matching `len(joint_names)`	`ValueError`
`metadata` is a `dict`	`ValueError`

This keeps downstream execution from failing deep inside IK or motor code with opaque IndexError/KeyError tracebacks.

IK Solver Details¶

The inverse kinematics solver uses:

Algorithm: Levenberg-Marquardt via roboticstoolbox ikine_LM, on the expert-calibrated DH model (with base + tool). Targets are solved in the torso frame, in millimetres; the returned joint degrees map directly to motor commands.
Robustness: Each point is attempted in layers (warm-start → ignore joint limits → random restarts), first success wins. This recovers many poses that a single limited solve would report as failures.
Convergence: Position error below tolerance.
Fallback: Linear interpolation for points that still fail.

Solver Parameters¶

Parameter	Effect
`max_iterations`	Iterations per search attempt (`ikine_LM` ilimit).
`slimit`	Random-restart attempts per point. Higher = more poses solved, slower on hard targets.
`tolerance`	Position tolerance (m). Smaller = more precise, harder to converge.

Validating the IK¶

Before driving real motors, sanity-check the IK offline (no hardware) with verify_ik. It solves a torso-frame target, runs forward kinematics on the result, and returns the round-trip position error:

from pib3 import verify_ik

q_deg, error_mm, ok = verify_ik("left", [150, 0, 350])  # torso-frame mm
print(ok, round(error_mm, 3), q_deg)  # expect ok=True, sub-mm error

Trajectory Generation¶

Overview¶

Main Function¶

generate_trajectory¶

generate_trajectory ¶

With custom config¶

Sequential trajectories (robot draws multiple images)¶

Usage¶

sketch_to_trajectory¶

sketch_to_trajectory ¶

Sequential trajectories¶

Usage¶

Trajectory Class¶

Trajectory dataclass ¶

__len__ ¶

to_json ¶

from_json classmethod ¶

to_webots_format ¶

to_robot_format ¶

Creating Trajectories¶

Saving and Loading¶

Format Conversion¶

JSON Format¶

Schema Validation¶

IK Solver Details¶

Solver Parameters¶

Validating the IK¶

Trajectory `dataclass` ¶

len ¶

from_json `classmethod` ¶