init

Trajectory initialization utilities.

This module provides functions for generating initial trajectory guesses using various interpolation methods. Keyframe-based APIs allow specifying values at specific nodes with automatic interpolation for intermediate nodes.

Example

Generate initial guess for a quadrotor passing through gates::

import openscvx as ox

n = 50
position.guess = ox.init.linspace(
    keyframes=[start_pos, gate1, gate2, end_pos],
    nodes=[0, 15, 35, n-1],
)

attitude.guess = ox.init.nlerp(
    keyframes=[q_start, q_gate1, q_gate2, q_end],
    nodes=[0, 15, 35, n-1],
)

ik_interpolation(keyframes: Sequence[Pose], nodes: Sequence[int], screw_axes: np.ndarray, T_home: np.ndarray, *, angles_init: np.ndarray = None, angles_min: np.ndarray = None, angles_max: np.ndarray = None, sequential: bool = False, damping: float = 0.001, max_iter: int = 200, tol: float = 1e-06) -> np.ndarray

Generate joint-angle trajectory guess via task-space interpolation and IK.

Interpolates end-effector poses between keyframes (linspace for position, slerp for orientation) and solves inverse kinematics at each trajectory node.

Parameters:

Name	Type	Description	Default
keyframes	Sequence[Pose]	Sequence of (position, quaternion_wxyz) tuples. Each position is array-like with shape (3,) and each quaternion is array-like with shape (4,) in [w, x, y, z] order.	required
nodes	Sequence[int]	Sequence of node indices where keyframes occur. Must be sorted in ascending order and have the same length as keyframes. The last node determines the output size (N = nodes[-1] + 1).	required
screw_axes	ndarray	(n_joints, 6) array of screw axes for Product of Exponentials.	required
T_home	ndarray	(4, 4) home configuration transform.	required
angles_init	ndarray	(n_joints,) initial joint angle guess. In parallel mode this seeds every node; in sequential mode it seeds only the first node. Defaults to zeros.	None
angles_min	ndarray	(n_joints,) optional lower joint limits.	None
angles_max	ndarray	(n_joints,) optional upper joint limits.	None
sequential	bool	If True, solve nodes sequentially (each node seeded by the previous solution). If False (default), solve all nodes in parallel from the same angles_init seed.	False
damping	float	Damping factor for least-squares IK.	0.001
max_iter	int	Maximum IK iterations per node.	200
tol	float	IK convergence tolerance.	1e-06

Returns:

Type	Description
ndarray	np.ndarray of shape (N, n_joints) containing joint angles at each node.

Example

Initialize a 7-DOF arm trajectory reaching for a target::

import openscvx as ox

angle.guess = ox.init.ik_interpolation(
    keyframes=[
        ([0.7, 0, 0.34], [1, 0, 0, 0]),  # home pose
        ([0.3, 0.3, 0.5], [1, 0, 0, 0]),  # target pose
    ],
    nodes=[0, 49],
    screw_axes=screw_axes,
    T_home=T_home,
)

Source code in openscvx/init/inverse_kinematics.py

