Types

Geometry

SE3Pose(position: np.ndarray, rotation: np.ndarray) dataclass

Represents a 6D pose in SE(3) - position and orientation. Orientation is stored as a 3x3 rotation matrix.

from_matrix(matrix: np.ndarray) -> SE3Pose classmethod

Create SE3Pose from 4x4 homogeneous transformation matrix.

Source code in fetch_planning/types/geometry.py

@classmethod
def from_matrix(cls, matrix: np.ndarray) -> "SE3Pose":
    """Create SE3Pose from 4x4 homogeneous transformation matrix."""
    matrix = np.asarray(matrix)
    if matrix.shape != (4, 4):
        raise ValueError(f"Matrix must be shape (4, 4), got {matrix.shape}")
    return cls(position=matrix[:3, 3], rotation=matrix[:3, :3])

from_position_quat(position: np.ndarray, quaternion: np.ndarray) -> SE3Pose classmethod

Create SE3Pose from position and quaternion (w, x, y, z).

Source code in fetch_planning/types/geometry.py

@classmethod
def from_position_quat(
    cls, position: np.ndarray, quaternion: np.ndarray
) -> "SE3Pose":
    """Create SE3Pose from position and quaternion (w, x, y, z)."""
    position = np.asarray(position)
    quaternion = np.asarray(quaternion)
    rotation = quaternion_to_matrix(quaternion)
    return cls(position=position, rotation=rotation)

from_position_rpy(position: np.ndarray, roll: float, pitch: float, yaw: float) -> SE3Pose classmethod

Create SE3Pose from position and roll-pitch-yaw angles.

Source code in fetch_planning/types/geometry.py

@classmethod
def from_position_rpy(
    cls, position: np.ndarray, roll: float, pitch: float, yaw: float
) -> "SE3Pose":
    """Create SE3Pose from position and roll-pitch-yaw angles."""
    from fetch_planning.utils.rot_utils import rpy_to_matrix

    position = np.asarray(position)
    rotation = rpy_to_matrix(roll, pitch, yaw)
    return cls(position=position, rotation=rotation)

to_matrix() -> np.ndarray

Convert to 4x4 homogeneous transformation matrix.

Source code in fetch_planning/types/geometry.py

def to_matrix(self) -> np.ndarray:
    """Convert to 4x4 homogeneous transformation matrix."""
    matrix = np.eye(4)
    matrix[:3, :3] = self.rotation
    matrix[:3, 3] = self.position
    return matrix

to_quaternion() -> np.ndarray

Convert rotation to quaternion (w, x, y, z).

Source code in fetch_planning/types/geometry.py

def to_quaternion(self) -> np.ndarray:
    """Convert rotation to quaternion (w, x, y, z)."""
    return matrix_to_quaternion(self.rotation)

to_rpy() -> tuple[float, float, float]

Convert rotation to roll-pitch-yaw angles.

Source code in fetch_planning/types/geometry.py

def to_rpy(self) -> tuple[float, float, float]:
    """Convert rotation to roll-pitch-yaw angles."""
    from fetch_planning.utils.rot_utils import matrix_to_rpy

    return matrix_to_rpy(self.rotation)

Inverse Kinematics

IKStatus

Bases: Enum

Status of IK solution attempt.

SolveType

Bases: Enum

TRAC-IK solve type.

SPEED: Return first valid solution (fastest). DISTANCE: Minimize joint displacement from seed configuration. MANIP1: Maximize manipulability (product of Jacobian singular values). MANIP2: Maximize isotropy (min/max Jacobian singular value ratio).

IKConfig(timeout: float = 0.2, epsilon: float = 1e-05, solve_type: SolveType = SolveType.SPEED, max_attempts: int = 10, position_tolerance: float = 0.0001, orientation_tolerance: float = 0.0001) dataclass

Configuration parameters for TRAC-IK solver.

IKResult(status: IKStatus, joint_positions: np.ndarray | None, final_error: float, iterations: int, position_error: float, orientation_error: float) dataclass

Result of an IK solution attempt.

Pink Constrained IK

PinkIKConfig(dt: float = 0.01, max_iterations: int = 300, convergence_thresh: float = 0.001, orientation_thresh: float = 0.01, position_cost: float = 1.0, orientation_cost: float = 1.0, lm_damping: float = 0.001, posture_cost: float = 0.001, com_cost: float = 0.0, camera_frame: str | None = None, camera_cost: float = 0.0, coupled_joints: list[CoupledJoint] = list(), self_collision: bool = False, collision_pairs: int = 3, collision_gain: float = 1.0, collision_d_min: float = 0.02, solver: str = 'osqp') dataclass

Configuration for the Pink constrained IK solver.

Attributes:

