roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/franka/ik.py

@@ -0,0 +1,543 @@
+"""Feedback-leashed inverse kinematics for Franka Cartesian control."""
+
+from __future__ import annotations
+
+import warnings
+from dataclasses import dataclass
+from typing import Sequence
+
+import numpy as np
+from scipy.optimize import Bounds, LinearConstraint, minimize
+
+from .settings import IKConfig
+
+
+@dataclass(frozen=True)
+class IKResult:
+    qd: np.ndarray
+    dq: np.ndarray
+    pose_error: np.ndarray
+    minimum_joint_margin: float
+    task_scale: float
+    joint_velocity_limited: bool
+    posture_limited: bool
+
+
+def _vector(value: Sequence[float], size: int, name: str) -> np.ndarray:
+    result = np.asarray(value, dtype=np.float64)
+    if result.shape != (size,) or not np.all(np.isfinite(result)):
+        raise ValueError(f"{name} must contain {size} finite values")
+    return result
+
+
+def normalize_quaternion(value: Sequence[float]) -> np.ndarray:
+    quaternion = _vector(value, 4, "quaternion")
+    norm = float(np.linalg.norm(quaternion))
+    if norm < 1e-12:
+        raise ValueError("quaternion norm is zero")
+    return quaternion / norm
+
+
+def quaternion_error_rotvec(target_xyzw: Sequence[float], current_xyzw: Sequence[float]) -> np.ndarray:
+    """Shortest-path world-frame rotation vector from current to target."""
+    target = normalize_quaternion(target_xyzw)
+    current = normalize_quaternion(current_xyzw)
+    if float(np.dot(target, current)) < 0.0:
+        target = -target
+    tx, ty, tz, tw = target
+    cx, cy, cz, cw = current
+    # target * inverse(current)
+    vector = np.array(
+        [tw * -cx + tx * cw + ty * -cz - tz * -cy,
+         tw * -cy - tx * -cz + ty * cw + tz * -cx,
+         tw * -cz + tx * -cy - ty * -cx + tz * cw],
+        dtype=np.float64,
+    )
+    scalar = tw * cw + tx * cx + ty * cy + tz * cz
+    vector_norm = float(np.linalg.norm(vector))
+    if vector_norm < 1e-12:
+        return np.zeros(3, dtype=np.float64)
+    angle = 2.0 * np.arctan2(vector_norm, max(0.0, float(scalar)))
+    return vector * (angle / vector_norm)
+
+
+def _clip_norm(vector: np.ndarray, limit: float) -> np.ndarray:
+    norm = float(np.linalg.norm(vector))
+    if norm <= limit or norm < 1e-12:
+        return vector.copy()
+    return vector * (limit / norm)
+
+
+def _adaptive_regularization(
+    min_singular_value: float, config: IKConfig
+) -> float:
+    """Keep normal-space response while damping only the singular region."""
+    if config.singularity_hard_threshold >= config.singularity_soft_threshold:
+        raise ValueError(
+            "singularity_hard_threshold must be less than "
+            "singularity_soft_threshold"
+        )
+    if config.singularity_max_regularization < config.regularization:
+        raise ValueError(
+            "singularity_max_regularization must be at least regularization"
+        )
+    if min_singular_value >= config.singularity_soft_threshold:
+        return float(config.regularization)
+    if min_singular_value <= config.singularity_hard_threshold:
+        return float(config.singularity_max_regularization)
+    ratio = (
+        config.singularity_soft_threshold - min_singular_value
+    ) / (
+        config.singularity_soft_threshold - config.singularity_hard_threshold
+    )
+    # Smoothstep avoids a damping derivative discontinuity at either threshold.
+    blend = ratio * ratio * (3.0 - 2.0 * ratio)
+    return float(
+        config.regularization
+        + blend * (config.singularity_max_regularization - config.regularization)
+    )
+
+
+def _uniform_task_scale(
+    velocity: np.ndarray, lower: np.ndarray, upper: np.ndarray
+) -> float:
+    """Largest [0, 1] scale that preserves the differential-IK direction."""
+    scale = 1.0
+    for value, low, high in zip(velocity, lower, upper):
+        if value > high:
+            scale = min(scale, float(high / value))
+        elif value < low:
+            scale = min(scale, float(low / value))
+    return float(np.clip(scale, 0.0, 1.0))
+
+
+def _solve_bounded_qp(
+    hessian: np.ndarray,
+    rhs: np.ndarray,
+    lower: np.ndarray,
+    upper: np.ndarray,
+    *,
+    projection_direction: np.ndarray | None = None,
+    max_projection: float | None = None,
+) -> np.ndarray:
+    """Solve the seven-dimensional IK QP with box and nullspace bounds."""
+    if np.any(lower > upper):
+        raise ValueError("joint velocity constraints are infeasible")
+    constraints = ()
+    if projection_direction is not None:
+        if (
+            max_projection is None
+            or not np.isfinite(max_projection)
+            or max_projection <= 0
+        ):
+            raise ValueError("max_projection must be positive")
+        direction = _vector(projection_direction, rhs.size, "projection_direction")
+        minimum = float(
+            np.sum(np.where(direction >= 0.0, direction * lower, direction * upper))
+        )
+        maximum = float(
+            np.sum(np.where(direction >= 0.0, direction * upper, direction * lower))
+        )
+        if minimum > max_projection or maximum < -max_projection:
+            raise np.linalg.LinAlgError(
+                "nullspace velocity cap is incompatible with the joint velocity box"
+            )
+        constraints = (
+            LinearConstraint(direction, -max_projection, max_projection),
+        )
+
+    def objective(value: np.ndarray) -> float:
+        return float(0.5 * value @ hessian @ value - rhs @ value)
+
+    with warnings.catch_warnings():
+        warnings.filterwarnings(
+            "ignore",
+            message="Values in x were outside bounds during a minimize step",
+            category=RuntimeWarning,
+            module="scipy.optimize._optimize",
+        )
+        result = minimize(
+            objective,
+            np.clip(np.linalg.solve(hessian, rhs), lower, upper),
+            jac=lambda value: hessian @ value - rhs,
+            bounds=Bounds(lower, upper),
+            constraints=constraints,
+            method="SLSQP",
+            options={"ftol": 1e-12, "maxiter": 100},
+        )
+    solution = np.clip(np.asarray(result.x, dtype=np.float64), lower, upper)
+    finite = bool(np.all(np.isfinite(solution)))
+    projection_ok = projection_direction is None or (
+        abs(float(direction @ solution)) <= float(max_projection) + 1e-7
+    )
+    if not result.success or not finite or not projection_ok:
+        raise np.linalg.LinAlgError(f"bounded IK QP failed: {result.message}")
+    return solution
+
+
+def _strict_nullspace_posture(
+    *,
+    primary_dq: np.ndarray,
+    direction: np.ndarray,
+    preferred_dq: np.ndarray,
+    lower: np.ndarray,
+    upper: np.ndarray,
+    max_velocity: float,
+) -> tuple[np.ndarray, bool]:
+    """Set the final feasible posture velocity in the exact 7-DoF nullspace.
+
+    A full-row-rank 6x7 Jacobian always has one structural null direction.  By
+    parameterising the correction as ``z * alpha`` with ``J z == 0``, the
+    secondary posture objective cannot alter the Cartesian velocity already
+    chosen by the bounded primary QP.  Crucially, ``max_velocity`` limits the
+    *final* structural-nullspace component, not the correction added this tick.
+    Otherwise a primary solution constrained around the previous command can
+    carry old nullspace velocity which is then added again on every cycle.
+
+    The scalar correction interval is the intersection of all seven joint
+    velocity boxes, so acceleration and joint limits remain authoritative
+    without another optimisation dependency.
+    """
+    if not np.isfinite(max_velocity) or max_velocity <= 0.0:
+        raise ValueError("posture_max_velocity must be positive")
+    if np.any(primary_dq < lower - 1e-9) or np.any(primary_dq > upper + 1e-9):
+        raise ValueError("primary IK velocity is outside posture bounds")
+
+    primary_alpha = float(np.dot(direction, primary_dq))
+    desired_alpha = float(np.dot(direction, preferred_dq))
+    target_alpha = float(np.clip(desired_alpha, -max_velocity, max_velocity))
+    desired_correction_alpha = target_alpha - primary_alpha
+
+    alpha_lower = -np.inf
+    alpha_upper = np.inf
+    for value, low, high, component in zip(
+        primary_dq, lower, upper, direction
+    ):
+        if abs(component) < 1e-12:
+            continue
+        first = float((low - value) / component)
+        second = float((high - value) / component)
+        alpha_lower = max(alpha_lower, min(first, second))
+        alpha_upper = min(alpha_upper, max(first, second))
+    if alpha_lower > alpha_upper + 1e-10:
+        raise np.linalg.LinAlgError(
+            "nullspace posture constraints are infeasible"
+        )
+    correction_alpha = float(
+        np.clip(desired_correction_alpha, alpha_lower, alpha_upper)
+    )
+    limited = bool(
+        abs(target_alpha - desired_alpha) > 1e-9
+        or abs(correction_alpha - desired_correction_alpha) > 1e-9
+    )
+    return primary_dq + direction * correction_alpha, limited
+
+
+def _posture_motion_activity(
+    *,
+    translation_speed: float,
+    rotation_speed: float,
+    translation_threshold: float,
+    rotation_threshold: float,
+) -> float:
+    """Smoothly enable posture motion from 1x to 2x each task threshold."""
+
+    def ramp(speed: float, threshold: float) -> float:
+        if threshold == 0.0:
+            return float(speed > 0.0)
+        ratio = float(np.clip((speed - threshold) / threshold, 0.0, 1.0))
+        return ratio * ratio * (3.0 - 2.0 * ratio)
+
+    return max(
+        ramp(translation_speed, translation_threshold),
+        ramp(rotation_speed, rotation_threshold),
+    )
+
+
+def solve_feedback_leashed_ik(
+    *,
+    q: Sequence[float],
+    current_pose7: Sequence[float],
+    target_pose7: Sequence[float],
+    target_twist: Sequence[float] | None = None,
+    jacobian: Sequence[Sequence[float]],
+    lower_limits: Sequence[float],
+    upper_limits: Sequence[float],
+    dt: float,
+    config: IKConfig,
+    previous_dq: Sequence[float] | None = None,
+    joint_velocity_bounds: tuple[np.ndarray, np.ndarray] | None = None,
+) -> IKResult:
+    """Compute one bounded q target based only on the latest measured q."""
+    qv = _vector(q, 7, "q")
+    current = _vector(current_pose7, 7, "current_pose7")
+    target = _vector(target_pose7, 7, "target_pose7")
+    lower = _vector(lower_limits, 7, "lower_limits")
+    upper = _vector(upper_limits, 7, "upper_limits")
+    jac = np.asarray(jacobian, dtype=np.float64)
+    if jac.shape != (6, 7) or not np.all(np.isfinite(jac)):
+        raise ValueError("jacobian must be a finite 6x7 matrix")
+    if not np.all(lower < upper):
+        raise ValueError("joint limits are invalid")
+    if np.any(qv < lower) or np.any(qv > upper):
+        raise ValueError("measured joints are outside model limits")
+    if not np.isfinite(config.joint_limit_margin) or config.joint_limit_margin <= 0.0:
+        raise ValueError("joint_limit_margin must be positive")
+    if not np.isfinite(config.nullspace_gain) or config.nullspace_gain < 0.0:
+        raise ValueError("nullspace_gain must be non-negative")
+    if not np.isfinite(config.nominal_dt) or config.nominal_dt <= 0.0:
+        raise ValueError("nominal_dt must be positive")
+    if not np.isfinite(config.target_lead_time) or config.target_lead_time <= 0.0:
+        raise ValueError("target_lead_time must be positive")
+    if (
+        not np.isfinite(config.translation_feedback_gain)
+        or config.translation_feedback_gain < 0.0
+    ):
+        raise ValueError("translation_feedback_gain must be non-negative")
+    if (
+        not np.isfinite(config.rotation_feedback_gain)
+        or config.rotation_feedback_gain < 0.0
+    ):
+        raise ValueError("rotation_feedback_gain must be non-negative")
+    posture_reference = _vector(
+        config.posture_reference, 7, "posture_reference"
+    )
+    posture_weights = _vector(config.posture_weights, 7, "posture_weights")
+    if (
+        config.posture_gain > 0.0
+        and (
+            np.any(posture_reference < lower)
+            or np.any(posture_reference > upper)
+        )
+    ):
+        raise ValueError("posture_reference is outside model limits")
+    if np.any(posture_weights < 0.0):
+        raise ValueError("posture_weights must be non-negative")
+    if not np.isfinite(config.posture_gain) or config.posture_gain < 0.0:
+        raise ValueError("posture_gain must be non-negative")
+    if (
+        not np.isfinite(config.posture_task_velocity_threshold)
+        or config.posture_task_velocity_threshold < 0.0
+    ):
+        raise ValueError("posture_task_velocity_threshold must be non-negative")
+    if (
+        not np.isfinite(config.posture_rotation_velocity_threshold)
+        or config.posture_rotation_velocity_threshold < 0.0
+    ):
+        raise ValueError(
+            "posture_rotation_velocity_threshold must be non-negative"
+        )
+    max_joint_velocity = _vector(
+        config.max_joint_velocity, 7, "max_joint_velocity"
+    )
+    max_joint_acceleration = _vector(
+        config.max_joint_acceleration, 7, "max_joint_acceleration"
+    )
+    if np.any(max_joint_velocity <= 0.0):
+        raise ValueError("max_joint_velocity must contain positive values")
+    if np.any(max_joint_acceleration <= 0.0):
+        raise ValueError("max_joint_acceleration must contain positive values")
+    previous_velocity = (
+        np.zeros(7, dtype=np.float64)
+        if previous_dq is None
+        else _vector(previous_dq, 7, "previous_dq")
+    )
+
+    budget_dt = min(max(float(dt), 0.0), config.nominal_dt)
+    if not np.isfinite(budget_dt) or budget_dt <= 0.0:
+        budget_dt = config.nominal_dt
+    error = np.concatenate(
+        [target[:3] - current[:3], quaternion_error_rotvec(target[3:], current[3:])]
+    )
+    twist = (
+        np.zeros(6, dtype=np.float64)
+        if target_twist is None
+        else _vector(target_twist, 6, "target_twist")
+    )
+    max_translation_velocity = float(config.max_translation_velocity)
+    max_rotation_velocity = float(config.max_rotation_velocity)
+    if (not np.isfinite(max_translation_velocity) or max_translation_velocity <= 0.0
+            or not np.isfinite(max_rotation_velocity) or max_rotation_velocity <= 0.0):
+        raise ValueError("Cartesian velocity limits must be positive and finite")
+    if (not np.isfinite(config.max_translation_step) or config.max_translation_step <= 0.0
+            or not np.isfinite(config.max_rotation_step) or config.max_rotation_step <= 0.0):
+        raise ValueError("Cartesian step limits must be positive and finite")
+    max_translation_velocity = min(
+        max_translation_velocity, config.max_translation_step / config.nominal_dt,
+    )
+    max_rotation_velocity = min(
+        max_rotation_velocity, config.max_rotation_step / config.nominal_dt,
+    )
+    feedback_velocity = np.concatenate(
+        [
+            _clip_norm(
+                config.translation_feedback_gain * error[:3],
+                max_translation_velocity,
+            ),
+            _clip_norm(
+                config.rotation_feedback_gain * error[3:],
+                max_rotation_velocity,
+            ),
+        ]
+    )
+    feedforward_velocity = np.concatenate(
+        [
+            _clip_norm(twist[:3], max_translation_velocity),
+            _clip_norm(twist[3:], max_rotation_velocity),
+        ]
+    )
+    cartesian_velocity = np.concatenate(
+        [
+            _clip_norm(
+                feedback_velocity[:3] + feedforward_velocity[:3],
+                max_translation_velocity,
+            ),
+            _clip_norm(
+                feedback_velocity[3:] + feedforward_velocity[3:],
+                max_rotation_velocity,
+            ),
+        ]
+    )
+
+    singular_values = np.linalg.svd(jac, compute_uv=False)
+    min_singular_value = float(singular_values[-1])
+    effective_regularization = _adaptive_regularization(
+        min_singular_value, config
+    )
+    damping2 = effective_regularization**2
+    hessian = jac.T @ jac + damping2 * np.eye(7)
+    task_rhs = jac.T @ cartesian_velocity
+    unconstrained_task_dq = np.linalg.solve(hessian, task_rhs)
+
+    lower_distance = qv - lower
+    upper_distance = upper - qv
+    centre = 0.5 * (lower + upper)
+    proximity = np.clip(
+        1.0 - np.minimum(lower_distance, upper_distance) / config.joint_limit_margin,
+        0.0, 1.0,
+    )
+    # Smoothly bias the redundant joint direction away from a nearby limit.
+    proximity = proximity * proximity * (3.0 - 2.0 * proximity)
+    limit_bias = config.nullspace_gain * proximity * np.sign(centre - qv)
+
+    posture_error = (
+        posture_reference - qv
+        if config.posture_gain > 0.0
+        else np.zeros(7, dtype=np.float64)
+    )
+    # Feed-forward twist is the trajectory's motion intent.  A smooth activity
+    # ramp keeps coordinated Hold exactly still and avoids a posture-velocity
+    # step as a segment starts or settles.  Translation and rotation use their
+    # own physical-unit thresholds; either kind of task motion can enable the
+    # secondary objective.
+    posture_activity = _posture_motion_activity(
+        # Motion intent is the complete Cartesian request. Teleop deliberately
+        # suppresses trajectory twist, but its pose-feedback motion must still
+        # keep posture control active. A settled Hold has neither component.
+        translation_speed=float(np.linalg.norm(cartesian_velocity[:3])),
+        rotation_speed=float(np.linalg.norm(cartesian_velocity[3:])),
+        translation_threshold=config.posture_task_velocity_threshold,
+        rotation_threshold=config.posture_rotation_velocity_threshold,
+    )
+    posture_preference = (
+        posture_activity
+        * config.posture_gain
+        * posture_weights
+        * posture_error
+    )
+    posture_preference += limit_bias
+
+    # Apply step budgets and directional slowdown to a symmetric velocity box.
+    # Intersect the official box afterwards: it may require inward motion and
+    # must never be relaxed by multiplying one of its bounds toward zero.
+    max_step = _vector(config.max_joint_step, 7, "max_joint_step")
+    if np.any(max_step <= 0.0):
+        raise ValueError("max_joint_step must contain positive values")
+    step_velocity = max_step * (budget_dt / config.nominal_dt) / config.nominal_dt
+    velocity_limit = np.minimum(max_joint_velocity, step_velocity)
+    base_lower = -velocity_limit * np.clip(lower_distance / config.joint_limit_margin, 0.0, 1.0)
+    base_upper = velocity_limit * np.clip(upper_distance / config.joint_limit_margin, 0.0, 1.0)
+    if joint_velocity_bounds is not None:
+        velocity_lower = _vector(joint_velocity_bounds[0], 7, "joint_velocity_bounds lower")
+        velocity_upper = _vector(joint_velocity_bounds[1], 7, "joint_velocity_bounds upper")
+        base_lower = np.maximum(base_lower, velocity_lower)
+        base_upper = np.minimum(base_upper, velocity_upper)
+    position_horizon = max(config.nominal_dt, config.target_lead_time)
+    base_lower = np.maximum(base_lower, (lower - qv) / position_horizon)
+    base_upper = np.minimum(base_upper, (upper - qv) / position_horizon)
+
+    acceleration_delta = max_joint_acceleration * budget_dt
+    lower_velocity = np.maximum(
+        base_lower, previous_velocity - acceleration_delta
+    )
+    upper_velocity = np.minimum(
+        base_upper, previous_velocity + acceleration_delta
+    )
+    if np.any(lower_velocity > upper_velocity):
+        raise ValueError("joint position, velocity and acceleration constraints are infeasible")
+
+    # First scale the complete Cartesian task uniformly.  This preserves its
+    # direction when a velocity, acceleration, or position constraint becomes
+    # active instead of letting independent joint clips bend the requested
+    # path.  During a reversal zero may be outside the acceleration window; in
+    # that transient the QP performs the required bounded deceleration.
+    zero_is_feasible = np.all(lower_velocity <= 0.0) and np.all(
+        upper_velocity >= 0.0
+    )
+    base_contains_zero = np.all(base_lower <= 0.0) and np.all(base_upper >= 0.0)
+    task_scale = 1.0
+    if base_contains_zero:
+        task_scale = _uniform_task_scale(
+            unconstrained_task_dq,
+            lower_velocity if zero_is_feasible else base_lower,
+            upper_velocity if zero_is_feasible else base_upper,
+        )
+    # A position-dependent official box can require inward motion and exclude
+    # zero. Scaling from zero is invalid there; the QP handles that box directly.
+    scaled_cartesian_velocity = task_scale * cartesian_velocity
+
+    # First solve the bounded Cartesian task with no posture trade-off.  Then
+    # use only its exact structural nullspace for the continuous secondary
+    # posture target.  This is a strict-priority hierarchy, not a weighted
+    # objective that can smooth or bend the requested end-effector velocity.
+    rhs = jac.T @ scaled_cartesian_velocity
+    _, _, vh = np.linalg.svd(jac, full_matrices=True)
+    null_direction = np.asarray(vh[-1], dtype=np.float64)
+    null_direction /= max(float(np.linalg.norm(null_direction)), 1e-12)
+    primary_dq = _solve_bounded_qp(
+        hessian,
+        rhs,
+        lower_velocity,
+        upper_velocity,
+        projection_direction=null_direction,
+        max_projection=config.posture_max_velocity,
+    )
+    dq, posture_limited = _strict_nullspace_posture(
+        primary_dq=primary_dq,
+        direction=null_direction,
+        preferred_dq=posture_preference,
+        lower=lower_velocity,
+        upper=upper_velocity,
+        max_velocity=config.posture_max_velocity,
+    )
+    if abs(float(null_direction @ dq)) > config.posture_max_velocity + 1e-7:
+        raise np.linalg.LinAlgError("final structural-nullspace velocity exceeded its hard cap")
+    # qd is a bounded local position lead, rebuilt from measured q every tick.
+    # The same constrained dq is also the damping reference, so the position
+    # and velocity terms agree and no target debt accumulates.
+    lead_time = float(config.target_lead_time)
+    qd = qv + dq * lead_time
+    joint_velocity_limited = bool(
+        task_scale < 1.0 - 1e-6
+        or np.any(np.isclose(dq, lower_velocity, atol=1e-6))
+        or np.any(np.isclose(dq, upper_velocity, atol=1e-6))
+    )
+    return IKResult(
+        qd=qd,
+        dq=dq,
+        pose_error=error,
+        minimum_joint_margin=float(np.min(np.minimum(lower_distance, upper_distance))),
+        task_scale=task_scale,
+        joint_velocity_limited=joint_velocity_limited,
+        posture_limited=posture_limited,
+    )

