reforge-core 2.0.2__tar.gz → 2.0.4__tar.gz

{reforge_core-2.0.2 → reforge_core-2.0.4}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.2
 Name: reforge-core
-Version: 2.0.2
+Version: 2.0.4
 Summary: Reforge Core SDK by Reforge Robotics.
 Author-Email: Reforge Robotics <info@reforgerobotics.com>
 License: MIT

{reforge_core-2.0.2 → reforge_core-2.0.4}/pyproject.toml

@@ -9,7 +9,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "reforge-core"
-version = "2.0.2"
+version = "2.0.4"
 description = "Reforge Core SDK by Reforge Robotics."
 readme = "README.md"
 requires-python = ">=3.11"

reforge_core-2.0.4/reforge_core/calibration/base_joint_planning.py

@@ -0,0 +1,543 @@
+"""Pure planning helpers for base-joint shaper calibration."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+from dataclasses import dataclass
+from itertools import product
+import math
+
+import numpy as np
+
+BASE_JOINT_AXIS_PARALLEL_ATOL = 1.0e-6
+CALIBRATION_SCHEMA_VERSION = 2
+DEFAULT_BASE_JOINT_ANGLE_COUNT = 4
+MAX_SAVED_SAMPLE_FREQUENCY_HZ = 500.0
+RUN_ROLE_NORMAL_FULL_AXIS = "normal_full_axis"
+RUN_ROLE_BASE_ONLY_AXIS = "base_only_axis"
+MOTION_MODE_SINE_SWEEP = "sine_sweep"
+MOTION_MODE_POINT_TO_POINT_TEST = "point_to_point_test"
+
+
+@dataclass(frozen=True)
+class BaseJointLimit:
+    """Store one base-joint lower and upper position limit.
+
+    Args:
+        lower_rad: Lower position limit [rad].
+        upper_rad: Upper position limit [rad].
+    """
+
+    lower_rad: float
+    upper_rad: float
+
+
+@dataclass(frozen=True)
+class BaseAngleGrid:
+    """Describe sampled base-joint angles and the margin rule used.
+
+    Args:
+        angles_rad: Grid positions for one base joint [rad].
+        margin_source: Rule used to avoid joint-limit endpoints.
+    """
+
+    angles_rad: tuple[float, ...]
+    margin_source: str
+
+
+@dataclass(frozen=True)
+class CalibrationRunDescriptor:
+    """Describe one planned calibration run before robot motion.
+
+    Args:
+        run_id: Stable logical run identifier.
+        run_role: Run role such as `normal_full_axis` or `base_only_axis`.
+        motion_mode: Execution mode such as `sine_sweep` or `point_to_point_test`.
+        swept_axis: Axis commanded by this run [joint index].
+        pose_index: Pose-grid index [count].
+        axis_index: Axis-loop index within the role [count].
+        base_joint_angles_rad: Active base-joint coordinates [rad].
+        v_rad: Pose elevation angle from horizontal [rad].
+        r_m: Pose radius from base [m].
+        uses_joint_tracker_mpc: Whether joint-tracker MPC compensation is planned.
+    """
+
+    run_id: str
+    run_role: str
+    motion_mode: str
+    swept_axis: int
+    pose_index: int
+    axis_index: int
+    base_joint_angles_rad: tuple[float, ...]
+    v_rad: float
+    r_m: float
+    uses_joint_tracker_mpc: bool = False
+
+    @property
+    def v_deg(self) -> float:
+        """Return the planned pose elevation angle in degrees.
+
+        Returns:
+            `float` pose elevation angle [deg].
+        """
+        return math.degrees(self.v_rad)
+
+    @property
+    def r_mm(self) -> float:
+        """Return the planned pose radius in millimeters.
+
+        Returns:
+            `float` pose radius [mm].
+        """
+        return 1000.0 * self.r_m
+
+
+def parse_base_joint_limit_specs(
+    limit_specs: Sequence[str] | None,
+) -> list[BaseJointLimit]:
+    """Parse CLI base-joint limit strings into typed lower/upper pairs.
+
+    Args:
+        limit_specs: Strings formatted as `"lower,upper"` in radians.
+    Returns:
+        `list[BaseJointLimit]` parsed limits [rad].
+    """
+    if not limit_specs:
+        return []
+
+    limits = []
+    for index, limit_spec in enumerate(limit_specs):
+        pieces = [piece.strip() for piece in limit_spec.split(",")]
+        if len(pieces) != 2:
+            raise ValueError(
+                f"Base joint limit {index} must contain exactly two comma-separated values."
+            )
+        lower_rad, upper_rad = (float(pieces[0]), float(pieces[1]))
+        if not (math.isfinite(lower_rad) and math.isfinite(upper_rad)):
+            raise ValueError(f"Base joint limit {index} values must be finite.")
+        if lower_rad >= upper_rad:
+            raise ValueError(
+                f"Base joint limit {index} lower bound must be less than upper bound."
+            )
+        limits.append(BaseJointLimit(lower_rad=lower_rad, upper_rad=upper_rad))
+    return limits
+
+
+def serialize_float_sequence(values: Sequence[float]) -> str:
+    """Serialize a float sequence as a semicolon-delimited metadata field.
+
+    Args:
+        values: Values to serialize.
+    Returns:
+        `str` semicolon-delimited float values.
+    """
+    return ";".join(f"{float(value):.17g}" for value in values)
+
+
+def deserialize_float_sequence(value: str) -> list[float]:
+    """Deserialize a semicolon-delimited metadata field into floats.
+
+    Args:
+        value: Semicolon-delimited float values.
+    Returns:
+        `list[float]` parsed values.
+    Raises:
+        ValueError: If any entry cannot be parsed as a float.
+    """
+    if value == "":
+        return []
+    return [float(piece) for piece in value.split(";") if piece != ""]
+
+
+def serialize_int_sequence(values: Sequence[int]) -> str:
+    """Serialize an integer sequence as a semicolon-delimited metadata field.
+
+    Args:
+        values: Values to serialize.
+    Returns:
+        `str` semicolon-delimited integer values.
+    """
+    return ";".join(str(int(value)) for value in values)
+
+
+def build_shaper_feature_metadata(base_joint_count: int) -> tuple[str, str]:
+    """Return serialized feature names and units for shaper model inputs.
+
+    Args:
+        base_joint_count: Number of consecutive base joints [count].
+    Returns:
+        `tuple[str, str]` serialized feature names and units.
+    Raises:
+        ValueError: If `base_joint_count` is less than one.
+    """
+    if base_joint_count < 1:
+        raise ValueError("base_joint_count must be >= 1.")
+    feature_names = [f"j{index}_rad" for index in range(base_joint_count)]
+    feature_names.extend(["v_deg", "r_mm", "inertia"])
+    feature_units = ["rad" for _ in range(base_joint_count)]
+    feature_units.extend(["deg", "mm", "kg_m2"])
+    return ";".join(feature_names), ";".join(feature_units)
+
+
+def build_base_angle_grid(
+    lower_rad: float,
+    upper_rad: float,
+    home_rad: float,
+    max_displacement_rad: float,
+    base_joint_angle_count: int,
+) -> BaseAngleGrid:
+    """Build a base-joint angle grid while avoiding joint-limit endpoints.
+
+    Args:
+        lower_rad: Lower joint position limit [rad].
+        upper_rad: Upper joint position limit [rad].
+        home_rad: Reference/home joint angle [rad].
+        max_displacement_rad: Maximum sine displacement used as endpoint margin [rad].
+        base_joint_angle_count: Number of grid samples [count].
+    Returns:
+        `BaseAngleGrid` positions and margin rule.
+    Raises:
+        ValueError: If limits, displacement, or count are invalid.
+    """
+    if base_joint_angle_count <= 0:
+        raise ValueError("base_joint_angle_count must be > 0.")
+    if not lower_rad < upper_rad:
+        raise ValueError("Base joint lower limit must be less than upper limit.")
+    if max_displacement_rad < 0.0 or not math.isfinite(max_displacement_rad):
+        raise ValueError("max_displacement_rad must be finite and non-negative.")
+
+    joint_range = upper_rad - lower_rad
+    if joint_range > 2.0 * max_displacement_rad:
+        start = lower_rad + max_displacement_rad
+        stop = upper_rad - max_displacement_rad
+        margin_source = "max_displacement"
+    elif joint_range > max_displacement_rad:
+        half_margin = max_displacement_rad / 2.0
+        start = lower_rad + half_margin
+        stop = upper_rad - half_margin
+        margin_source = "half_max_displacement"
+    else:
+        if base_joint_angle_count == 1:
+            clipped_home = min(max(home_rad, lower_rad), upper_rad)
+            return BaseAngleGrid((float(clipped_home),), "home_fallback")
+        spacing = joint_range / float(base_joint_angle_count + 1)
+        start = lower_rad + spacing
+        stop = upper_rad - spacing
+        margin_source = "interior_fallback"
+
+    angles = np.linspace(start, stop, base_joint_angle_count, dtype=float)
+    return BaseAngleGrid(tuple(float(angle) for angle in angles), margin_source)
+
+
+def validate_base_joint_configuration(
+    num_joints: int,
+    axes_to_command: int,
+    base_joint_count: int,
+    joint_axes: Sequence[Sequence[float]],
+    joint_limits: tuple[Sequence[float], Sequence[float]],
+    explicit_limits: Sequence[BaseJointLimit] | None = None,
+    axis_parallel_atol: float = BASE_JOINT_AXIS_PARALLEL_ATOL,
+) -> list[BaseJointLimit]:
+    """Validate base-joint count, axes, and limits for calibration planning.
+
+    Args:
+        num_joints: Robot joint count [count].
+        axes_to_command: Number of shaped axes in the calibration [count].
+        base_joint_count: Consecutive base-joint count starting at index zero [count].
+        joint_axes: World-frame joint axes for the reference configuration.
+        joint_limits: Lower and upper joint limits for all joints [rad].
+        explicit_limits: Optional CLI-provided limits for leading base joints [rad].
+        axis_parallel_atol: Absolute tolerance for z-parallel axis checks.
+    Returns:
+        `list[BaseJointLimit]` resolved base-joint limits [rad].
+    Raises:
+        ValueError: If the requested base-joint configuration is unsupported.
+    """
+    if base_joint_count < 1:
+        raise ValueError("base_joint_count must be >= 1.")
+    if base_joint_count > num_joints:
+        raise ValueError("base_joint_count must be <= num_joints.")
+    if base_joint_count > axes_to_command:
+        raise ValueError("base_joint_count must be <= axes_to_command.")
+    if len(joint_axes) < base_joint_count:
+        raise ValueError("Not enough joint axes to validate requested base joints.")
+
+    lower_limits, upper_limits = joint_limits
+    explicit_limits = explicit_limits or []
+    resolved_limits = []
+    world_z = np.array([0.0, 0.0, 1.0], dtype=float)
+
+    for joint_index in range(base_joint_count):
+        axis = np.asarray(joint_axes[joint_index], dtype=float)
+        norm = np.linalg.norm(axis)
+        if norm <= 0.0:
+            raise ValueError(f"Base joint {joint_index} axis has zero length.")
+        normalized_axis = axis / norm
+        is_parallel = np.allclose(
+            np.abs(normalized_axis), world_z, atol=axis_parallel_atol
+        )
+        if not is_parallel:
+            raise ValueError(
+                f"Base joint {joint_index} axis is not parallel to world z."
+            )
+
+        if joint_index < len(explicit_limits):
+            resolved_limits.append(explicit_limits[joint_index])
+            continue
+
+        lower_rad = float(lower_limits[joint_index])
+        upper_rad = float(upper_limits[joint_index])
+        if not (
+            math.isfinite(lower_rad)
+            and math.isfinite(upper_rad)
+            and lower_rad < upper_rad
+        ):
+            raise ValueError(
+                f"Base joint {joint_index} is missing finite lower/upper limits; "
+                "provide --base_joint_limits."
+            )
+        resolved_limits.append(BaseJointLimit(lower_rad=lower_rad, upper_rad=upper_rad))
+
+    return resolved_limits
+
+
+def resolve_base_joint_limits_from_model(
+    robot_model: object,
+    reference_joint_angles: Sequence[float],
+    axes_to_command: int,
+    base_joint_count: int,
+    explicit_limits: Sequence[BaseJointLimit] | None = None,
+) -> list[BaseJointLimit]:
+    """Validate base joints using the dynamics model API and return limits.
+
+    Args:
+        robot_model: Dynamics-like object exposing `joint_limits` and
+            `get_joint_axes(reference_joint_angles)`.
+        reference_joint_angles: Reference joint configuration [rad].
+        axes_to_command: Number of shaped axes [count].
+        base_joint_count: Consecutive base-joint count starting at joint zero [count].
+        explicit_limits: Optional CLI-provided limits for leading base joints [rad].
+    Returns:
+        `list[BaseJointLimit]` resolved limits [rad].
+    Raises:
+        RuntimeError: If the robot model is missing.
+        ValueError: If model limits or world-frame axes do not satisfy the base-joint rules.
+    """
+    if robot_model is None:
+        raise RuntimeError(
+            "Base-joint validation requires a loaded robot dynamics model."
+        )
+    num_joints = int(getattr(robot_model, "num_joints"))
+    joint_axes = robot_model.get_joint_axes(reference_joint_angles)
+    joint_limits = robot_model.joint_limits
+    return validate_base_joint_configuration(
+        num_joints=num_joints,
+        axes_to_command=axes_to_command,
+        base_joint_count=base_joint_count,
+        joint_axes=joint_axes,
+        joint_limits=joint_limits,
+        explicit_limits=explicit_limits,
+    )
+
+
+def build_base_angle_grids(
+    base_joint_limits: Sequence[BaseJointLimit],
+    reference_base_angles_rad: Sequence[float],
+    max_displacement_rad: float,
+    base_joint_angle_count: int,
+) -> list[BaseAngleGrid]:
+    """Build one endpoint-safe base-angle grid per base joint.
+
+    Args:
+        base_joint_limits: Base joint limits [rad].
+        reference_base_angles_rad: Reference/home angles for base joints [rad].
+        max_displacement_rad: Maximum sine displacement used as endpoint margin [rad].
+        base_joint_angle_count: Number of grid samples per base joint [count].
+    Returns:
+        `list[BaseAngleGrid]` one grid per base joint.
+    Raises:
+        ValueError: If inputs are inconsistent or invalid.
+    """
+    if len(base_joint_limits) != len(reference_base_angles_rad):
+        raise ValueError(
+            "Base joint limits and reference angles must have equal length."
+        )
+    return [
+        build_base_angle_grid(
+            lower_rad=limit.lower_rad,
+            upper_rad=limit.upper_rad,
+            home_rad=reference_base_angles_rad[index],
+            max_displacement_rad=max_displacement_rad,
+            base_joint_angle_count=base_joint_angle_count,
+        )
+        for index, limit in enumerate(base_joint_limits)
+    ]
+
+
+def estimate_shaper_run_count(
+    base_joint_count: int,
+    base_joint_angle_count: int,
+    pose_count: int,
+    axes_to_command: int,
+    includes_joint_tracker_mpc: bool = False,
+) -> int:
+    """Estimate planned shaper calibration run count.
+
+    Args:
+        base_joint_count: Consecutive base-joint count [count].
+        base_joint_angle_count: Grid samples per base joint [count].
+        pose_count: Number of `V`, `R` poses [count].
+        axes_to_command: Number of normal full-axis sweeps [count].
+        includes_joint_tracker_mpc: Whether a one-pose joint-tracker prepass is included.
+    Returns:
+        `int` total planned sweep count.
+    Raises:
+        ValueError: If any count is invalid.
+    """
+    if min(base_joint_count, base_joint_angle_count, pose_count, axes_to_command) <= 0:
+        raise ValueError("All run-count inputs must be positive.")
+    normal_count = pose_count * axes_to_command
+    base_count = sum(
+        (base_joint_angle_count ** (base_joint_count - index)) * pose_count
+        for index in range(base_joint_count)
+    )
+    joint_tracker_count = axes_to_command if includes_joint_tracker_mpc else 0
+    return int(joint_tracker_count + normal_count + base_count)
+
+
+def generate_calibration_run_descriptors(
+    v_list: Sequence[float],
+    r_list: Sequence[float],
+    axes_to_command: int,
+    start_axis: int,
+    reference_base_angles_rad: Sequence[float],
+    base_angle_grids: Sequence[BaseAngleGrid],
+    test_mode: bool = False,
+    uses_joint_tracker_mpc: bool = False,
+) -> list[CalibrationRunDescriptor]:
+    """Generate ordered normal and base-grid calibration run descriptors.
+
+    Args:
+        v_list: Pose-grid elevation angles [rad].
+        r_list: Pose-grid radii [m].
+        axes_to_command: Number of normal full-axis sweeps [count].
+        start_axis: First commanded axis [joint index].
+        reference_base_angles_rad: Fixed reference base-joint angles [rad].
+        base_angle_grids: Base-joint grids [rad].
+        test_mode: Whether descriptors represent point-to-point test motion.
+        uses_joint_tracker_mpc: Whether joint-tracker MPC compensation is planned.
+    Returns:
+        `list[CalibrationRunDescriptor]` ordered planned runs.
+    Raises:
+        ValueError: If the base metadata is inconsistent.
+    """
+    base_joint_count = len(base_angle_grids)
+    if base_joint_count < 1:
+        raise ValueError("At least one base angle grid is required.")
+    if len(reference_base_angles_rad) != base_joint_count:
+        raise ValueError("Reference base angles must match base angle grid count.")
+
+    motion_mode = (
+        MOTION_MODE_POINT_TO_POINT_TEST if test_mode else MOTION_MODE_SINE_SWEEP
+    )
+    pose_pairs = [
+        (pose_index, float(v_value), float(r_value))
+        for pose_index, (v_value, r_value) in enumerate(product(v_list, r_list))
+    ]
+    descriptors = []
+
+    for pose_index, v_value, r_value in pose_pairs:
+        for axis_offset, move_axis in enumerate(
+            range(start_axis, start_axis + axes_to_command)
+        ):
+            descriptors.append(
+                CalibrationRunDescriptor(
+                    run_id=f"normal-p{pose_index:04d}-a{move_axis:02d}",
+                    run_role=RUN_ROLE_NORMAL_FULL_AXIS,
+                    motion_mode=motion_mode,
+                    swept_axis=move_axis,
+                    pose_index=pose_index,
+                    axis_index=axis_offset,
+                    base_joint_angles_rad=tuple(
+                        float(v) for v in reference_base_angles_rad
+                    ),
+                    v_rad=v_value,
+                    r_m=r_value,
+                    uses_joint_tracker_mpc=uses_joint_tracker_mpc,
+                )
+            )
+
+    for swept_base_joint in reversed(range(base_joint_count)):
+        downstream_grid_values = [
+            base_angle_grids[index].angles_rad
+            for index in range(swept_base_joint, base_joint_count)
+        ]
+        for permutation_index, downstream_angles in enumerate(
+            product(*downstream_grid_values)
+        ):
+            base_angles = list(float(value) for value in reference_base_angles_rad)
+            for offset, angle_rad in enumerate(downstream_angles):
+                base_angles[swept_base_joint + offset] = float(angle_rad)
+            for pose_index, v_value, r_value in pose_pairs:
+                descriptors.append(
+                    CalibrationRunDescriptor(
+                        run_id=(
+                            f"base-j{swept_base_joint}-g{permutation_index:04d}"
+                            f"-p{pose_index:04d}"
+                        ),
+                        run_role=RUN_ROLE_BASE_ONLY_AXIS,
+                        motion_mode=motion_mode,
+                        swept_axis=swept_base_joint,
+                        pose_index=pose_index,
+                        axis_index=swept_base_joint,
+                        base_joint_angles_rad=tuple(base_angles),
+                        v_rad=v_value,
+                        r_m=r_value,
+                        uses_joint_tracker_mpc=uses_joint_tracker_mpc,
+                    )
+                )
+    return descriptors
+
+
+def resolve_saved_sample_frequency(sample_frequency_hz: float) -> float:
+    """Return the sample frequency used for persisted calibration files.
+
+    Args:
+        sample_frequency_hz: Acquisition sample frequency [Hz].
+    Returns:
+        `float` saved sample frequency [Hz].
+    Raises:
+        ValueError: If `sample_frequency_hz` is not positive and finite.
+    """
+    if sample_frequency_hz <= 0.0 or not math.isfinite(sample_frequency_hz):
+        raise ValueError("sample_frequency_hz must be positive and finite.")
+    return min(float(sample_frequency_hz), MAX_SAVED_SAMPLE_FREQUENCY_HZ)
+
+
+def downsample_indices_for_storage(
+    sample_count: int, sample_frequency_hz: float
+) -> np.ndarray:
+    """Return row indices that cap saved calibration data at 500 Hz.
+
+    Args:
+        sample_count: Number of recorded samples [count].
+        sample_frequency_hz: Acquisition sample frequency [Hz].
+    Returns:
+        `np.ndarray` integer sample indices to keep.
+    Raises:
+        ValueError: If inputs are invalid.
+    """
+    if sample_count < 0:
+        raise ValueError("sample_count must be >= 0.")
+    if sample_count == 0:
+        return np.array([], dtype=int)
+    saved_frequency_hz = resolve_saved_sample_frequency(sample_frequency_hz)
+    if sample_frequency_hz <= saved_frequency_hz:
+        return np.arange(sample_count, dtype=int)
+
+    step = sample_frequency_hz / saved_frequency_hz
+    indices = np.floor(np.arange(0.0, float(sample_count), step)).astype(int)
+    indices = np.unique(np.clip(indices, 0, sample_count - 1))
+    if indices[-1] != sample_count - 1:
+        indices = np.append(indices, sample_count - 1)
+    return indices.astype(int)