def ik_interpolation(
    keyframes: Sequence[Pose],
    nodes: Sequence[int],
    screw_axes: np.ndarray,
    T_home: np.ndarray,
    *,
    angles_init: np.ndarray = None,
    angles_min: np.ndarray = None,
    angles_max: np.ndarray = None,
    sequential: bool = False,
    damping: float = 1e-3,
    max_iter: int = 200,
    tol: float = 1e-6,
) -> np.ndarray:
    """Generate joint-angle trajectory guess via task-space interpolation and IK.

    Interpolates end-effector poses between keyframes (linspace for position,
    slerp for orientation) and solves inverse kinematics at each trajectory
    node.

    Args:
        keyframes: Sequence of (position, quaternion_wxyz) tuples. Each position
            is array-like with shape (3,) and each quaternion is array-like with
            shape (4,) in [w, x, y, z] order.
        nodes: Sequence of node indices where keyframes occur. Must be sorted in
            ascending order and have the same length as keyframes. The last node
            determines the output size (N = nodes[-1] + 1).
        screw_axes: (n_joints, 6) array of screw axes for Product of Exponentials.
        T_home: (4, 4) home configuration transform.
        angles_init: (n_joints,) initial joint angle guess. In parallel mode
            this seeds every node; in sequential mode it seeds only the first
            node. Defaults to zeros.
        angles_min: (n_joints,) optional lower joint limits.
        angles_max: (n_joints,) optional upper joint limits.
        sequential: If True, solve nodes sequentially (each node seeded by the
            previous solution). If False (default), solve all nodes in parallel
            from the same ``angles_init`` seed.
        damping: Damping factor for least-squares IK.
        max_iter: Maximum IK iterations per node.
        tol: IK convergence tolerance.

    Returns:
        np.ndarray of shape (N, n_joints) containing joint angles at each node.

    Example:
        Initialize a 7-DOF arm trajectory reaching for a target::

            import openscvx as ox

            angle.guess = ox.init.ik_interpolation(
                keyframes=[
                    ([0.7, 0, 0.34], [1, 0, 0, 0]),  # home pose
                    ([0.3, 0.3, 0.5], [1, 0, 0, 0]),  # target pose
                ],
                nodes=[0, 49],
                screw_axes=screw_axes,
                T_home=T_home,
            )
    """
    positions = [np.asarray(kf[0], dtype=np.float64) for kf in keyframes]
    quaternions = [np.asarray(kf[1], dtype=np.float64) for kf in keyframes]

    for i, (p, quat) in enumerate(zip(positions, quaternions)):
        if p.shape != (3,):
            raise ValueError(f"Keyframe {i} position has shape {p.shape}, expected (3,)")
        if quat.shape != (4,):
            raise ValueError(f"Keyframe {i} quaternion has shape {quat.shape}, expected (4,)")

    # Interpolate task-space trajectory
    p_traj = jnp.array(linspace(keyframes=positions, nodes=nodes))  # (N, 3)
    quat_traj = jnp.array(slerp(keyframes=quaternions, nodes=nodes))  # (N, 4)
    R_traj = jax.vmap(_quat_wxyz_to_rotmat)(quat_traj)  # (N, 3, 3)

    n_joints = screw_axes.shape[0]
    screw_axes_j = jnp.array(screw_axes)
    T_home_j = jnp.array(T_home)
    angles_lo = jnp.array(angles_min) if angles_min is not None else jnp.full(n_joints, -jnp.inf)
    angles_hi = jnp.array(angles_max) if angles_max is not None else jnp.full(n_joints, jnp.inf)

    angles0 = jnp.array(angles_init) if angles_init is not None else jnp.zeros(n_joints)

    if sequential:
        # Solve nodes in order, seeding each from the previous solution
        def scan_step(prev_angles, node_data):
            p, R = node_data
            sol = _ik_loop_pose(
                screw_axes_j,
                T_home_j,
                p,
                R,
                prev_angles,
                angles_lo,
                angles_hi,
                damping,
                tol,
                max_iter,
            )
            return sol, sol

        _, result = jax.lax.scan(scan_step, angles0, (p_traj, R_traj))
    else:
        # Solve all nodes in parallel from the same seed
        N = nodes[-1] + 1
        angles0_all = jnp.broadcast_to(angles0, (N, n_joints))
        result = jax.vmap(
            lambda p, R, a0: _ik_loop_pose(
                screw_axes_j, T_home_j, p, R, a0, angles_lo, angles_hi, damping, tol, max_iter
            )
        )(p_traj, R_traj, angles0_all)

    return np.array(result)

linspace(keyframes: Sequence[Keyframe], nodes: Sequence[int]) -> np.ndarray

Generate trajectory guess via piecewise linear interpolation between keyframes.

Keyframes specify values at particular nodes, and linear interpolation fills in the values for all nodes in between. This generalizes np.linspace to support multiple waypoints.

Parameters:

Name	Type	Description	Default
keyframes	Sequence[Keyframe]	Sequence of values at keyframe nodes. Each keyframe should be array-like with the same shape, or a scalar.	required
nodes	Sequence[int]	Sequence of node indices where keyframes occur. Must be sorted in ascending order and have the same length as keyframes. The last node determines the output size (N = nodes[-1] + 1).	required

Returns:

Type	Description
ndarray	np.ndarray of shape (N, *keyframe_shape) containing interpolated values
ndarray	for all nodes, where N = nodes[-1] + 1.

Raises:

Type	Description
ValueError	If keyframes and nodes have different lengths, or nodes are not sorted.

Note

The first node should typically be 0 and the last node should be N-1 (where N is your trajectory length). Nodes before the first keyframe or after the last will be left as zeros.

Example

Interpolate position through three waypoints::

import openscvx as ox

