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 nav_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 nav_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 nav_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 nav_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 nav_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 nav_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 nav_planning/types/geometry.py

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

    return matrix_to_rpy(self.rotation)

Planning

PlanningStatus

Bases: Enum

Status of a motion planning attempt.

PlannerConfig(planner_name: str = 'rrtc', time_limit: float = 10.0, point_radius: float = 0.0, 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 = True, 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:~nav_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:nav_planning.locobot / :mod:nav_planning.ridgeback; 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:~nav_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

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