roboapi/hardware/franka/limits.py

@@ -0,0 +1,110 @@
+"""Official model limits and FR3 position-dependent velocity bounds.
+
+FR3 5.9+ supplies its parameters in the robot's URDF. Earlier FR3 systems use
+the libfranka 0.13.3 formula; Panda uses fixed nominal limits.
+
+Sources:
+https://frankarobotics.github.io/docs/robot_specifications.html
+https://github.com/frankarobotics/libfranka/blob/0.21.2/src/joint_velocity_limits.cpp
+https://github.com/frankarobotics/libfranka/blob/0.13.3/include/franka/rate_limiting.h
+https://github.com/frankarobotics/docs/blob/73f863bb76c6fc1f9ecdb3c6838569e552f3bad0/source/control-parameters-joint-fr3.csv
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class FrankaLimits:
+    joint_lower: tuple[float, ...]
+    joint_upper: tuple[float, ...]
+    joint_velocity: tuple[float, ...]
+    joint_acceleration: tuple[float, ...]
+    cartesian_velocity: tuple[float, float]
+    _velocity_lower: tuple[float, ...] | None = field(default=None, repr=False)
+    _velocity_upper: tuple[float, ...] | None = field(default=None, repr=False)
+    _velocity_offset: tuple[float, ...] | None = field(default=None, repr=False)
+    _deceleration: tuple[float, ...] | None = field(default=None, repr=False)
+
+    def velocity_bounds(self, q) -> tuple[np.ndarray, np.ndarray]:
+        positions = np.asarray(q, dtype=np.float64)
+        if positions.shape != (7,) or not np.all(np.isfinite(positions)):
+            raise ValueError("joint positions must contain seven finite values")
+        maximum = np.asarray(self.joint_velocity)
+        if self._velocity_offset is None:
+            return -maximum, maximum.copy()
+        offset = np.asarray(self._velocity_offset)
+        deceleration = np.asarray(self._deceleration)
+        upper = np.minimum(maximum, np.maximum(
+            0.0, -offset + np.sqrt(np.maximum(
+                0.0, 2.0 * deceleration * (np.asarray(self._velocity_upper) - positions),
+            )),
+        ))
+        lower = np.maximum(-maximum, np.minimum(
+            0.0, offset - np.sqrt(np.maximum(
+                0.0, 2.0 * deceleration * (positions - np.asarray(self._velocity_lower)),
+            )),
+        ))
+        # libfranka's kJointVelocityLimitsTolerance is 0.001 for both FR3 versions.
+        return lower + 0.001, upper - 0.001
+
+
+_FR3_VELOCITY = (2.62, 2.62, 2.62, 2.62, 5.26, 4.18, 5.26)
+_FR3_ACCELERATION = (10.0,) * 7
+_FR3_56 = FrankaLimits(
+    joint_lower=(-2.7437, -1.7837, -2.9007, -3.0421, -2.8065, 0.5445, -3.0159),
+    joint_upper=(2.7437, 1.7837, 2.9007, -0.1518, 2.8065, 4.5169, 3.0159),
+    joint_velocity=_FR3_VELOCITY,
+    joint_acceleration=_FR3_ACCELERATION,
+    cartesian_velocity=(3.0, 2.5),
+    # These curve endpoints differ from the older system's joint-position limits.
+    _velocity_lower=(-2.7501, -1.7918, -2.9065, -3.0481, -2.8101, 0.54092, -3.0196),
+    _velocity_upper=(2.7501, 1.7918, 2.9065, -0.1458, 2.8101, 4.5205, 3.0196),
+    _velocity_offset=(0.30, 0.20, 0.20, 0.30, 0.35, 0.35, 0.35),
+    _deceleration=(6.0, 2.585, 3.5, 4.0, 17.0, 5.5, 17.0),
+)
+_PANDA = FrankaLimits(
+    joint_lower=(-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973),
+    joint_upper=(2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973),
+    joint_velocity=(2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61),
+    joint_acceleration=(15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0),
+    cartesian_velocity=(1.7, 2.5),
+)
+
+
+def load_limits(model: str, model_urdf: str | None) -> FrankaLimits:
+    nominal = {"fr3": _FR3_56, "panda": _PANDA}[model]
+    if model_urdf is None or model == "panda":
+        return nominal
+    root = ET.fromstring(model_urdf)
+
+    joints = {}
+    names = {name: index for index in range(7)
+             for name in (f"joint{index + 1}", f"fr3_joint{index + 1}")}
+    for joint in root.findall("joint"):
+        name = joint.get("name", "")
+        index = names.get(name)
+        if index is None:
+            continue
+        limit = joint.find("limit")
+        curve = joint.find("position_based_velocity_limits")
+        lower, upper, velocity = (float(limit.attrib[key]) for key in ("lower", "upper", "velocity"))
+        offset = float(curve.attrib["velocity_offset"])
+        deceleration = float(curve.attrib["deceleration_limit"])
+        joints[index] = lower, upper, velocity, offset, deceleration
+    lower, upper, velocity, offset, deceleration = zip(*(joints[i] for i in range(7)))
+    return FrankaLimits(
+        joint_lower=lower,
+        joint_upper=upper,
+        joint_velocity=velocity,
+        joint_acceleration=_FR3_ACCELERATION,
+        cartesian_velocity=(3.0, 2.5),
+        _velocity_lower=lower,
+        _velocity_upper=upper,
+        _velocity_offset=offset,
+        _deceleration=deceleration,
+    )