position_guess = ox.init.linspace(
    keyframes=[[0, 0, 0], [5, 10, 5], [10, 0, 0]],
    nodes=[0, 25, 49],
)  # Returns shape (50, 3)
position.guess = position_guess

Simple start-to-end interpolation (equivalent to np.linspace)::

position_guess = ox.init.linspace(
    keyframes=[[0, 0, 0], [10, 0, 0]],
    nodes=[0, 49],
)  # Returns shape (50, 3)

Source code in openscvx/init/interpolation.py

def linspace(
    keyframes: Sequence[Keyframe],
    nodes: Sequence[int],
) -> np.ndarray:
    """Generate trajectory guess via piecewise linear interpolation between keyframes.

    Keyframes specify values at particular nodes, and linear interpolation fills
    in the values for all nodes in between. This generalizes np.linspace to support
    multiple waypoints.

    Args:
        keyframes: Sequence of values at keyframe nodes. Each keyframe should be
            array-like with the same shape, or a scalar.
        nodes: Sequence of node indices where keyframes occur. Must be sorted in
            ascending order and have the same length as keyframes. The last node
            determines the output size (N = nodes[-1] + 1).

    Returns:
        np.ndarray of shape (N, *keyframe_shape) containing interpolated values
        for all nodes, where N = nodes[-1] + 1.

    Raises:
        ValueError: If keyframes and nodes have different lengths, or nodes are
            not sorted.

    Note:
        The first node should typically be 0 and the last node should be N-1
        (where N is your trajectory length). Nodes before the first keyframe
        or after the last will be left as zeros.

    Example:
        Interpolate position through three waypoints::

            import openscvx as ox

            position_guess = ox.init.linspace(
                keyframes=[[0, 0, 0], [5, 10, 5], [10, 0, 0]],
                nodes=[0, 25, 49],
            )  # Returns shape (50, 3)
            position.guess = position_guess

        Simple start-to-end interpolation (equivalent to np.linspace)::

            position_guess = ox.init.linspace(
                keyframes=[[0, 0, 0], [10, 0, 0]],
                nodes=[0, 49],
            )  # Returns shape (50, 3)
    """
    _validate_keyframe_inputs(keyframes, nodes)
    N = nodes[-1] + 1

    # Convert keyframes to numpy arrays
    keyframes_arr = [np.atleast_1d(np.asarray(kf, dtype=np.float64)) for kf in keyframes]
    keyframe_shape = keyframes_arr[0].shape

    # Validate all keyframes have the same shape
    for i, kf in enumerate(keyframes_arr):
        if kf.shape != keyframe_shape:
            raise ValueError(f"Keyframe {i} has shape {kf.shape}, expected {keyframe_shape}")

    # Initialize output array
    result = np.zeros((N, *keyframe_shape), dtype=np.float64)

    # Interpolate between consecutive keyframe pairs
    for i in range(len(nodes) - 1):
        start_node = nodes[i]
        end_node = nodes[i + 1]
        start_val = keyframes_arr[i]
        end_val = keyframes_arr[i + 1]

        # Number of nodes in this segment (inclusive of start, exclusive of end)
        n_segment = end_node - start_node

        for j in range(n_segment):
            t = j / n_segment
            result[start_node + j] = (1 - t) * start_val + t * end_val

    # Set the final keyframe value
    result[nodes[-1]] = keyframes_arr[-1]

    return result

nlerp(keyframes: Sequence[Quaternion], nodes: Sequence[int]) -> np.ndarray

Generate quaternion trajectory guess via normalized linear interpolation.

NLERP performs linear interpolation between quaternions followed by normalization. This is faster than SLERP but produces non-constant angular velocity. For initial guesses this is typically sufficient.

Quaternion convention: [w, x, y, z] (scalar-first).

Parameters:

Name	Type	Description	Default
keyframes	Sequence[Quaternion]	Sequence of unit quaternions at keyframe nodes. Each quaternion should be array-like with shape (4,) in [w, x, y, z] order.	required
nodes	Sequence[int]	Sequence of node indices where keyframes occur. Must be sorted in ascending order and have the same length as keyframes.	required

Returns:

Type	Description
ndarray	np.ndarray of shape (N, 4) containing normalized interpolated quaternions
ndarray	for all nodes, where N = nodes[-1] + 1.

Raises:

Type	Description
ValueError	If keyframes and nodes have different lengths, nodes are not sorted, or quaternions don't have shape (4,).

Note

The first node should typically be 0 and the last node should be N-1. Nodes outside the keyframe range will be left as zeros.