Name	Type	Description
dt	float	Integration timestep for differential IK (seconds).
max_iterations	int	Maximum solver iterations before returning.
convergence_thresh	float	Position convergence threshold (meters).
orientation_thresh	float	Orientation convergence threshold (radians).
position_cost	float	End-effector position tracking weight.
orientation_cost	float	End-effector orientation tracking weight.
lm_damping	float	Levenberg-Marquardt damping for singularity avoidance.
posture_cost	float	Posture regularization weight (toward seed).
com_cost	float	Center-of-mass stability weight (0 disables). Penalizes CoM drift from the seed configuration to prevent tipping.
camera_frame	str | None	Frame name to stabilize (e.g. "Link_Camera_Head_Forehead"). When set with camera_cost > 0, adds a FrameTask that anchors this frame to its pose at the seed configuration, keeping the camera observation stable while the arm moves.
camera_cost	float	Weight for the camera stability task (0 disables).
coupled_joints	list[CoupledJoint]	Linear joint coupling constraints enforced as hard constraints. Each entry locks slave = multiplier * master + offset on every iteration, replacing inaccurate CoM modelling with a kinematic heuristic.
self_collision	bool	Enable collision barrier (self-collision + obstacles).
collision_pairs	int	Number of closest collision pairs to evaluate per step.
collision_gain	float	Barrier gain — higher means harder repulsion.
collision_d_min	float	Minimum allowed distance between collision pairs (m).
solver	str	QP solver backend for qpsolvers (e.g. "osqp", "quadprog").

CoupledJoint(master: str, slave: str, multiplier: float = 2.0, offset: float = 0.0) dataclass

Linear coupling constraint: slave = multiplier * master + offset.

Enforced as a hard constraint on both the velocity and position level during the Pink IK iterative solve.

ConstrainedIKResult(status: IKStatus, joint_positions: np.ndarray | None, final_error: float, iterations: int, position_error: float, orientation_error: float, trajectory: np.ndarray | None = None) dataclass

Result of a constrained IK solve, with trajectory from seed to solution.

Shares fields with :class:IKResult; adds trajectory.

Planning

PlanningStatus

Bases: Enum

Status of a motion planning attempt.

PlannerConfig(planner_name: str = 'rrtc', time_limit: float = 10.0, point_radius: float = 0.01, simplify: bool = True, interpolate: bool = True, resolution: float = 64.0, joint_limits: dict[str, tuple[float, float]] | None = None) dataclass

Configuration parameters for the motion planner.

PlanningResult(status: PlanningStatus, path: np.ndarray | None, planning_time_ns: int, path_cost: float) dataclass

Result of a motion planning attempt.

KinodynamicConfig(time_limit: float = 1.0, rho: float = 1.0, velocity_scale: float = 1.0, acceleration_scale: float = 1.0, allow_reverse: bool | None = None, limit_aware_duration: bool = True, max_extension_time: float = 3.0, velocity_sample_scale: float = 0.5, velocity_metric_weight: float = 0.3, rest_sample_probability: float = 0.3, spin_probability: float = 0.3, heading_tolerance: float = 0.02, simplify: bool = True, simplify_iterations: int = 5, simplify_time_limit: float = 0.05, max_iterations: int = 200000, seed: int = 0) dataclass

Parameters for :meth:~fetch_planning.planning.MotionPlanner.plan_kinodynamic.

The planner is FLASK (flatness-based kinodynamic RRT-Connect). Joint and base limits default to the robot values in :mod:fetch_planning.fetch; velocity_scale / acceleration_scale shrink or grow all of them at once.

KinodynamicResult(status: PlanningStatus, trajectory: KinodynamicTrajectory | None, planning_time_ns: int, iterations: int, cost: float, simplify_time_ns: int = 0, start_tree_size: int = 0, goal_tree_size: int = 0, edges_checked: int = 0) dataclass

Result of a kinodynamic planning attempt.

On success trajectory is a :class:~fetch_planning.trajectory.KinodynamicTrajectory — already time-parameterised, so no separate time-parameterisation step is needed.

Robot

RobotConfig(urdf_path: str, joint_names: list[str], camera: CameraConfig) dataclass

ChainConfig(base_link: str, ee_link: str, num_joints: int, urdf_path: str) dataclass

Configuration for a kinematic chain used by TRAC-IK.

with_urdf_path(urdf_path: str) -> ChainConfig

Return a copy with a different URDF path.

Source code in fetch_planning/types/robot.py

def with_urdf_path(self, urdf_path: str) -> "ChainConfig":
    """Return a copy with a different URDF path."""
    return replace(self, urdf_path=urdf_path)

CameraConfig(link_name: str, width: int, height: int, fov: float, near: float, far: float) dataclass