Example

Interpolate attitude from identity to 180-degree rotation about z-axis::

import openscvx as ox

attitude_guess = ox.init.nlerp(
    keyframes=[
        [1, 0, 0, 0],  # Identity quaternion
        [0, 0, 0, 1],  # 180 deg about z
    ],
    nodes=[0, 49],
)  # Returns shape (50, 4)
attitude.guess = attitude_guess

Interpolate through intermediate attitudes::

attitude_guess = ox.init.nlerp(
    keyframes=[
        [1, 0, 0, 0],           # Identity
        [0.707, 0, 0, 0.707],   # 90 deg about z
        [0, 0, 0, 1],           # 180 deg about z
    ],
    nodes=[0, 25, 49],
)

Source code in openscvx/init/interpolation.py

def nlerp(
    keyframes: Sequence[Quaternion],
    nodes: Sequence[int],
) -> np.ndarray:
    """Generate quaternion trajectory guess via normalized linear interpolation.

    NLERP performs linear interpolation between quaternions followed by
    normalization. This is faster than SLERP but produces non-constant angular
    velocity. For initial guesses this is typically sufficient.

    Quaternion convention: [w, x, y, z] (scalar-first).

    Args:
        keyframes: Sequence of unit quaternions at keyframe nodes. Each quaternion
            should be array-like with shape (4,) in [w, x, y, z] order.
        nodes: Sequence of node indices where keyframes occur. Must be sorted in
            ascending order and have the same length as keyframes.

    Returns:
        np.ndarray of shape (N, 4) containing normalized interpolated quaternions
        for all nodes, where N = nodes[-1] + 1.

    Raises:
        ValueError: If keyframes and nodes have different lengths, nodes are not
            sorted, or quaternions don't have shape (4,).

    Note:
        The first node should typically be 0 and the last node should be N-1.
        Nodes outside the keyframe range will be left as zeros.

    Example:
        Interpolate attitude from identity to 180-degree rotation about z-axis::

            import openscvx as ox

            attitude_guess = ox.init.nlerp(
                keyframes=[
                    [1, 0, 0, 0],  # Identity quaternion
                    [0, 0, 0, 1],  # 180 deg about z
                ],
                nodes=[0, 49],
            )  # Returns shape (50, 4)
            attitude.guess = attitude_guess

        Interpolate through intermediate attitudes::

            attitude_guess = ox.init.nlerp(
                keyframes=[
                    [1, 0, 0, 0],           # Identity
                    [0.707, 0, 0, 0.707],   # 90 deg about z
                    [0, 0, 0, 1],           # 180 deg about z
                ],
                nodes=[0, 25, 49],
            )
    """
    _validate_keyframe_inputs(keyframes, nodes)
    N = nodes[-1] + 1

    # Convert keyframes to numpy arrays and validate shape
    keyframes_arr = []
    for i, kf in enumerate(keyframes):
        q = np.asarray(kf, dtype=np.float64)
        if q.shape != (4,):
            raise ValueError(f"Keyframe {i} has shape {q.shape}, expected (4,) for quaternion")
        # Normalize input quaternions
        q = q / np.linalg.norm(q)
        keyframes_arr.append(q)

    # Initialize output array
    result = np.zeros((N, 4), dtype=np.float64)

    # Interpolate between consecutive keyframe pairs
    for i in range(len(nodes) - 1):
        start_node = nodes[i]
        end_node = nodes[i + 1]
        q0 = keyframes_arr[i]
        q1 = keyframes_arr[i + 1]

        # Ensure shortest path interpolation
        if np.dot(q0, q1) < 0:
            q1 = -q1

        n_segment = end_node - start_node

        for j in range(n_segment):
            t = j / n_segment
            # Linear interpolation
            q_interp = (1 - t) * q0 + t * q1
            # Normalize
            result[start_node + j] = q_interp / np.linalg.norm(q_interp)

    # Set the final keyframe value (normalized)
    result[nodes[-1]] = keyframes_arr[-1]

    return result

slerp(keyframes: Sequence[Quaternion], nodes: Sequence[int]) -> np.ndarray

Generate quaternion trajectory guess via spherical linear interpolation.

SLERP interpolates along the great arc on the unit quaternion sphere, producing constant angular velocity between keyframes. This is more accurate than NLERP but slightly more expensive to compute.

Quaternion convention: [w, x, y, z] (scalar-first).

Parameters:

Name	Type	Description	Default
keyframes	Sequence[Quaternion]	Sequence of unit quaternions at keyframe nodes. Each quaternion should be array-like with shape (4,) in [w, x, y, z] order.	required
nodes	Sequence[int]	Sequence of node indices where keyframes occur. Must be sorted in ascending order and have the same length as keyframes.	required

Returns:

Type	Description
ndarray	np.ndarray of shape (N, 4) containing interpolated quaternions for all nodes,
ndarray	where N = nodes[-1] + 1.

Raises:

Type	Description
ValueError	If keyframes and nodes have different lengths, nodes are not sorted, or quaternions don't have shape (4,).

Note

The first node should typically be 0 and the last node should be N-1. Nodes outside the keyframe range will be left as zeros.

Example

Interpolate attitude with constant angular velocity::

import openscvx as ox

attitude_guess = ox.init.slerp(
    keyframes=[
        [1, 0, 0, 0],  # Identity quaternion
        [0, 0, 0, 1],  # 180 deg about z
    ],
    nodes=[0, 49],
)  # Returns shape (50, 4)
attitude.guess = attitude_guess

Source code in openscvx/init/interpolation.py

def slerp(
    keyframes: Sequence[Quaternion],
    nodes: Sequence[int],
) -> np.ndarray:
    """Generate quaternion trajectory guess via spherical linear interpolation.

    SLERP interpolates along the great arc on the unit quaternion sphere,
    producing constant angular velocity between keyframes. This is more accurate
    than NLERP but slightly more expensive to compute.

    Quaternion convention: [w, x, y, z] (scalar-first).

    Args:
        keyframes: Sequence of unit quaternions at keyframe nodes. Each quaternion
            should be array-like with shape (4,) in [w, x, y, z] order.
        nodes: Sequence of node indices where keyframes occur. Must be sorted in
            ascending order and have the same length as keyframes.

    Returns:
        np.ndarray of shape (N, 4) containing interpolated quaternions for all nodes,
        where N = nodes[-1] + 1.

    Raises:
        ValueError: If keyframes and nodes have different lengths, nodes are not
            sorted, or quaternions don't have shape (4,).

    Note:
        The first node should typically be 0 and the last node should be N-1.
        Nodes outside the keyframe range will be left as zeros.

    Example:
        Interpolate attitude with constant angular velocity::

            import openscvx as ox

            attitude_guess = ox.init.slerp(
                keyframes=[
                    [1, 0, 0, 0],  # Identity quaternion
                    [0, 0, 0, 1],  # 180 deg about z
                ],
                nodes=[0, 49],
            )  # Returns shape (50, 4)
            attitude.guess = attitude_guess
    """
    _validate_keyframe_inputs(keyframes, nodes)
    N = nodes[-1] + 1

    # Convert keyframes to numpy arrays and validate shape
    keyframes_arr = []
    for i, kf in enumerate(keyframes):
        q = np.asarray(kf, dtype=np.float64)
        if q.shape != (4,):
            raise ValueError(f"Keyframe {i} has shape {q.shape}, expected (4,) for quaternion")
        # Normalize input quaternions
        q = q / np.linalg.norm(q)
        keyframes_arr.append(q)

    # Initialize output array
    result = np.zeros((N, 4), dtype=np.float64)

    # Interpolate between consecutive keyframe pairs
    for i in range(len(nodes) - 1):
        start_node = nodes[i]
        end_node = nodes[i + 1]
        q0 = keyframes_arr[i]
        q1 = keyframes_arr[i + 1]

        # Ensure shortest path interpolation
        dot = np.dot(q0, q1)
        if dot < 0:
            q1 = -q1
            dot = -dot

        n_segment = end_node - start_node

        # Handle near-identical quaternions (fall back to NLERP)
        if dot > 0.9995:
            for j in range(n_segment):
                t = j / n_segment
                q_interp = (1 - t) * q0 + t * q1
                result[start_node + j] = q_interp / np.linalg.norm(q_interp)
        else:
            # SLERP formula
            theta = np.arccos(dot)
            sin_theta = np.sin(theta)

            for j in range(n_segment):
                t = j / n_segment
                s0 = np.sin((1 - t) * theta) / sin_theta
                s1 = np.sin(t * theta) / sin_theta
                result[start_node + j] = s0 * q0 + s1 * q1

    # Set the final keyframe value (normalized)
    result[nodes[-1]] = keyframes_arr[-1]

    return result