API reference
Auto-generated from docstrings. The public surface is small by design — most users only touch Manipulator, Solution, and (when relevant) TolerancePolicy / Diagnostic.
Entry point: Manipulator
Public IK + FK handle for a robot arm.
Construct via the factory classmethods (e.g. :meth:from_urdf), then use
:meth:fk and :meth:ik for the common path. The dispatched analytical
solver auto-routes based on the arm's kinematic topology -- users do not
need to import or name a specific solver.
.. note::
The constructor signature Manipulator(kinbody) is the escape hatch
for callers who already have a :class:KinBody (e.g. from a custom
loader). Most users should use :meth:from_urdf instead.
Source code in src/ssik/manipulator.py
| class Manipulator:
"""Public IK + FK handle for a robot arm.
Construct via the factory classmethods (e.g. :meth:`from_urdf`), then use
:meth:`fk` and :meth:`ik` for the common path. The dispatched analytical
solver auto-routes based on the arm's kinematic topology -- users do not
need to import or name a specific solver.
.. note::
The constructor signature ``Manipulator(kinbody)`` is the escape hatch
for callers who already have a :class:`KinBody` (e.g. from a custom
loader). Most users should use :meth:`from_urdf` instead.
"""
__slots__ = (
"_kb",
"_plan",
"_policy",
"_prebuilt",
"_solver_module",
"_solver_params",
"_warned_cold_coverage",
)
def __init__(
self,
kinbody: KinBody,
*,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> None:
"""Wrap a pre-built :class:`KinBody`.
:param kinbody: a POE-normalised :class:`KinBody`. If you have a URDF,
use :meth:`from_urdf` instead -- it builds the KinBody for you.
:param policy: tolerance policy used by the topology dispatcher.
Defaults to :data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
"""
self._kb: KinBody = kinbody
self._policy: TolerancePolicy = policy
self._plan: DispatchPlan = dispatch(kinbody, policy=policy)
self._solver_module: ModuleType = importlib.import_module(
SOLVERS[self._plan.solver_name].module_path
)
# The dispatched solver is fixed here, so its parameter set never changes;
# cache it once instead of re-inspecting the signature on every solve().
self._solver_params: frozenset[str] = frozenset(
inspect.signature(self._solver_module.solve).parameters
)
# Guards the one-time cold-coverage warning (#328).
self._warned_cold_coverage: bool = False
# Set only by :meth:`from_prebuilt`, which routes solve() back to the
# artifact's own solver. ``__slots__`` has no class-level default, so
# this has to be assigned on every construction path.
self._prebuilt: ModuleType | None = None
# ------------------------------------------------------------------
# Factories
# ------------------------------------------------------------------
@classmethod
def from_urdf(
cls,
path: str | Path,
*,
base: str | None = None,
ee: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
xacro_args: dict[str, str] | None = None,
) -> Manipulator:
"""Load a URDF (or xacro) and build a :class:`Manipulator` for the chain
between ``base`` and ``ee``.
The kinematic chain is POE-normalised internally so the dispatcher
can match the arm's topology against the solver roster.
Ingestion mirrors ``ssik add-arm``: the URDF is stripped to a
mesh-free, kinematics-only copy before loading, so vendor URDFs that
reference meshes by an unresolvable ``package://`` path (ABB, FANUC,
...) load without a ROS workspace on disk. Xacro descriptions
(``.xacro`` / ``*.urdf.xacro``, or a ``.urdf`` with a xacro namespace)
are expanded automatically via ``xacrodoc`` (``pip install ssik[xacro]``).
:param path: path to the URDF or xacro file.
:param base: name of the base link in the URDF. When omitted, the base
of the longest actuated chain is auto-detected.
:param ee: name of the end-effector link in the URDF. When omitted, the
kinematic flange of the longest actuated chain is auto-detected
(trailing fixed ``tool0`` / gripper frames are skipped).
:param policy: tolerance policy. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:param xacro_args: substitution args for parametrized xacro descriptions
(e.g. ``{"ur_type": "ur10e"}``); ignored for plain URDFs.
:raises FileNotFoundError: if ``path`` doesn't exist.
:raises ValueError: if ``base`` or ``ee`` are not link names in the URDF,
or (when auto-detecting) if the URDF has no actuated joints.
:raises ImportError: if the optional dependency ``urchin`` is not
installed (``pip install ssik[urdf]``).
"""
# Imported lazily so the urchin dependency is only required when
# from_urdf is actually called (it's an optional extra).
from ssik._urdf import load_urdf_kinbody_robust, suggest_base_ee
# urchin raises ValueError on missing files; rewrite to the more
# idiomatic FileNotFoundError for the public API contract.
path_obj = Path(path)
if not path_obj.exists():
raise FileNotFoundError(f"URDF file not found: {path_obj}")
# Auto-detect base/ee (longest actuated chain) when not given, so the
# library entry point is as one-click as ``ssik add-arm``.
if base is None or ee is None:
det_base, det_ee, _notes = suggest_base_ee(path, xacro_args)
base = base if base is not None else det_base
ee = ee if ee is not None else det_ee
kb = load_urdf_kinbody_robust(path, base, ee, xacro_args=xacro_args)
return cls(kb, policy=policy)
@classmethod
def from_prebuilt(
cls,
name: str,
*,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Build a :class:`Manipulator` on one of the arms ssik ships, by name.
This is the supported way to reach the chart API on a prebuilt arm
(#587). A shipped artifact exposes ``solve`` and ``fk``, but
:meth:`self_motion` and :meth:`solve_path` live here, so before this
constructor the only route was the artifact's private baked KinBody --
which ``docs/semver_policy.md`` explicitly places outside the semver
contract. ``ssik.list_arms()`` is the catalog; this turns a row of it
into an arm you can ask for its self-motion manifold.
:meth:`solve` on the result is the **artifact's** solver, not the live
one. That matters: the artifact bakes per-arm work the live path redoes
on every call, and on the Panda the two measure 0.089 ms against 3.627
ms for the same 502 solutions. So the object is not a slower stand-in
for ``import panda_ik`` -- it is that same solver, with the chart API
attached. The two cases that cannot be delegated fall back to the live
path: ``explain=True`` (artifacts do not build a :class:`Diagnostic`)
and any ``solver_kwargs`` beyond ``native`` (artifacts take a fixed
parameter set). Everything else -- :meth:`fk`, :meth:`self_motion`,
:meth:`solve_path`, the introspection properties -- is computed here
from the artifact's own geometry.
Four spellings resolve, tried in that order, so an exact catalog name
can never be shadowed by an abbreviation::
Manipulator.from_prebuilt("franka_panda_ik") # list_arms() name
Manipulator.from_prebuilt("panda_ik") # module basename
Manipulator.from_prebuilt("panda") # either, without _ik
Imports exactly the one artifact named, so this stays as lazy as
``list_arms()`` itself (#421).
:param name: the arm, in any of the spellings above.
:param policy: tolerance policy for the dispatcher and for the live
fallback. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:raises ValueError: if no arm matches (the message lists near misses),
or if an abbreviation matches several (it lists them all).
"""
from ssik.prebuilt import _resolve_arm
_canonical, module = _resolve_arm(name)
arm = cls(module._KB, policy=policy)
arm._prebuilt = module
return arm
@classmethod
def from_mjcf(
cls,
path: str | Path,
*,
base: str | None = None,
ee: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Load a MuJoCo MJCF and build a :class:`Manipulator` for the chain
between two bodies.
The MJCF is compiled by ``mujoco`` (resolving ``<default>`` classes,
``<compiler>`` angle/coordinate settings, ``<include>``, keyframes), and
the POE-normalised chain is read straight off the compiled model, so no
XML is hand-parsed. Solvers and the dispatcher are format-agnostic: the
resulting arm is identical in form to one from :meth:`from_urdf`.
Only single-DOF joints (``hinge`` -> revolute, ``slide`` -> prismatic)
are supported; ``ball`` / ``free`` joints raise. ``q=0`` corresponds to
MuJoCo's reference ``qpos0``.
:param path: path to the ``.xml`` / ``.mjcf`` model file.
:param base: name of the base body. When omitted, the base of the
longest actuated chain is auto-detected (for multi-limb robots pass
an explicit ``base``/``ee`` to select a limb).
:param ee: name of the end-effector body. When omitted, the flange of
the longest actuated chain is auto-detected (trailing gripper/tool
bodies are skipped).
:param policy: tolerance policy. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:raises FileNotFoundError: if ``path`` doesn't exist.
:raises ValueError: if ``base``/``ee`` are not body names, or ``ee`` is
not a descendant of ``base``.
:raises NotImplementedError: on ball/free joints.
:raises ImportError: if the optional dependency ``mujoco`` is not
installed (``pip install ssik[mjcf]``).
"""
# Imported lazily so the mujoco dependency is only required when
# from_mjcf is actually called (it's an optional extra).
from ssik._mjcf import load_mjcf_kinbody_normalized, suggest_base_ee_mjcf
path_obj = Path(path)
if not path_obj.exists():
raise FileNotFoundError(f"MJCF file not found: {path_obj}")
if base is None or ee is None:
det_base, det_ee, _notes = suggest_base_ee_mjcf(path)
base = base if base is not None else det_base
ee = ee if ee is not None else det_ee
kb = load_mjcf_kinbody_normalized(path, base, ee)
return cls(kb, policy=policy)
@classmethod
def from_axes(
cls,
joint_axes: ArrayLike,
offsets: ArrayLike,
*,
limits: ArrayLike | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Build from ik-geo / EAIK product-of-exponentials ``H, P`` parameters.
:param joint_axes: ``(N, 3)`` unit rotation axes in the base frame at
zero configuration (ik-geo ``H``).
:param offsets: ``(N + 1, 3)`` inter-joint translation vectors in the
base frame at zero (ik-geo ``P``): ``offsets[0]`` is base -> joint 0,
``offsets[i]`` is joint ``i-1`` -> joint ``i``, and ``offsets[N]`` is
joint ``N-1`` -> end-effector.
:param limits: optional ``(N, 2)`` per-joint ``(lower, upper)`` ranges.
:param policy: tolerance policy for the dispatcher.
Reproduces ``FK = prod_i [Trans(P[i]) Rot(H[i], q_i)] . Trans(P[N])`` --
the same convention as :class:`eaik.HPRobot`.
"""
h = np.asarray(joint_axes, dtype=np.float64)
p = np.asarray(offsets, dtype=np.float64)
n = h.shape[0]
if h.shape != (n, 3):
raise ValueError(f"joint_axes must be (N, 3); got {h.shape}")
if p.shape != (n + 1, 3):
raise ValueError(f"offsets must be (N + 1, 3) for N={n}; got {p.shape}")
lim = _coerce_limits(limits, n)
specs = [
JointSpec(
parent_link_T=_trans(p[i]),
axis=h[i],
joint_type="revolute",
child_link_T=_trans(p[n]) if i == n - 1 else None,
limits=lim[i],
)
for i in range(n)
]
return cls(build_kinbody(specs), policy=policy)
@classmethod
def from_transforms(
cls,
joint_trafos: ArrayLike,
joint_axis: ArrayLike = (0.0, 0.0, 1.0),
*,
limits: ArrayLike | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Build from per-joint zero-pose homogeneous transforms (EAIK
``HomogeneousRobot``).
:param joint_trafos: ``(N + 1, 4, 4)`` relative zero-pose transforms:
``joint_trafos[i]`` maps frame ``i-1`` to frame ``i`` (``[0]`` is
base -> joint 0), and ``joint_trafos[N]`` maps joint ``N-1`` -> EE.
:param joint_axis: rotation axis in each joint's local frame: one ``(3,)``
axis shared by every joint (default Z), or ``(N, 3)``, one per joint
-- a UR arm's joints turn about ``z, y, y, y, z, y`` in their own
frames, and folding that into the transforms instead would move the
flange rotation off the last one.
:param limits: optional ``(N, 2)`` per-joint ``(lower, upper)`` ranges.
:param policy: tolerance policy for the dispatcher.
"""
t = np.asarray(joint_trafos, dtype=np.float64)
if t.ndim != 3 or t.shape[1:] != (4, 4):
raise ValueError(f"joint_trafos must be (N + 1, 4, 4); got {t.shape}")
n = t.shape[0] - 1
axis = np.asarray(joint_axis, dtype=np.float64)
if axis.shape == (3,):
axes = np.broadcast_to(axis, (n, 3))
elif axis.shape == (n, 3):
axes = axis
else:
raise ValueError(f"joint_axis must be (3,) or ({n}, 3); got {axis.shape}")
lim = _coerce_limits(limits, n)
specs = [
JointSpec(
parent_link_T=t[i],
axis=axes[i],
joint_type="revolute",
child_link_T=t[n] if i == n - 1 else None,
limits=lim[i],
)
for i in range(n)
]
return cls(build_kinbody(specs), policy=policy)
@classmethod
def from_dh(
cls,
dh_alpha: ArrayLike,
dh_a: ArrayLike,
dh_d: ArrayLike,
*,
limits: ArrayLike | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Build from standard (distal) Denavit-Hartenberg parameters (EAIK
``DhRobot``). All joints revolute; the joint variable is the Z rotation.
Per-joint link transform is the classic
``Rot_z(theta_i) . Trans_z(d_i) . Trans_x(a_i) . Rot_x(alpha_i)``.
:param dh_alpha: ``(N,)`` link twists (rad).
:param dh_a: ``(N,)`` link lengths (m).
:param dh_d: ``(N,)`` link offsets (m).
:param limits: optional ``(N, 2)`` per-joint ``(lower, upper)`` ranges.
:param policy: tolerance policy for the dispatcher.
"""
alpha = np.asarray(dh_alpha, dtype=np.float64)
a = np.asarray(dh_a, dtype=np.float64)
d = np.asarray(dh_d, dtype=np.float64)
n = alpha.shape[0]
if not (alpha.shape == (n,) and a.shape == (n,) and d.shape == (n,)):
raise ValueError(
f"dh_alpha / dh_a / dh_d must be equal-length 1-D; got "
f"{alpha.shape}, {a.shape}, {d.shape}"
)
lim = _coerce_limits(limits, n)
z_axis = np.array([0.0, 0.0, 1.0])
specs = [
JointSpec(
parent_link_T=np.eye(4),
axis=z_axis,
joint_type="revolute",
# theta (the joint's Z rotation) is applied first; the fixed
# remainder of the standard-DH link transform is the child T.
child_link_T=_transz(float(d[i])) @ _transx(float(a[i])) @ _rotx(float(alpha[i])),
limits=lim[i],
)
for i in range(n)
]
return cls(build_kinbody(specs), policy=policy)
# ------------------------------------------------------------------
# Introspection
# ------------------------------------------------------------------
@property
def dof(self) -> int:
"""Number of joints in the chain."""
return len(self._kb.joints)
@property
def joint_limits(self) -> list[tuple[float, float] | None]:
"""Per-joint ``(lower, upper)`` limits, or ``None`` for unconstrained.
Units are radians for revolute joints, metres for prismatic. Joints
without explicit limits in the source URDF are ``None`` (typically
continuous revolute joints).
"""
return [j.limits for j in self._kb.joints]
@property
def dispatch_plan(self) -> DispatchPlan:
"""Diagnostic info on which analytical solver was selected and why.
Useful for understanding why a given arm dispatches to a particular
solver (the ``reason`` field is a multi-line user-facing explanation).
"""
return self._plan
@property
def kinbody(self) -> KinBody:
"""The underlying POE-normalised :class:`KinBody`.
Power-user escape hatch for callers who need to invoke a specific
solver, run codegen, or interact with the kinematics primitives
directly. Most users should not need this.
"""
return self._kb
@property
def solver_name(self) -> str:
"""Dotted name of the dispatched solver, e.g. ``"ikgeo.three_parallel"``."""
return self._plan.solver_name
def __repr__(self) -> str:
mod = self._prebuilt
origin = f", prebuilt {mod.__name__.rpartition('.')[2]}" if mod is not None else ""
return (
f"<Manipulator: {self.dof}-DOF, dispatched to "
f"{self._plan.solver_name} (tier {self._plan.tier}){origin}>"
)
def prepare(self) -> None:
"""Do this arm's one-time per-arm preparation now, not on first use.
For an arm dispatched to ``ikgeo.general_6r`` this is the symbolic
Raghavan-Roth derivation that the Python solver needs and the numeric
tensor that the native backend needs, which together take seconds to a
minute on the first call. Both are kept in memory for this process and
written to the per-arm preparation cache on disk, so a later process
on the same machine, or a container started from an image built after
calling this, loads them in milliseconds (``docs/api.md``, "Per-arm
preparation cache"). For every other solver, and for an arm from
:meth:`from_prebuilt` (whose artifact ships this work baked), there is
nothing to prepare and this returns at once.
Calling it again is cheap and changes nothing. It never changes what
:meth:`solve` returns, only when the preparation cost is paid.
"""
if self._prebuilt is not None or self._plan.solver_name != "ikgeo.general_6r":
return
from ssik._native import rr_native_geometry
from ssik.kinematics.poe_to_dh import poe_to_dh
from ssik.solvers.ikgeo._raghavan_roth import _cached_best_leftvar
# The Python solver's AE-3 pick derives every linearity choice; then the
# native tensor for the chosen one (a cache hit skips its derivation).
alpha, a, d = poe_to_dh(self._kb).to_dh_tuple()
_cached_best_leftvar(tuple(alpha.tolist()), tuple(a.tolist()), tuple(d.tolist()))
rr_native_geometry(self._kb)
# ------------------------------------------------------------------
# Forward kinematics
# ------------------------------------------------------------------
def fk(self, q: ArrayLike) -> NDArray[np.float64]:
"""Forward kinematics: return the 4x4 SE(3) pose at config ``q``.
:param q: joint vector of length :attr:`dof`. Float-castable
(list, tuple, numpy array all work).
:returns: 4x4 numpy array. Top-left 3x3 is the end-effector
rotation matrix; column 3 (rows 0..2) is the position.
:raises ValueError: if ``len(q) != dof``.
"""
q_arr = np.asarray(q, dtype=np.float64)
if q_arr.shape != (self.dof,):
raise ValueError(f"fk expected q of shape ({self.dof},), got {q_arr.shape}")
result: NDArray[np.float64] = poe_forward_kinematics(self._kb, q_arr)
return result
# ------------------------------------------------------------------
# Self-motion charts (redundant 7R)
# ------------------------------------------------------------------
def self_motion(self, T_target: ArrayLike, *, native: bool = True) -> SelfMotionManifold:
"""The self-motion manifold at ``T_target``, as charts (redundant 7R only).
Returns a :class:`~ssik.chart.SelfMotionManifold`: every closed-form branch
``q(t)`` of ``FK^-1(T_target)`` with a stable label and its domain, plus
the inverse map ``locate(q)``. See :mod:`ssik.chart` for the per-family
meaning of the label and the redundancy coordinate.
:param native: use the C++ extension when available (default); ``False``
forces the pure-Python reference.
:raises NotImplementedError: when the dispatched solver has no
closed-form chart (6R arms, approximate-class 7R, joint-lock 7R).
"""
from ssik.chart import charts as _charts
return _charts(
self._kb,
T_target,
solver_name=self._plan.solver_name,
policy=self._policy,
native=native,
)
def solve_path(
self,
poses: ArrayLike,
*,
q0: ArrayLike | None = None,
native: bool = True,
max_step: float = 0.5,
) -> PathTrack:
"""Solve a sequence of poses in one call, carrying chart labels along it.
Every branch at ``poses[0]`` (or only those through ``q0``, one start or
``(k, dof)`` of them) is
continued pose to pose by label lookup; see :func:`ssik.chart.track_all`.
Chart-capable arms only, as :meth:`self_motion`.
:param poses: ``(N, 4, 4)`` pose path. A closed path (last pose equal
to the first) exposes the monodromy as ``result.permutation``.
:raises NotImplementedError: when the dispatched solver has no
closed-form chart.
"""
from ssik.chart import track_all
return track_all(
self._kb,
poses,
q0=q0,
solver_name=self._plan.solver_name,
policy=self._policy,
native=native,
max_step=max_step,
)
def tracker(
self,
q0: ArrayLike,
*,
max_joint_speed: float | ArrayLike | None = None,
jump_threshold: float = 0.5,
allow_jump: bool = False,
respect_limits: bool = True,
native: bool = True,
t0: float | None = None,
) -> Tracker:
"""A streaming IK :class:`~ssik.Tracker` on this arm, starting at ``q0``.
Shorthand for ``ssik.Tracker(self, q0, ...)``; the parameters are the
constructor's. Feed it one pose at a time with ``update(T, t)``.
"""
from ssik.tracker import Tracker
return Tracker(
self,
q0,
max_joint_speed=max_joint_speed,
jump_threshold=jump_threshold,
allow_jump=allow_jump,
respect_limits=respect_limits,
native=native,
t0=t0,
)
# ------------------------------------------------------------------
# Inverse kinematics
# ------------------------------------------------------------------
@overload
def solve(
self,
T_target: ArrayLike,
*,
explain: Literal[False] = False,
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> list[Solution]: ...
@overload
def solve(
self,
T_target: ArrayLike,
*,
explain: Literal[True],
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> tuple[list[Solution], Diagnostic]: ...
def solve(
self,
T_target: ArrayLike,
*,
explain: bool = False,
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> list[Solution] | tuple[list[Solution], Diagnostic]:
"""Inverse kinematics: find every ``q`` such that ``fk(q) ≈ T_target``.
:param T_target: 4x4 SE(3) target pose.
:param explain: when ``True``, returns ``(sols, Diagnostic)``
instead of just ``sols``. The diagnostic carries dispatch +
filter attribution for triaging empty-list failures. Default
``False`` preserves the v1.0 return signature.
:param max_solutions: optional cap on returned IKs (post-dedup,
post-limits filter). ``None`` = full redundancy enumeration.
``1`` combined with ``q_seed`` is the trajectory-tracking idiom.
On 7R jointlock arms the cap also short-circuits the lock-sweep
internally for a 10-15x speedup.
:param q_seed: optional length-:attr:`dof` seed. When provided,
returned solutions are sorted by distance from ``q_seed``
(closest first, via ``seed_metric``); with ``max_solutions``
this returns the nearest configs to the seed. On jointlock-7R
arms it also drives the lock-outward-from-seed fast path (#331).
:param seed_metric: distance used to rank against ``q_seed``.
``"wrap_linf"`` (default) minimises the largest single-joint
wrap-to-pi move (holds the branch during tracking); ``"wrap_l2"``
minimises the summed move. Ignored when ``q_seed`` is ``None``.
:param seed_tolerance: optional max per-joint deviation from ``q_seed``
(radians, wrap-to-pi). When set, only solutions with every joint
within ``seed_tolerance`` are returned -- a hard tracking guarantee
that may return an empty list. ``None`` (default) keeps the
best-effort behaviour. Requires ``q_seed``.
:param respect_limits: when ``True`` (default), solutions outside
URDF joint limits are dropped. Pass ``False`` for the raw
geometric set (analysis / debugging), or ``"wrap"`` for the full
geometric set with each joint wrapped into its range where a
``+- 2*pi`` representative exists and nothing dropped.
:param enumerate_windings: when ``True`` (default since v6.0, #562),
a joint whose limits span more than one turn (UR-family
``[-2*pi, 2*pi]``) contributes every
in-limit ``q_i + 2*pi*k`` representative, as a Cartesian product
across such joints. These are finite-limit lifts of the same
geometric branch, not new branches, but they are distinct
admissible configurations that differ in distance and in the
motions available from the robot's current state. Counts rise
accordingly: a UR pose goes from 8 solutions to 256. Pass
``False`` for one representative per geometric branch, which is
the pre-6.0 result set and the cheaper path. Requires
``respect_limits``; ``max_solutions`` and ``q_seed`` still return
the same top-k they would from the complete set.
:param allow_refinement: opt into Newton polish for near-miss
algebraic candidates. Default ``False`` -- the algebraic
path is already at machine precision on tier-0 / SRS arms.
Set ``True`` on tier-2 RR arms to recover edge-case
candidates whose algebraic FK drifts above ``fk_atol``.
Independent of the polish ``general_6r`` always applies to
candidates that already pass (``docs/api.md``, "Polished
general_6r solutions").
:param allow_rescue: when ``True`` (default), if the analytical
path returns no solutions for a target within the arm's reach
(a measure-zero rank-deficient ridge), recover the IK via the
T-perturbation rescue (#319) -- matching the baked ``ssik build``
artifact's coverage so ``from_urdf`` isn't a worse "try before
you build" path (#328). Set ``False`` for a guaranteed
analytical-only result.
:param policy: tolerance policy. Rarely customised. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:param refinement_max_iters: cap on Newton iterations per candidate
when ``allow_refinement=True``.
:param solver_kwargs: forwarded verbatim to the dispatched solver
for power users (``swivel_samples`` for SRS-class,
``linearity_joint`` for RR, etc.).
:returns: list of :class:`Solution`, one per analytical IK branch.
Empty list iff no candidate FK-closed within
``policy.subproblem_numerical`` (or all IKs were filtered by
``respect_limits=True``). Check ``if not sols:`` for
"unreachable target". When ``explain=True``, returns
``(sols, Diagnostic)`` -- inspect ``Diagnostic.summary()``
to attribute empty-list failures.
:raises TypeError: if ``T_target`` or ``q_seed`` is not an array of
real numbers, or ``max_solutions`` is not an integer.
:raises ValueError: if ``T_target`` is not a finite ``(4, 4)`` rigid
transform, ``q_seed`` is not a finite ``(dof,)`` vector, or
``max_solutions`` is negative. The exact rules, the same on every
backend, are in ``docs/api.md`` ("Input validation").
"""
from ssik._solve_inputs import check_solve_inputs
from ssik.postprocess import finalize_solutions as _ps_finalize
# Built by from_prebuilt: answer from the artifact's own solver, which
# bakes per-arm work this path redoes per call (0.089 ms against 3.627
# ms on the Panda, same 502 solutions). The artifact runs the same input
# check as the live path below (ssik._solve_inputs), so a bad T_target
# or q_seed raises identically either way, checked once.
# ``explain`` and extra solver_kwargs have no artifact equivalent, so
# those calls fall through; every artifact takes exactly the fixed
# parameter set used here, which test_prebuilt_namespace pins.
if self._prebuilt is not None and not explain and set(solver_kwargs) <= {"native"}:
sols: list[Solution] = self._prebuilt.solve(
T_target,
max_solutions=max_solutions,
q_seed=q_seed,
respect_limits=respect_limits,
allow_refinement=allow_refinement,
allow_rescue=allow_rescue,
policy=policy,
refinement_max_iters=refinement_max_iters,
seed_metric=seed_metric,
seed_tolerance=seed_tolerance,
enumerate_windings=enumerate_windings,
**solver_kwargs,
)
return sols
T, q_seed_arr, max_solutions = check_solve_inputs(
T_target,
q_seed,
self.dof,
max_solutions=max_solutions,
seed_tolerance=seed_tolerance,
refinement_max_iters=refinement_max_iters,
policy=policy,
)
if max_solutions == 0: # at most 0 solutions: checked, nothing to solve
if not explain:
return []
return [], Diagnostic(
solver_name=self._plan.solver_name,
solver_tier=self._plan.tier,
dispatch_reason=self._plan.reason,
raw_candidates=0,
dropped_by_limits=0,
dropped_by_max_solutions=0,
geometric_branches=0,
winding_representatives=0,
final_count=0,
max_fk_residual=float("nan"),
refinement_engaged=0,
fk_atol=policy.subproblem_numerical,
)
# Filter kwargs by the dispatched solver's signature so callers can
# pass q_seed (or any other not-universally-supported kwarg) without
# tripping TypeError on solvers that don't accept it. Cached at __init__
# (the dispatched solver is fixed there), so this is a set lookup per IK.
params = self._solver_params
kwargs: dict[str, Any] = {"policy": policy}
if "allow_refinement" in params:
kwargs["allow_refinement"] = allow_refinement
if "refinement_max_iters" in params:
kwargs["refinement_max_iters"] = refinement_max_iters
# When respect_limits=True, don't short-circuit the inner solver's
# sweep on max_solutions: the closest-to-seed branch the solver
# picks first may be out-of-limits and the postprocess pass would
# drop it, leaving zero results. Force the full sweep, then
# filter + trim. The user opts out via respect_limits=False. Likewise
# when a seed is given to a solver that does not take one (the SRS
# 7R path): its first solution is not the one nearest the seed, so
# capping it before the seed ranking returned an arbitrary branch --
# 1.5 rad from a seed with a solution 0.118 away on the iiwa14.
if "max_solutions" in params:
seed_unseen = q_seed_arr is not None and "q_seed" not in params
kwargs["max_solutions"] = None if respect_limits or seed_unseen else max_solutions
if q_seed_arr is not None and "q_seed" in params:
kwargs["q_seed"] = q_seed_arr
if "flag_continua" in params:
kwargs["flag_continua"] = True
# Power-user kwargs override our defaults.
kwargs.update(solver_kwargs)
# One-time coverage warning (#328): the cold jointlock-7R path (no
# build-time cached-RR prime) dispatches non-tier-0 inner sub-chains to
# the universal Husty-Pfurner solver, which has coverage gaps the baked
# ``ssik build`` artifact's cached-RR derivations don't. The
# T-perturbation rescue below recovers many such poses, but for arms
# whose cold path is broadly gappy it can't fully match the prebuilt --
# so surface the difference rather than let it be silent.
if self._plan.solver_name == "jointlock.seven_r" and not self._warned_cold_coverage:
self._warned_cold_coverage = True
warnings.warn(
f"{self._plan.solver_name}: solving this 7R arm from a URDF uses the "
"universal Husty-Pfurner inner solver, which can have reduced coverage "
"(and is slower) vs the cached-RR artifact from `ssik build`. For full "
"coverage and speed, build the per-arm artifact once.",
UserWarning,
stacklevel=2,
)
# Internal solver functions still return (sols, is_ls); unwrap the
# tuple at the public-API boundary. `is_ls` is redundant with
# `len(sols) == 0` in every shipped solver -- ssik #238 item 1.
sols, _is_ls = self._solver_module.solve(self._kb, T, **kwargs)
# Bulletproof rescue (#319 / #328): when the analytical path returns
# nothing for a target within the arm's reach, recover the IK via the
# T-perturbation rescue -- the same runtime layer the baked ``ssik
# build`` artifacts apply. Without this, ``from_urdf(...).solve(T)``
# silently has worse coverage than the prebuilt at measure-zero
# rank-deficient ridges. Gated by the reach-sphere (triangle-inequality
# upper bound on ``|T_pos|``) so far-field unreachable targets stay
# cheap. ``allow_rescue=False`` is the guaranteed-analytical escape.
if not sols and allow_rescue:
reach_radius = sum(
float(np.linalg.norm(j.T_left[:3, 3])) + float(np.linalg.norm(j.T_right[:3, 3]))
for j in self._kb.joints
)
if float(np.linalg.norm(T[:3, 3])) <= reach_radius:
from ssik.refinement.rescue import rescue_via_T_perturbation
def _analytic(T_pert: NDArray[np.float64], **rescue_kwargs: Any) -> list[Solution]:
inner: dict[str, Any] = {"policy": policy}
inner.update({k: v for k, v in rescue_kwargs.items() if k in params})
inner_sols, _ = self._solver_module.solve(self._kb, T_pert, **inner)
return list(inner_sols)
# The Newton polish inside the rescue wants a spatial Jacobian, and this
# KinBody has a closed-form one -- the same POE walk the baked prebuilt
# solvers hand in as ``_spatial_jacobian``. Left at None it central-
# differences the FK instead: 2N extra FK calls per iteration for a
# derivative we can write down.
from ssik.refinement import kinbody_jacobian
sols = rescue_via_T_perturbation(
self.fk,
_analytic,
T,
jacobian_fn=lambda q: kinbody_jacobian(self._kb, q),
)
raw_candidate_count = len(sols)
# A solution the solver flagged as a sample of a singular continuum moves
# to the continuum's point under the rule (ssik.continuum,
# docs/api.md#singular-continua), as in every artifact. Only flagged
# solutions pay.
from ssik.continuum import free_of, slide_continua
from ssik.refinement import kinbody_jacobian as _kinbody_jacobian
sols = slide_continua(
sols,
free_of(sols),
self._kb,
T,
fk=self.fk,
jac=lambda q: _kinbody_jacobian(self._kb, q),
q_seed=q_seed_arr,
respect_limits=respect_limits,
dedup_atol=policy.subproblem_dedup,
)
# Cross-arm postprocess pass (the one shared pipeline -- see
# ssik.postprocess.finalize_solutions): solvers that didn't honour kwargs
# natively get limits + seed + truncate applied here so the public API is
# uniform. ``counts`` feeds the Diagnostic below.
counts = {
"dropped_by_limits": 0,
"dropped_by_max_solutions": 0,
"geometric_branches": 0,
"winding_representatives": 0,
}
result: list[Solution] = _ps_finalize(
sols,
self._kb,
respect_limits=respect_limits,
q_seed=q_seed_arr,
seed_metric=seed_metric,
seed_tolerance=seed_tolerance,
max_solutions=max_solutions,
# Manipulator runs the pipeline once, so this is the lifting call --
# but only when the caller wanted limits honoured at all.
enumerate_windings=enumerate_windings and bool(respect_limits),
counts=counts,
T_target=T,
)
dropped_by_limits = counts["dropped_by_limits"]
dropped_by_max_solutions = counts["dropped_by_max_solutions"]
if not explain:
return result
refinement_engaged = sum(1 for s in result if s.refinement_used == "lm")
max_fk = max((s.fk_residual for s in result), default=float("nan"))
diag = Diagnostic(
solver_name=self._plan.solver_name,
solver_tier=self._plan.tier,
dispatch_reason=self._plan.reason,
raw_candidates=raw_candidate_count,
dropped_by_limits=dropped_by_limits,
dropped_by_max_solutions=dropped_by_max_solutions,
geometric_branches=counts["geometric_branches"],
winding_representatives=counts["winding_representatives"],
final_count=len(result),
max_fk_residual=max_fk,
refinement_engaged=refinement_engaged,
fk_atol=policy.subproblem_numerical,
)
return result, diag
|
dof
property
Number of joints in the chain.
solver_name
property
Dotted name of the dispatched solver, e.g. "ikgeo.three_parallel".
kinbody
property
The underlying POE-normalised :class:KinBody.
Power-user escape hatch for callers who need to invoke a specific
solver, run codegen, or interact with the kinematics primitives
directly. Most users should not need this.
from_prebuilt
classmethod
from_prebuilt(
name: str, *, policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY
) -> Manipulator
Build a :class:Manipulator on one of the arms ssik ships, by name.
This is the supported way to reach the chart API on a prebuilt arm
(#587). A shipped artifact exposes solve and fk, but
:meth:self_motion and :meth:solve_path live here, so before this
constructor the only route was the artifact's private baked KinBody --
which docs/semver_policy.md explicitly places outside the semver
contract. ssik.list_arms() is the catalog; this turns a row of it
into an arm you can ask for its self-motion manifold.
:meth:solve on the result is the artifact's solver, not the live
one. That matters: the artifact bakes per-arm work the live path redoes
on every call, and on the Panda the two measure 0.089 ms against 3.627
ms for the same 502 solutions. So the object is not a slower stand-in
for import panda_ik -- it is that same solver, with the chart API
attached. The two cases that cannot be delegated fall back to the live
path: explain=True (artifacts do not build a :class:Diagnostic)
and any solver_kwargs beyond native (artifacts take a fixed
parameter set). Everything else -- :meth:fk, :meth:self_motion,
:meth:solve_path, the introspection properties -- is computed here
from the artifact's own geometry.
Four spellings resolve, tried in that order, so an exact catalog name
can never be shadowed by an abbreviation::
Manipulator.from_prebuilt("franka_panda_ik") # list_arms() name
Manipulator.from_prebuilt("panda_ik") # module basename
Manipulator.from_prebuilt("panda") # either, without _ik
Imports exactly the one artifact named, so this stays as lazy as
list_arms() itself (#421).
Parameters:
| Name |
Type |
Description |
Default |
name
|
str
|
the arm, in any of the spellings above.
|
required
|
policy
|
TolerancePolicy
|
tolerance policy for the dispatcher and for the live
fallback. Defaults to
:data:~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY.
|
DEFAULT_TOLERANCE_POLICY
|
Raises:
| Type |
Description |
ValueError
|
if no arm matches (the message lists near misses),
or if an abbreviation matches several (it lists them all).
|
Source code in src/ssik/manipulator.py
| @classmethod
def from_prebuilt(
cls,
name: str,
*,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> Manipulator:
"""Build a :class:`Manipulator` on one of the arms ssik ships, by name.
This is the supported way to reach the chart API on a prebuilt arm
(#587). A shipped artifact exposes ``solve`` and ``fk``, but
:meth:`self_motion` and :meth:`solve_path` live here, so before this
constructor the only route was the artifact's private baked KinBody --
which ``docs/semver_policy.md`` explicitly places outside the semver
contract. ``ssik.list_arms()`` is the catalog; this turns a row of it
into an arm you can ask for its self-motion manifold.
:meth:`solve` on the result is the **artifact's** solver, not the live
one. That matters: the artifact bakes per-arm work the live path redoes
on every call, and on the Panda the two measure 0.089 ms against 3.627
ms for the same 502 solutions. So the object is not a slower stand-in
for ``import panda_ik`` -- it is that same solver, with the chart API
attached. The two cases that cannot be delegated fall back to the live
path: ``explain=True`` (artifacts do not build a :class:`Diagnostic`)
and any ``solver_kwargs`` beyond ``native`` (artifacts take a fixed
parameter set). Everything else -- :meth:`fk`, :meth:`self_motion`,
:meth:`solve_path`, the introspection properties -- is computed here
from the artifact's own geometry.
Four spellings resolve, tried in that order, so an exact catalog name
can never be shadowed by an abbreviation::
Manipulator.from_prebuilt("franka_panda_ik") # list_arms() name
Manipulator.from_prebuilt("panda_ik") # module basename
Manipulator.from_prebuilt("panda") # either, without _ik
Imports exactly the one artifact named, so this stays as lazy as
``list_arms()`` itself (#421).
:param name: the arm, in any of the spellings above.
:param policy: tolerance policy for the dispatcher and for the live
fallback. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:raises ValueError: if no arm matches (the message lists near misses),
or if an abbreviation matches several (it lists them all).
"""
from ssik.prebuilt import _resolve_arm
_canonical, module = _resolve_arm(name)
arm = cls(module._KB, policy=policy)
arm._prebuilt = module
return arm
|
from_urdf
classmethod
from_urdf(
path: str | Path,
*,
base: str | None = None,
ee: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
xacro_args: dict[str, str] | None = None,
) -> Manipulator
Load a URDF (or xacro) and build a :class:Manipulator for the chain
between base and ee.
The kinematic chain is POE-normalised internally so the dispatcher
can match the arm's topology against the solver roster.
Ingestion mirrors ssik add-arm: the URDF is stripped to a
mesh-free, kinematics-only copy before loading, so vendor URDFs that
reference meshes by an unresolvable package:// path (ABB, FANUC,
...) load without a ROS workspace on disk. Xacro descriptions
(.xacro / *.urdf.xacro, or a .urdf with a xacro namespace)
are expanded automatically via xacrodoc (pip install ssik[xacro]).
Parameters:
| Name |
Type |
Description |
Default |
path
|
str | Path
|
path to the URDF or xacro file.
|
required
|
base
|
str | None
|
name of the base link in the URDF. When omitted, the base
of the longest actuated chain is auto-detected.
|
None
|
ee
|
str | None
|
name of the end-effector link in the URDF. When omitted, the
kinematic flange of the longest actuated chain is auto-detected
(trailing fixed tool0 / gripper frames are skipped).
|
None
|
policy
|
TolerancePolicy
|
tolerance policy. Defaults to
:data:~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY.
|
DEFAULT_TOLERANCE_POLICY
|
xacro_args
|
dict[str, str] | None
|
substitution args for parametrized xacro descriptions
(e.g. {"ur_type": "ur10e"}); ignored for plain URDFs.
|
None
|
Raises:
| Type |
Description |
FileNotFoundError
|
|
ValueError
|
if base or ee are not link names in the URDF,
or (when auto-detecting) if the URDF has no actuated joints.
|
ImportError
|
if the optional dependency urchin is not
installed (pip install ssik[urdf]).
|
Source code in src/ssik/manipulator.py
| @classmethod
def from_urdf(
cls,
path: str | Path,
*,
base: str | None = None,
ee: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
xacro_args: dict[str, str] | None = None,
) -> Manipulator:
"""Load a URDF (or xacro) and build a :class:`Manipulator` for the chain
between ``base`` and ``ee``.
The kinematic chain is POE-normalised internally so the dispatcher
can match the arm's topology against the solver roster.
Ingestion mirrors ``ssik add-arm``: the URDF is stripped to a
mesh-free, kinematics-only copy before loading, so vendor URDFs that
reference meshes by an unresolvable ``package://`` path (ABB, FANUC,
...) load without a ROS workspace on disk. Xacro descriptions
(``.xacro`` / ``*.urdf.xacro``, or a ``.urdf`` with a xacro namespace)
are expanded automatically via ``xacrodoc`` (``pip install ssik[xacro]``).
:param path: path to the URDF or xacro file.
:param base: name of the base link in the URDF. When omitted, the base
of the longest actuated chain is auto-detected.
:param ee: name of the end-effector link in the URDF. When omitted, the
kinematic flange of the longest actuated chain is auto-detected
(trailing fixed ``tool0`` / gripper frames are skipped).
:param policy: tolerance policy. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:param xacro_args: substitution args for parametrized xacro descriptions
(e.g. ``{"ur_type": "ur10e"}``); ignored for plain URDFs.
:raises FileNotFoundError: if ``path`` doesn't exist.
:raises ValueError: if ``base`` or ``ee`` are not link names in the URDF,
or (when auto-detecting) if the URDF has no actuated joints.
:raises ImportError: if the optional dependency ``urchin`` is not
installed (``pip install ssik[urdf]``).
"""
# Imported lazily so the urchin dependency is only required when
# from_urdf is actually called (it's an optional extra).
from ssik._urdf import load_urdf_kinbody_robust, suggest_base_ee
# urchin raises ValueError on missing files; rewrite to the more
# idiomatic FileNotFoundError for the public API contract.
path_obj = Path(path)
if not path_obj.exists():
raise FileNotFoundError(f"URDF file not found: {path_obj}")
# Auto-detect base/ee (longest actuated chain) when not given, so the
# library entry point is as one-click as ``ssik add-arm``.
if base is None or ee is None:
det_base, det_ee, _notes = suggest_base_ee(path, xacro_args)
base = base if base is not None else det_base
ee = ee if ee is not None else det_ee
kb = load_urdf_kinbody_robust(path, base, ee, xacro_args=xacro_args)
return cls(kb, policy=policy)
|
solve
solve(
T_target: ArrayLike,
*,
explain: Literal[False] = False,
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> list[Solution]
solve(
T_target: ArrayLike,
*,
explain: Literal[True],
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> tuple[list[Solution], Diagnostic]
solve(
T_target: ArrayLike,
*,
explain: bool = False,
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> list[Solution] | tuple[list[Solution], Diagnostic]
Inverse kinematics: find every q such that fk(q) ≈ T_target.
Parameters:
| Name |
Type |
Description |
Default |
T_target
|
ArrayLike
|
|
required
|
explain
|
bool
|
when True, returns (sols, Diagnostic)
instead of just sols. The diagnostic carries dispatch +
filter attribution for triaging empty-list failures. Default
False preserves the v1.0 return signature.
|
False
|
max_solutions
|
int | None
|
optional cap on returned IKs (post-dedup,
post-limits filter). None = full redundancy enumeration.
1 combined with q_seed is the trajectory-tracking idiom.
On 7R jointlock arms the cap also short-circuits the lock-sweep
internally for a 10-15x speedup.
|
None
|
q_seed
|
ArrayLike | None
|
optional length-:attr:dof seed. When provided,
returned solutions are sorted by distance from q_seed
(closest first, via seed_metric); with max_solutions
this returns the nearest configs to the seed. On jointlock-7R
arms it also drives the lock-outward-from-seed fast path (#331).
|
None
|
seed_metric
|
str
|
distance used to rank against q_seed.
"wrap_linf" (default) minimises the largest single-joint
wrap-to-pi move (holds the branch during tracking); "wrap_l2"
minimises the summed move. Ignored when q_seed is None.
|
'wrap_linf'
|
seed_tolerance
|
float | None
|
optional max per-joint deviation from q_seed
(radians, wrap-to-pi). When set, only solutions with every joint
within seed_tolerance are returned -- a hard tracking guarantee
that may return an empty list. None (default) keeps the
best-effort behaviour. Requires q_seed.
|
None
|
respect_limits
|
bool | Literal['wrap']
|
when True (default), solutions outside
URDF joint limits are dropped. Pass False for the raw
geometric set (analysis / debugging), or "wrap" for the full
geometric set with each joint wrapped into its range where a
+- 2*pi representative exists and nothing dropped.
|
True
|
enumerate_windings
|
bool
|
when True (default since v6.0, #562),
a joint whose limits span more than one turn (UR-family
[-2*pi, 2*pi]) contributes every
in-limit q_i + 2*pi*k representative, as a Cartesian product
across such joints. These are finite-limit lifts of the same
geometric branch, not new branches, but they are distinct
admissible configurations that differ in distance and in the
motions available from the robot's current state. Counts rise
accordingly: a UR pose goes from 8 solutions to 256. Pass
False for one representative per geometric branch, which is
the pre-6.0 result set and the cheaper path. Requires
respect_limits; max_solutions and q_seed still return
the same top-k they would from the complete set.
|
True
|
allow_refinement
|
bool
|
opt into Newton polish for near-miss
algebraic candidates. Default False -- the algebraic
path is already at machine precision on tier-0 / SRS arms.
Set True on tier-2 RR arms to recover edge-case
candidates whose algebraic FK drifts above fk_atol.
Independent of the polish general_6r always applies to
candidates that already pass (docs/api.md, "Polished
general_6r solutions").
|
False
|
allow_rescue
|
bool
|
when True (default), if the analytical
path returns no solutions for a target within the arm's reach
(a measure-zero rank-deficient ridge), recover the IK via the
T-perturbation rescue (#319) -- matching the baked ssik build
artifact's coverage so from_urdf isn't a worse "try before
you build" path (#328). Set False for a guaranteed
analytical-only result.
|
True
|
policy
|
TolerancePolicy
|
tolerance policy. Rarely customised. Defaults to
:data:~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY.
|
DEFAULT_TOLERANCE_POLICY
|
refinement_max_iters
|
int
|
cap on Newton iterations per candidate
when allow_refinement=True.
|
15
|
solver_kwargs
|
Any
|
forwarded verbatim to the dispatched solver
for power users (swivel_samples for SRS-class,
linearity_joint for RR, etc.).
|
{}
|
Returns:
| Type |
Description |
list[Solution] | tuple[list[Solution], Diagnostic]
|
list of :class:Solution, one per analytical IK branch.
Empty list iff no candidate FK-closed within
policy.subproblem_numerical (or all IKs were filtered by
respect_limits=True). Check if not sols: for
"unreachable target". When explain=True, returns
(sols, Diagnostic) -- inspect Diagnostic.summary()
to attribute empty-list failures.
|
Raises:
| Type |
Description |
TypeError
|
if T_target or q_seed is not an array of
real numbers, or max_solutions is not an integer.
|
ValueError
|
if T_target is not a finite (4, 4) rigid
transform, q_seed is not a finite (dof,) vector, or
max_solutions is negative. The exact rules, the same on every
backend, are in docs/api.md ("Input validation").
|
Source code in src/ssik/manipulator.py
| def solve(
self,
T_target: ArrayLike,
*,
explain: bool = False,
max_solutions: int | None = None,
q_seed: ArrayLike | None = None,
respect_limits: bool | Literal["wrap"] = True,
allow_refinement: bool = False,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
refinement_max_iters: int = 15,
seed_metric: str = "wrap_linf",
seed_tolerance: float | None = None,
allow_rescue: bool = True,
enumerate_windings: bool = True,
**solver_kwargs: Any,
) -> list[Solution] | tuple[list[Solution], Diagnostic]:
"""Inverse kinematics: find every ``q`` such that ``fk(q) ≈ T_target``.
:param T_target: 4x4 SE(3) target pose.
:param explain: when ``True``, returns ``(sols, Diagnostic)``
instead of just ``sols``. The diagnostic carries dispatch +
filter attribution for triaging empty-list failures. Default
``False`` preserves the v1.0 return signature.
:param max_solutions: optional cap on returned IKs (post-dedup,
post-limits filter). ``None`` = full redundancy enumeration.
``1`` combined with ``q_seed`` is the trajectory-tracking idiom.
On 7R jointlock arms the cap also short-circuits the lock-sweep
internally for a 10-15x speedup.
:param q_seed: optional length-:attr:`dof` seed. When provided,
returned solutions are sorted by distance from ``q_seed``
(closest first, via ``seed_metric``); with ``max_solutions``
this returns the nearest configs to the seed. On jointlock-7R
arms it also drives the lock-outward-from-seed fast path (#331).
:param seed_metric: distance used to rank against ``q_seed``.
``"wrap_linf"`` (default) minimises the largest single-joint
wrap-to-pi move (holds the branch during tracking); ``"wrap_l2"``
minimises the summed move. Ignored when ``q_seed`` is ``None``.
:param seed_tolerance: optional max per-joint deviation from ``q_seed``
(radians, wrap-to-pi). When set, only solutions with every joint
within ``seed_tolerance`` are returned -- a hard tracking guarantee
that may return an empty list. ``None`` (default) keeps the
best-effort behaviour. Requires ``q_seed``.
:param respect_limits: when ``True`` (default), solutions outside
URDF joint limits are dropped. Pass ``False`` for the raw
geometric set (analysis / debugging), or ``"wrap"`` for the full
geometric set with each joint wrapped into its range where a
``+- 2*pi`` representative exists and nothing dropped.
:param enumerate_windings: when ``True`` (default since v6.0, #562),
a joint whose limits span more than one turn (UR-family
``[-2*pi, 2*pi]``) contributes every
in-limit ``q_i + 2*pi*k`` representative, as a Cartesian product
across such joints. These are finite-limit lifts of the same
geometric branch, not new branches, but they are distinct
admissible configurations that differ in distance and in the
motions available from the robot's current state. Counts rise
accordingly: a UR pose goes from 8 solutions to 256. Pass
``False`` for one representative per geometric branch, which is
the pre-6.0 result set and the cheaper path. Requires
``respect_limits``; ``max_solutions`` and ``q_seed`` still return
the same top-k they would from the complete set.
:param allow_refinement: opt into Newton polish for near-miss
algebraic candidates. Default ``False`` -- the algebraic
path is already at machine precision on tier-0 / SRS arms.
Set ``True`` on tier-2 RR arms to recover edge-case
candidates whose algebraic FK drifts above ``fk_atol``.
Independent of the polish ``general_6r`` always applies to
candidates that already pass (``docs/api.md``, "Polished
general_6r solutions").
:param allow_rescue: when ``True`` (default), if the analytical
path returns no solutions for a target within the arm's reach
(a measure-zero rank-deficient ridge), recover the IK via the
T-perturbation rescue (#319) -- matching the baked ``ssik build``
artifact's coverage so ``from_urdf`` isn't a worse "try before
you build" path (#328). Set ``False`` for a guaranteed
analytical-only result.
:param policy: tolerance policy. Rarely customised. Defaults to
:data:`~ssik.core.tolerances.DEFAULT_TOLERANCE_POLICY`.
:param refinement_max_iters: cap on Newton iterations per candidate
when ``allow_refinement=True``.
:param solver_kwargs: forwarded verbatim to the dispatched solver
for power users (``swivel_samples`` for SRS-class,
``linearity_joint`` for RR, etc.).
:returns: list of :class:`Solution`, one per analytical IK branch.
Empty list iff no candidate FK-closed within
``policy.subproblem_numerical`` (or all IKs were filtered by
``respect_limits=True``). Check ``if not sols:`` for
"unreachable target". When ``explain=True``, returns
``(sols, Diagnostic)`` -- inspect ``Diagnostic.summary()``
to attribute empty-list failures.
:raises TypeError: if ``T_target`` or ``q_seed`` is not an array of
real numbers, or ``max_solutions`` is not an integer.
:raises ValueError: if ``T_target`` is not a finite ``(4, 4)`` rigid
transform, ``q_seed`` is not a finite ``(dof,)`` vector, or
``max_solutions`` is negative. The exact rules, the same on every
backend, are in ``docs/api.md`` ("Input validation").
"""
from ssik._solve_inputs import check_solve_inputs
from ssik.postprocess import finalize_solutions as _ps_finalize
# Built by from_prebuilt: answer from the artifact's own solver, which
# bakes per-arm work this path redoes per call (0.089 ms against 3.627
# ms on the Panda, same 502 solutions). The artifact runs the same input
# check as the live path below (ssik._solve_inputs), so a bad T_target
# or q_seed raises identically either way, checked once.
# ``explain`` and extra solver_kwargs have no artifact equivalent, so
# those calls fall through; every artifact takes exactly the fixed
# parameter set used here, which test_prebuilt_namespace pins.
if self._prebuilt is not None and not explain and set(solver_kwargs) <= {"native"}:
sols: list[Solution] = self._prebuilt.solve(
T_target,
max_solutions=max_solutions,
q_seed=q_seed,
respect_limits=respect_limits,
allow_refinement=allow_refinement,
allow_rescue=allow_rescue,
policy=policy,
refinement_max_iters=refinement_max_iters,
seed_metric=seed_metric,
seed_tolerance=seed_tolerance,
enumerate_windings=enumerate_windings,
**solver_kwargs,
)
return sols
T, q_seed_arr, max_solutions = check_solve_inputs(
T_target,
q_seed,
self.dof,
max_solutions=max_solutions,
seed_tolerance=seed_tolerance,
refinement_max_iters=refinement_max_iters,
policy=policy,
)
if max_solutions == 0: # at most 0 solutions: checked, nothing to solve
if not explain:
return []
return [], Diagnostic(
solver_name=self._plan.solver_name,
solver_tier=self._plan.tier,
dispatch_reason=self._plan.reason,
raw_candidates=0,
dropped_by_limits=0,
dropped_by_max_solutions=0,
geometric_branches=0,
winding_representatives=0,
final_count=0,
max_fk_residual=float("nan"),
refinement_engaged=0,
fk_atol=policy.subproblem_numerical,
)
# Filter kwargs by the dispatched solver's signature so callers can
# pass q_seed (or any other not-universally-supported kwarg) without
# tripping TypeError on solvers that don't accept it. Cached at __init__
# (the dispatched solver is fixed there), so this is a set lookup per IK.
params = self._solver_params
kwargs: dict[str, Any] = {"policy": policy}
if "allow_refinement" in params:
kwargs["allow_refinement"] = allow_refinement
if "refinement_max_iters" in params:
kwargs["refinement_max_iters"] = refinement_max_iters
# When respect_limits=True, don't short-circuit the inner solver's
# sweep on max_solutions: the closest-to-seed branch the solver
# picks first may be out-of-limits and the postprocess pass would
# drop it, leaving zero results. Force the full sweep, then
# filter + trim. The user opts out via respect_limits=False. Likewise
# when a seed is given to a solver that does not take one (the SRS
# 7R path): its first solution is not the one nearest the seed, so
# capping it before the seed ranking returned an arbitrary branch --
# 1.5 rad from a seed with a solution 0.118 away on the iiwa14.
if "max_solutions" in params:
seed_unseen = q_seed_arr is not None and "q_seed" not in params
kwargs["max_solutions"] = None if respect_limits or seed_unseen else max_solutions
if q_seed_arr is not None and "q_seed" in params:
kwargs["q_seed"] = q_seed_arr
if "flag_continua" in params:
kwargs["flag_continua"] = True
# Power-user kwargs override our defaults.
kwargs.update(solver_kwargs)
# One-time coverage warning (#328): the cold jointlock-7R path (no
# build-time cached-RR prime) dispatches non-tier-0 inner sub-chains to
# the universal Husty-Pfurner solver, which has coverage gaps the baked
# ``ssik build`` artifact's cached-RR derivations don't. The
# T-perturbation rescue below recovers many such poses, but for arms
# whose cold path is broadly gappy it can't fully match the prebuilt --
# so surface the difference rather than let it be silent.
if self._plan.solver_name == "jointlock.seven_r" and not self._warned_cold_coverage:
self._warned_cold_coverage = True
warnings.warn(
f"{self._plan.solver_name}: solving this 7R arm from a URDF uses the "
"universal Husty-Pfurner inner solver, which can have reduced coverage "
"(and is slower) vs the cached-RR artifact from `ssik build`. For full "
"coverage and speed, build the per-arm artifact once.",
UserWarning,
stacklevel=2,
)
# Internal solver functions still return (sols, is_ls); unwrap the
# tuple at the public-API boundary. `is_ls` is redundant with
# `len(sols) == 0` in every shipped solver -- ssik #238 item 1.
sols, _is_ls = self._solver_module.solve(self._kb, T, **kwargs)
# Bulletproof rescue (#319 / #328): when the analytical path returns
# nothing for a target within the arm's reach, recover the IK via the
# T-perturbation rescue -- the same runtime layer the baked ``ssik
# build`` artifacts apply. Without this, ``from_urdf(...).solve(T)``
# silently has worse coverage than the prebuilt at measure-zero
# rank-deficient ridges. Gated by the reach-sphere (triangle-inequality
# upper bound on ``|T_pos|``) so far-field unreachable targets stay
# cheap. ``allow_rescue=False`` is the guaranteed-analytical escape.
if not sols and allow_rescue:
reach_radius = sum(
float(np.linalg.norm(j.T_left[:3, 3])) + float(np.linalg.norm(j.T_right[:3, 3]))
for j in self._kb.joints
)
if float(np.linalg.norm(T[:3, 3])) <= reach_radius:
from ssik.refinement.rescue import rescue_via_T_perturbation
def _analytic(T_pert: NDArray[np.float64], **rescue_kwargs: Any) -> list[Solution]:
inner: dict[str, Any] = {"policy": policy}
inner.update({k: v for k, v in rescue_kwargs.items() if k in params})
inner_sols, _ = self._solver_module.solve(self._kb, T_pert, **inner)
return list(inner_sols)
# The Newton polish inside the rescue wants a spatial Jacobian, and this
# KinBody has a closed-form one -- the same POE walk the baked prebuilt
# solvers hand in as ``_spatial_jacobian``. Left at None it central-
# differences the FK instead: 2N extra FK calls per iteration for a
# derivative we can write down.
from ssik.refinement import kinbody_jacobian
sols = rescue_via_T_perturbation(
self.fk,
_analytic,
T,
jacobian_fn=lambda q: kinbody_jacobian(self._kb, q),
)
raw_candidate_count = len(sols)
# A solution the solver flagged as a sample of a singular continuum moves
# to the continuum's point under the rule (ssik.continuum,
# docs/api.md#singular-continua), as in every artifact. Only flagged
# solutions pay.
from ssik.continuum import free_of, slide_continua
from ssik.refinement import kinbody_jacobian as _kinbody_jacobian
sols = slide_continua(
sols,
free_of(sols),
self._kb,
T,
fk=self.fk,
jac=lambda q: _kinbody_jacobian(self._kb, q),
q_seed=q_seed_arr,
respect_limits=respect_limits,
dedup_atol=policy.subproblem_dedup,
)
# Cross-arm postprocess pass (the one shared pipeline -- see
# ssik.postprocess.finalize_solutions): solvers that didn't honour kwargs
# natively get limits + seed + truncate applied here so the public API is
# uniform. ``counts`` feeds the Diagnostic below.
counts = {
"dropped_by_limits": 0,
"dropped_by_max_solutions": 0,
"geometric_branches": 0,
"winding_representatives": 0,
}
result: list[Solution] = _ps_finalize(
sols,
self._kb,
respect_limits=respect_limits,
q_seed=q_seed_arr,
seed_metric=seed_metric,
seed_tolerance=seed_tolerance,
max_solutions=max_solutions,
# Manipulator runs the pipeline once, so this is the lifting call --
# but only when the caller wanted limits honoured at all.
enumerate_windings=enumerate_windings and bool(respect_limits),
counts=counts,
T_target=T,
)
dropped_by_limits = counts["dropped_by_limits"]
dropped_by_max_solutions = counts["dropped_by_max_solutions"]
if not explain:
return result
refinement_engaged = sum(1 for s in result if s.refinement_used == "lm")
max_fk = max((s.fk_residual for s in result), default=float("nan"))
diag = Diagnostic(
solver_name=self._plan.solver_name,
solver_tier=self._plan.tier,
dispatch_reason=self._plan.reason,
raw_candidates=raw_candidate_count,
dropped_by_limits=dropped_by_limits,
dropped_by_max_solutions=dropped_by_max_solutions,
geometric_branches=counts["geometric_branches"],
winding_representatives=counts["winding_representatives"],
final_count=len(result),
max_fk_residual=max_fk,
refinement_engaged=refinement_engaged,
fk_atol=policy.subproblem_numerical,
)
return result, diag
|
prepare
Do this arm's one-time per-arm preparation now, not on first use.
For an arm dispatched to ikgeo.general_6r this is the symbolic
Raghavan-Roth derivation that the Python solver needs and the numeric
tensor that the native backend needs, which together take seconds to a
minute on the first call. Both are kept in memory for this process and
written to the per-arm preparation cache on disk, so a later process
on the same machine, or a container started from an image built after
calling this, loads them in milliseconds (docs/api.md, "Per-arm
preparation cache"). For every other solver, and for an arm from
:meth:from_prebuilt (whose artifact ships this work baked), there is
nothing to prepare and this returns at once.
Calling it again is cheap and changes nothing. It never changes what
:meth:solve returns, only when the preparation cost is paid.
Source code in src/ssik/manipulator.py
| def prepare(self) -> None:
"""Do this arm's one-time per-arm preparation now, not on first use.
For an arm dispatched to ``ikgeo.general_6r`` this is the symbolic
Raghavan-Roth derivation that the Python solver needs and the numeric
tensor that the native backend needs, which together take seconds to a
minute on the first call. Both are kept in memory for this process and
written to the per-arm preparation cache on disk, so a later process
on the same machine, or a container started from an image built after
calling this, loads them in milliseconds (``docs/api.md``, "Per-arm
preparation cache"). For every other solver, and for an arm from
:meth:`from_prebuilt` (whose artifact ships this work baked), there is
nothing to prepare and this returns at once.
Calling it again is cheap and changes nothing. It never changes what
:meth:`solve` returns, only when the preparation cost is paid.
"""
if self._prebuilt is not None or self._plan.solver_name != "ikgeo.general_6r":
return
from ssik._native import rr_native_geometry
from ssik.kinematics.poe_to_dh import poe_to_dh
from ssik.solvers.ikgeo._raghavan_roth import _cached_best_leftvar
# The Python solver's AE-3 pick derives every linearity choice; then the
# native tensor for the chosen one (a cache hit skips its derivation).
alpha, a, d = poe_to_dh(self._kb).to_dh_tuple()
_cached_best_leftvar(tuple(alpha.tolist()), tuple(a.tolist()), tuple(d.tolist()))
rr_native_geometry(self._kb)
|
fk
fk(q: ArrayLike) -> NDArray[np.float64]
Forward kinematics: return the 4x4 SE(3) pose at config q.
Parameters:
| Name |
Type |
Description |
Default |
q
|
ArrayLike
|
joint vector of length :attr:dof. Float-castable
(list, tuple, numpy array all work).
|
required
|
Returns:
| Type |
Description |
NDArray[float64]
|
4x4 numpy array. Top-left 3x3 is the end-effector
rotation matrix; column 3 (rows 0..2) is the position.
|
Raises:
| Type |
Description |
ValueError
|
|
Source code in src/ssik/manipulator.py
| def fk(self, q: ArrayLike) -> NDArray[np.float64]:
"""Forward kinematics: return the 4x4 SE(3) pose at config ``q``.
:param q: joint vector of length :attr:`dof`. Float-castable
(list, tuple, numpy array all work).
:returns: 4x4 numpy array. Top-left 3x3 is the end-effector
rotation matrix; column 3 (rows 0..2) is the position.
:raises ValueError: if ``len(q) != dof``.
"""
q_arr = np.asarray(q, dtype=np.float64)
if q_arr.shape != (self.dof,):
raise ValueError(f"fk expected q of shape ({self.dof},), got {q_arr.shape}")
result: NDArray[np.float64] = poe_forward_kinematics(self._kb, q_arr)
return result
|
self_motion
self_motion(T_target: ArrayLike, *, native: bool = True) -> SelfMotionManifold
The self-motion manifold at T_target, as charts (redundant 7R only).
Returns a :class:~ssik.chart.SelfMotionManifold: every closed-form branch
q(t) of FK^-1(T_target) with a stable label and its domain, plus
the inverse map locate(q). See :mod:ssik.chart for the per-family
meaning of the label and the redundancy coordinate.
Parameters:
| Name |
Type |
Description |
Default |
native
|
bool
|
use the C++ extension when available (default); False
forces the pure-Python reference.
|
True
|
Raises:
| Type |
Description |
NotImplementedError
|
when the dispatched solver has no
closed-form chart (6R arms, approximate-class 7R, joint-lock 7R).
|
Source code in src/ssik/manipulator.py
| def self_motion(self, T_target: ArrayLike, *, native: bool = True) -> SelfMotionManifold:
"""The self-motion manifold at ``T_target``, as charts (redundant 7R only).
Returns a :class:`~ssik.chart.SelfMotionManifold`: every closed-form branch
``q(t)`` of ``FK^-1(T_target)`` with a stable label and its domain, plus
the inverse map ``locate(q)``. See :mod:`ssik.chart` for the per-family
meaning of the label and the redundancy coordinate.
:param native: use the C++ extension when available (default); ``False``
forces the pure-Python reference.
:raises NotImplementedError: when the dispatched solver has no
closed-form chart (6R arms, approximate-class 7R, joint-lock 7R).
"""
from ssik.chart import charts as _charts
return _charts(
self._kb,
T_target,
solver_name=self._plan.solver_name,
policy=self._policy,
native=native,
)
|
solve_path
solve_path(
poses: ArrayLike,
*,
q0: ArrayLike | None = None,
native: bool = True,
max_step: float = 0.5,
) -> PathTrack
Solve a sequence of poses in one call, carrying chart labels along it.
Every branch at poses[0] (or only those through q0, one start or
(k, dof) of them) is
continued pose to pose by label lookup; see :func:ssik.chart.track_all.
Chart-capable arms only, as :meth:self_motion.
Parameters:
| Name |
Type |
Description |
Default |
poses
|
ArrayLike
|
(N, 4, 4) pose path. A closed path (last pose equal
to the first) exposes the monodromy as result.permutation.
|
required
|
Raises:
| Type |
Description |
NotImplementedError
|
when the dispatched solver has no
closed-form chart.
|
Source code in src/ssik/manipulator.py
| def solve_path(
self,
poses: ArrayLike,
*,
q0: ArrayLike | None = None,
native: bool = True,
max_step: float = 0.5,
) -> PathTrack:
"""Solve a sequence of poses in one call, carrying chart labels along it.
Every branch at ``poses[0]`` (or only those through ``q0``, one start or
``(k, dof)`` of them) is
continued pose to pose by label lookup; see :func:`ssik.chart.track_all`.
Chart-capable arms only, as :meth:`self_motion`.
:param poses: ``(N, 4, 4)`` pose path. A closed path (last pose equal
to the first) exposes the monodromy as ``result.permutation``.
:raises NotImplementedError: when the dispatched solver has no
closed-form chart.
"""
from ssik.chart import track_all
return track_all(
self._kb,
poses,
q0=q0,
solver_name=self._plan.solver_name,
policy=self._policy,
native=native,
max_step=max_step,
)
|
tracker
tracker(
q0: ArrayLike,
*,
max_joint_speed: float | ArrayLike | None = None,
jump_threshold: float = 0.5,
allow_jump: bool = False,
respect_limits: bool = True,
native: bool = True,
t0: float | None = None,
) -> Tracker
A streaming IK :class:~ssik.Tracker on this arm, starting at q0.
Shorthand for ssik.Tracker(self, q0, ...); the parameters are the
constructor's. Feed it one pose at a time with update(T, t).
Source code in src/ssik/manipulator.py
| def tracker(
self,
q0: ArrayLike,
*,
max_joint_speed: float | ArrayLike | None = None,
jump_threshold: float = 0.5,
allow_jump: bool = False,
respect_limits: bool = True,
native: bool = True,
t0: float | None = None,
) -> Tracker:
"""A streaming IK :class:`~ssik.Tracker` on this arm, starting at ``q0``.
Shorthand for ``ssik.Tracker(self, q0, ...)``; the parameters are the
constructor's. Feed it one pose at a time with ``update(T, t)``.
"""
from ssik.tracker import Tracker
return Tracker(
self,
q0,
max_joint_speed=max_joint_speed,
jump_threshold=jump_threshold,
allow_jump=allow_jump,
respect_limits=respect_limits,
native=native,
t0=t0,
)
|
Every public entry point that takes a pose or a joint vector checks it once,
before it picks a backend: each prebuilt artifact's solve and fk,
Manipulator.solve, Manipulator.self_motion, Manipulator.solve_path,
Tracker (its q0, and the target of every update) and the ssik.teleop
frame helpers. A malformed call raises the same exception, with the same message, on
native=True and native=False, and the native extension only ever sees
well-formed input. The implementation is ssik._solve_inputs.
Arrays (T_target, q_seed, an artifact's fk(q), solve_path's
poses and q0). Anything numpy converts to an array of real numbers is
accepted and converted to a C-contiguous float64 array: float64, float32
and integer arrays, lists and tuples, Fortran-ordered arrays and strided views,
and object arrays whose elements are all real numbers. The caller's array is
never modified. Rejected:
| Input |
Exception |
bool, complex, string, bytes, datetime or structured dtype; an object array with any element that is not a real number (None, a string) |
TypeError |
Ragged nesting ([[1, 0, 0, 0], [0, 1, 0]]) |
ValueError |
Wrong shape: T_target not (4, 4), q_seed or fk's q not (dof,), poses not (N, 4, 4), q0 not (dof,) or (k, dof) |
ValueError |
A NaN or infinite entry in T_target, q_seed, poses or q0 |
ValueError |
Converting a rejected dtype would invent or drop information: a complex
target's imaginary part, a string's parse. fk(q) checks only the shape, so a
NaN joint value gives a NaN pose, as Manipulator.fk does.
Rigid targets. T_target (and each pose of a path) must be a rigid
transform. With R its rotation block and tol =
policy.subproblem_numerical (1e-5 by default), it is rejected with a
ValueError when
||R^T R - I||_F > max(3 * tol, 1e-12),det(R) <= 0 (a reflection), or- its bottom row is further than
max(tol, 1e-12) (Euclidean) from
[0, 0, 0, 1].
The tolerance rejects only a target no configuration can reach. Every solver
accepts a configuration only when ||FK(q) - T_target||_F is within tol or
tighter, and FK(q) is rigid, so that residual is at least the distance d
from R to the nearest rotation. For d < 1, ||R^T R - I||_F <= 3 d: each
singular value s of R is within d of 1, and
|s^2 - 1| = |s - 1| (s + 1) <= 3 |s - 1|. A target past 3 * tol therefore
has d > tol and could only ever have returned []. The bottom row adds its
distance to the residual directly. The 1e-12 floor applies when a policy is
tighter than round-off: computing R^T R itself rounds by about 1e-15, which
must not reject a rigid target.
A target inside the tolerance is solved as given, never projected onto SO(3),
and fk_residual is measured against it. Float64 round-off from upstream pose
arithmetic (around 1e-15) and a float32 pose (around 1e-7) are well
inside. A target off SO(3) by more than one arm's own acceptance gate still
returns no solutions there, as before: the three_parallel artifacts accept at
1e-7 and the exact seven_r.spherical_shoulder arms at 1e-10, so a
float32 pose can return [] on those.
Options. These are checked with the arrays:
| Option |
Rule |
Otherwise |
max_solutions |
None or an integer >= 0 (Python or numpy) |
TypeError for a non-integer, ValueError below 0 |
seed_tolerance |
None, or a real number that is not NaN, and only with q_seed |
TypeError for a non-number, ValueError for NaN or without q_seed |
refinement_max_iters |
an integer >= 0 |
TypeError for a non-integer, ValueError below 0 |
seed_metric |
"wrap_linf" or "wrap_l2" when q_seed is given |
ValueError |
max_solutions means "at most this many solutions". A cap of 0 returns []
on every backend without solving, once every other input has passed its checks
(as heapq.nsmallest(0, ...) does), so a caller computing a remaining budget
can pass it unchanged. A negative cap used to mean different things on
different backends (all solutions, none, or an error) and is rejected. A cap
larger than any result is no cap on either backend. The
rules for the remaining options (respect_limits, enumerate_windings,
allow_rescue, policy, a bool given as an integer) are tracked in #575.
The native extension. ssik._ssik_native is internal and its functions
are not part of the semver contract, but none of them reads memory it was not
given. Each checks every array's shape and every index argument's range, and
the target's and seed's finiteness, before it reads them, and raises
ValueError otherwise. In C++, ssik::Solver checks its data and its input
in the same way and throws std::invalid_argument; the *_artifact_solve
functions do not, and their preconditions are in
cpp/README.md.
The native backend
solve(..., native=True), the default on every prebuilt arm, runs the arm's
solver family in C++ through ssik::Solver (ssik_cpp/solve.hpp), built from
ssik.cpp.solver_data(arm). The families are ssik.cpp.NATIVE_FAMILIES.
A call the native backend cannot honour (a non-default policy,
allow_refinement=True, explain=True, or a family without a native solver)
runs the Python backend instead; so does every call when the extension is not
installed (Windows wheels). solve() on an arm built at runtime runs the
Python backend; solver_data still serves such an arm to a C++ consumer.
C++ consumers get the same function. For d = ssik.cpp.solver_data(arm)
and options that ssik::ArtifactParams can express (the default policy,
allow_refinement=False), ssik::Solver(d).solve<N>(T, p) returns the same
list, in the same order, as the native backend for that data. For a prebuilt
arm that is arm.solve(T, native=True, ...). The wheel's "Native wheel build"
CI job checks this from a C++ program built against the installed wheel, on
one arm of every native family and on runtime-built arms
(cpp/examples/wheel_consumer/).
Native against Python. The two backends return the same solutions up to
the following contract, gated per shipped arm by tests/test_native_parity.py
(rules a-g in tests/_native_parity.py), on in-limits, near-limit,
near-singular and at-pi poses:
| family |
contract |
| every family |
every native solution closes FK within the policy's subproblem_numerical and, with respect_limits=True, lies within the joint limits; native is never empty where Python is not |
6R (ikgeo.*) |
the two sets are equal within the match tolerance, in both directions, except in the documented excused classes (a solution one backend's rescue or in-limits resolver adds, or a solution just across a limit) |
7R (seven_r.*, jointlock.seven_r) |
native covers every Python solution, except at a kinematic singularity, where the backends sample a continuum at their own points and only non-emptiness holds |
Redundant 7R families are sampling solvers: they return points of the
self-motion manifold at fixed internal sample schedules, so native and Python
need not return the same points, only the coverage above.
Per-arm preparation cache
An arm built at runtime (from_urdf, from_mjcf, from_axes, ...) that
dispatches to ikgeo.general_6r needs a symbolic Raghavan-Roth derivation
before its first solve: seconds to a minute of sympy per arm (about 28 s for
the JACO 2). ssik keeps that work on disk, so it is paid once per machine, not
once per process.
What is cached. Two kinds of entry, both functions of the arm's DH
geometry alone:
- the symbolic derivation for one linearity choice, which the Python solver
turns back into callables when it loads it (the first solve in a later
process takes about 1 s on the JACO 2, not 28 s);
- the numeric coefficient tensor that the native backend evaluates (loading it
takes milliseconds).
Nothing else is cached, and an arm on any other solver has nothing to cache.
Where. In $SSIK_DERIVATION_CACHE when it is set, else in
$XDG_CACHE_HOME/ssik/rr-derivations, else in ~/.cache/ssik/rr-derivations.
SSIK_DERIVATION_CACHE=off (or an empty value) disables the cache: nothing is
read or written, and every process derives afresh. Derivations are stored as
.pkl files and tensors as .npz files. File names and contents are not part
of the contract.
When an entry misses. An entry is keyed on the exact DH values, the
linearity choice, the source code that produces it, and the versions of
sympy, mpmath and (for tensors) numpy. Any change to these makes a lookup miss,
so the cache never returns a result that differs from a fresh derivation: a
hit is bit-identical to what the derivation would compute. Upgrading ssik
therefore usually re-derives once. Old entries stay on disk until you delete
them. An entry that cannot be read, is truncated or damaged, or was written
for another key is a miss. It is derived again and replaced.
Safe across processes. Entries are written to a temporary file and moved
into place with an atomic rename, so concurrent processes can share one
directory and never read a partial entry. A read-only or full directory is not
an error: the result is computed and used, just not stored. You can delete the
directory at any time.
Preparing ahead of time. Manipulator.prepare() does an arm's one-time
preparation immediately rather than on first use. On a general_6r arm it
derives (or loads) what both backends need, keeps it in memory and writes it to
the cache. It returns None, does nothing on other arms or on
from_prebuilt arms, and calling it again is cheap. A deployment that starts
every process from a fresh container image can run it during the image build,
with the cache directory inside the image:
# At image build time, with SSIK_DERIVATION_CACHE pointing into the image.
import ssik
ssik.Manipulator.from_mjcf("robot.xml", base="base", ee="ee_site").prepare()
The same environment variable must point at that directory when the
container runs. An artifact from ssik build needs none of this: it ships the
same work baked into the artifact.
Per-call return: Solution
A single analytical IK solution.
Parameters:
| Name |
Type |
Description |
Default |
q
|
NDArray[float64]
|
joint-angle vector. Length matches the chain's DOF.
|
required
|
fk_residual
|
float
|
||FK(q) - T_target||_F at the returned q. The
solver measures it; when post-processing snaps a value onto pi or
clamps it onto a joint limit, it is measured again at the moved value
(docs/api.md#joint-limits). The caller can compare against any
target tolerance; the solver's own fk_atol was a filter, not a
contract on the value reported here.
|
required
|
refinement_used
|
RefinementMode
|
"none" if the solution passed the solver's
acceptance gate on its own (general_6r then polishes it to machine
precision on the same branch, see docs/api.md); "lm" if it
failed the gate and Levenberg-Marquardt refinement rescued it (when
allow_refinement=True) or the T-perturbation rescue recovered it.
|
'none'
|
Source code in src/ssik/core/solution.py
| @dataclass(frozen=True)
class Solution:
"""A single analytical IK solution.
:param q: joint-angle vector. Length matches the chain's DOF.
:param fk_residual: ``||FK(q) - T_target||_F`` at the returned ``q``. The
solver measures it; when post-processing snaps a value onto ``pi`` or
clamps it onto a joint limit, it is measured again at the moved value
(``docs/api.md#joint-limits``). The caller can compare against any
target tolerance; the solver's own ``fk_atol`` was a *filter*, not a
contract on the value reported here.
:param refinement_used: ``"none"`` if the solution passed the solver's
acceptance gate on its own (``general_6r`` then polishes it to machine
precision on the same branch, see ``docs/api.md``); ``"lm"`` if it
failed the gate and Levenberg-Marquardt refinement rescued it (when
``allow_refinement=True``) or the T-perturbation rescue recovered it.
"""
q: NDArray[np.float64]
fk_residual: float
refinement_used: RefinementMode = "none"
|
Polished general_6r solutions
general_6r (Raghavan-Roth, including the jointlock sub-solves of Rizon 4 and
Rizon 10) finds its candidates with an ill-conditioned eigen-solve and accepts
one when its FK residual is within TolerancePolicy.subproblem_numerical
(1e-5). An accepted angle is then only as accurate as that residual over the
Jacobian's smallest singular value. So, on both backends and by default, every
accepted candidate gets a few Newton steps on the true FK before duplicates are
merged. The polished point replaces the candidate only if its residual drops to
1e-12 and it stays within twice the first Newton step of the candidate, the
region where Newton's method provably converges to the candidate's own root,
and it keeps every value (or 2*pi winding) that the candidate had within
round-off (1e-9 rad, the floor of the joint-limit band) of a
limit. Otherwise the candidate is returned exactly as the solver produced it,
which happens near a singularity or where polishing would cross a limit. Polish
never changes which candidates are accepted, never takes a value within
round-off of a limit out of it, and never moves one to another branch. A
candidate whose value lies further across a limit, but within its own
error band, can be polished to where its root really is, and the
limit pass then judges the polished point by its own, smaller band.
refinement_used stays "none". The definition is in
ssik.refinement.polish.
This is not allow_refinement. That option (off by default) tries to rescue a
candidate that failed the acceptance gate, and tags a candidate it rescues
"lm". It stops as soon as the residual is within the gate, so a rescued
solution is only as accurate as the gate.
Singular continua
At a singular pose a 6R arm can have a one-parameter family of solutions, a
continuum, instead of isolated ones. The common case is a wrist whose
outer axes line up (sin q5 = 0): only q4 + q6 (or q4 - q6) is fixed, and
every split of it reaches the target. A solver sees such a family only as
whatever samples its arithmetic lands on. On both backends, and for every 6R
family (three_parallel, spherical_two_parallel, general_6r), solve()
returns:
- Seeded (
q_seed given): from each continuum, the point nearest the seed
within the joint limits. "Nearest" is along the continuum: the point where
the continuum's tangent is orthogonal to the seed offset (every joint
compared on the circle), or, if that point is outside the limits, the
in-limit point reached by the shortest walk along the continuum from it.
The usual seed ranking then orders it among the other solutions, so a
configuration on a continuum returns itself as the nearest solution.
- Unseeded: one representative per continuum, the point whose free
joint is at
0 (modulo 2*pi). The free joint is q6 for a locked wrist
of the closed-form families and, for general_6r, the highest-index joint
the continuum moves (by at least a tenth of its unit tangent). If that point
is outside the limits, the representative is the in-limit point nearest it
along the continuum. If the continuum does not reach it (a UR-type arm whose
elbow cannot follow), the representative is where the walk toward it stops,
at the turn of the continuum nearest it.
With respect_limits=False (or "wrap") the limits play no part: the point
is the seed's nearest, or the one with the free joint at 0. A continuum with
no in-limit point is dropped by the limit pass, as any out-of-limit solution
is.
Only a solution the solver flags pays for this, and nothing measures the
Jacobian of every solution:
- The closed-form cores measure how close the wrist is to locked: the sine
between the wrist-roll axis and the pitched outer wrist axis. Within
1e-4
they flag the candidate. That covers the precision to which each core
resolves the wrist pitch at an exact lock, a double root (within 5.4e-8 of
it through spherical_two_parallel's closed form, 5.6e-6 through
three_parallel's numerical SP6, over 200 exactly locked poses per arm), so
both backends flag the same continuum whichever side of the lock their
arithmetic lands. Both keep their wrist angles (on a lock they are a point of
the continuum) unless they divide zero by zero (sine within 1e-9, the
rule the spherical-shoulder 7R core already uses) or the angles are unusable:
spherical_two_parallel's miss the target's wrist rotation by more than
1e-4; three_parallel's cannot give a candidate within its 1e-7 FK gate
(they miss the wrist rotation by more than that, or leave the elbow out of
reach where another point of the lock reaches it). Then they split the lock.
three_parallel, whose numerical wrist pitch is too coarse to read the
angles there (near a lock each SP1 angle alone is accurate only to about
eps / sine, though their sum is fixed), makes its shoulder and pitch exact for the lock and sets q6 to the seed's value
(0 unseeded) and to that plus pi, each moved to the nearest of 64 values
around the circle at which the elbow can reach, reading the other wrist
angle off the target. spherical_two_parallel, on the lock, sets q6 to
the seed's value (0 unseeded), and near it returns the two branches on
either side.
general_6r flags an accepted candidate whose Jacobian may be rank
deficient: the Frobenius condition number of J^T J + 1e-9 I, the matrix
its polish solves with anyway, is at least 5e7. That bounds
(sigma_max / sigma_min)^2 from above, so it misses none, and the slide
confirms with an SVD.
A flagged solution moves only along the directions where J is rank
deficient (sigma <= 1e-4 sigma_max), each step corrected back onto the
solution set by Gauss-Newton steps that never move along them, and the moved
point is kept only if it closes FK to 1e-10; otherwise the solution stays as
the solver produced it. Every point of an exact continuum closes FK to
round-off, so there the rule's point is returned on both backends. At a pose
only near a continuum, where the family closes FK approximately, an isolated
solution stays where the solver put it, unless the rule's point is itself an
exact solution: a seed that is one is returned. Samples that reach the same
point merge. fk_residual is the moved point's. refinement_used keeps the
solver's tag. The definition, with every constant, is in ssik.continuum; the
native backend runs the same one (cpp/include/ssik_cpp/continuum.hpp).
Exactly singular poses get different values from earlier releases, which
returned whatever sample the arithmetic produced, or none.
Diagnostic record: Diagnostic
Returned alongside the solution list when solve(T, explain=True).
Per-call diagnostic record from solve(T, explain=True).
All counts are post-filter aggregates -- the solver's own
enumeration cost (number of raw algebraic candidates before any
filtering) is in raw_candidates; each subsequent filter is a
delta. final_count == len(returned_solutions).
The fields are deliberately data-only (no methods that re-run the
solve, no live references to the kinbody). A diagnostic is a
snapshot, safe to log / serialise.
Source code in src/ssik/core/diagnostic.py
| @dataclass(frozen=True)
class Diagnostic:
"""Per-call diagnostic record from ``solve(T, explain=True)``.
All counts are post-filter aggregates -- the solver's own
enumeration cost (number of raw algebraic candidates before any
filtering) is in ``raw_candidates``; each subsequent filter is a
delta. ``final_count == len(returned_solutions)``.
The fields are deliberately data-only (no methods that re-run the
solve, no live references to the kinbody). A diagnostic is a
snapshot, safe to log / serialise.
"""
solver_name: str
"""Dispatched solver, e.g. ``"ikgeo.three_parallel"``."""
solver_tier: int
"""0 = closed-form, 1 = univariate search, 2 = bivariate search."""
dispatch_reason: str
"""Human-readable explanation of why the dispatcher picked this solver.
Often the most useful field for 'why did ssik refuse my arm?' triage."""
raw_candidates: int
"""Raw analytical candidate count returned by the inner solver, before
any post-processing. ``0`` means the solver itself returned nothing --
pose is outside the analytical reach or the solver's preconditions
aren't met. ``>0`` with ``final_count == 0`` means every candidate was
filtered (limits, etc.)."""
dropped_by_limits: int
"""Candidates that fell outside URDF joint limits (after the ``q ± 2π``
rescue pass). ``0`` when ``respect_limits=False`` was passed."""
dropped_by_max_solutions: int
"""Surviving candidates truncated by the ``max_solutions`` cap."""
final_count: int
"""Number of solutions in the returned list."""
max_fk_residual: float
"""Worst ``fk_residual`` among returned solutions. ``nan`` when
``final_count == 0``."""
refinement_engaged: int
"""How many candidates ran through Levenberg-Marquardt polish (when
``allow_refinement=True`` was passed). ``0`` when refinement was off."""
fk_atol: float
"""The FK-closure threshold the solver used. Useful when the user
customised the tolerance policy and wants the live value back."""
geometric_branches: int = 0
"""Distinct geometric IK branches that survived the limit filter, before
winding enumeration (#562). This is the count that pre-6.0 releases
returned."""
winding_representatives: int = 0
"""Size of the complete in-limit set after winding enumeration -- the
finite-limit lifts (``q + 2*pi*k``) of those branches on joints whose limits
span more than one turn. Equal to ``geometric_branches`` when the arm has no
such joint or ``enumerate_windings=False``. Reported separately so lifts are
never mistaken for additional geometric branches, and truthful even when the
cap meant the full set was never built."""
warnings: tuple[str, ...] = field(default_factory=tuple)
"""Optional conditioning / robustness flags raised during the solve.
Empty tuple in the common-path. Today: forward-compatible reservation;
populated by the HP / RR solvers once they wire up conditioning checks
(#178)."""
def summary(self) -> str:
"""One-paragraph human-readable summary for logging / triage.
Distinguishes the four common cases:
- reachable (returned >=1 IK)
- unreachable (0 raw candidates)
- all-filtered (raw candidates > 0, final 0 -- joint limits etc.)
- capped (max_solutions truncation)
"""
lines = [
f"solver: {self.solver_name} (tier {self.solver_tier})",
f"dispatch: {self.dispatch_reason}",
]
if self.final_count > 0:
lines.append(
f" -> {self.raw_candidates} candidates "
f"-> {self.final_count} returned (max FK {self.max_fk_residual:.1e}, "
f"threshold {self.fk_atol:.0e})"
)
if self.dropped_by_limits:
lines.append(f" filtered by joint limits: {self.dropped_by_limits}")
if self.winding_representatives > self.geometric_branches:
lines.append(
f" winding lifts: {self.geometric_branches} geometric branches "
f"-> {self.winding_representatives} in-limit configurations"
)
if self.dropped_by_max_solutions:
lines.append(f" capped by max_solutions: {self.dropped_by_max_solutions}")
elif self.raw_candidates == 0:
lines.append(
" -> 0 raw candidates: pose appears unreachable "
"(or outside this solver's analytical envelope)"
)
else:
lines.append(f" -> {self.raw_candidates} raw candidates, all filtered:")
if self.dropped_by_limits:
lines.append(
f" dropped by joint limits: {self.dropped_by_limits} "
f"(pass respect_limits=False for the raw geometric set)"
)
if self.refinement_engaged:
lines.append(f" LM polish engaged on {self.refinement_engaged} candidates")
for w in self.warnings:
lines.append(f" warning: {w}")
return "\n".join(lines)
|
solver_name
instance-attribute
Dispatched solver, e.g. "ikgeo.three_parallel".
solver_tier
instance-attribute
0 = closed-form, 1 = univariate search, 2 = bivariate search.
dispatch_reason
instance-attribute
Human-readable explanation of why the dispatcher picked this solver.
Often the most useful field for 'why did ssik refuse my arm?' triage.
raw_candidates
instance-attribute
Raw analytical candidate count returned by the inner solver, before
any post-processing. 0 means the solver itself returned nothing --
pose is outside the analytical reach or the solver's preconditions
aren't met. >0 with final_count == 0 means every candidate was
filtered (limits, etc.).
dropped_by_limits
instance-attribute
Candidates that fell outside URDF joint limits (after the q ± 2π
rescue pass). 0 when respect_limits=False was passed.
dropped_by_max_solutions
instance-attribute
dropped_by_max_solutions: int
Surviving candidates truncated by the max_solutions cap.
final_count
instance-attribute
Number of solutions in the returned list.
max_fk_residual
instance-attribute
Worst fk_residual among returned solutions. nan when
final_count == 0.
refinement_engaged
instance-attribute
How many candidates ran through Levenberg-Marquardt polish (when
allow_refinement=True was passed). 0 when refinement was off.
fk_atol
instance-attribute
The FK-closure threshold the solver used. Useful when the user
customised the tolerance policy and wants the live value back.
geometric_branches
class-attribute
instance-attribute
geometric_branches: int = 0
Distinct geometric IK branches that survived the limit filter, before
winding enumeration (#562). This is the count that pre-6.0 releases
returned.
winding_representatives
class-attribute
instance-attribute
winding_representatives: int = 0
Size of the complete in-limit set after winding enumeration -- the
finite-limit lifts (q + 2*pi*k) of those branches on joints whose limits
span more than one turn. Equal to geometric_branches when the arm has no
such joint or enumerate_windings=False. Reported separately so lifts are
never mistaken for additional geometric branches, and truthful even when the
cap meant the full set was never built.
warnings
class-attribute
instance-attribute
warnings: tuple[str, ...] = field(default_factory=tuple)
Optional conditioning / robustness flags raised during the solve.
Empty tuple in the common-path. Today: forward-compatible reservation;
populated by the HP / RR solvers once they wire up conditioning checks
(#178).
summary
One-paragraph human-readable summary for logging / triage.
Distinguishes the four common cases:
- reachable (returned >=1 IK)
- unreachable (0 raw candidates)
- all-filtered (raw candidates > 0, final 0 -- joint limits etc.)
- capped (max_solutions truncation)
Source code in src/ssik/core/diagnostic.py
| def summary(self) -> str:
"""One-paragraph human-readable summary for logging / triage.
Distinguishes the four common cases:
- reachable (returned >=1 IK)
- unreachable (0 raw candidates)
- all-filtered (raw candidates > 0, final 0 -- joint limits etc.)
- capped (max_solutions truncation)
"""
lines = [
f"solver: {self.solver_name} (tier {self.solver_tier})",
f"dispatch: {self.dispatch_reason}",
]
if self.final_count > 0:
lines.append(
f" -> {self.raw_candidates} candidates "
f"-> {self.final_count} returned (max FK {self.max_fk_residual:.1e}, "
f"threshold {self.fk_atol:.0e})"
)
if self.dropped_by_limits:
lines.append(f" filtered by joint limits: {self.dropped_by_limits}")
if self.winding_representatives > self.geometric_branches:
lines.append(
f" winding lifts: {self.geometric_branches} geometric branches "
f"-> {self.winding_representatives} in-limit configurations"
)
if self.dropped_by_max_solutions:
lines.append(f" capped by max_solutions: {self.dropped_by_max_solutions}")
elif self.raw_candidates == 0:
lines.append(
" -> 0 raw candidates: pose appears unreachable "
"(or outside this solver's analytical envelope)"
)
else:
lines.append(f" -> {self.raw_candidates} raw candidates, all filtered:")
if self.dropped_by_limits:
lines.append(
f" dropped by joint limits: {self.dropped_by_limits} "
f"(pass respect_limits=False for the raw geometric set)"
)
if self.refinement_engaged:
lines.append(f" LM polish engaged on {self.refinement_engaged} candidates")
for w in self.warnings:
lines.append(f" warning: {w}")
return "\n".join(lines)
|
Tuning: TolerancePolicy
Numeric tolerances for kinematic predicates and subproblem solvers.
Structural-predicate fields are consumed by
:mod:ssik.kinematics.predicates and
:func:ssik.core.topology.describe_topology.
Subproblem-solver fields are consumed by SP1-SP6 under
:mod:ssik.subproblems. Their names capture the reason each tolerance
exists so diagnostic messages can say e.g. "SP6 sign branch rejected:
closure distance 1.2e-4 > subproblem_numerical 1e-5" rather than citing
raw numbers.
Attributes:
axis_parallel: cross-product magnitude below which two unit-vector
axes are considered parallel (or anti-parallel). For unit
vectors ||a x b|| = sin(theta) so the default 1e-8
accepts axes differing by up to ~1 microradian.
axis_intersect: perpendicular distance below which two lines in
3D are considered to intersect. Default matches the
axis_parallel tolerance in spirit -- metric-scale chains.
subproblem_feasibility: threshold on residuals that decide whether
a subproblem input admits an exact solution or only a
least-squares approximation (is_ls=True). Used by SP1's
|p_perp| vs |q_perp| check, SP4's |rhs| - R boundary,
SP2's sphere mismatch, etc.
subproblem_numerical: threshold for filtering spurious candidates
produced by quartic / ellipse root-finding inside SP5 and SP6.
Unit-circle closure is checked to this tolerance; returned
solutions that fail are dropped.
subproblem_postverify: tighter closure gate for SP5's refined
candidates. Gauss-Newton refinement drives every genuine SP5
solution to machine precision (measured <= 6e-16 over ~3e5 calls),
so a refined candidate still above this threshold is a spurious
near-double-quartic-root least-squares point (a non-zero local
minimum, FK ~1e-6), not an IK solution -- it is dropped (#337 /
#159). Far above genuine residuals yet well below
subproblem_numerical (the artifact's FK-closure gate, which
this must not loosen); default 1e-9.
subproblem_degeneracy: rank / collinearity threshold. A QR leading
coefficient or sin-of-angle-between-axes below this value
marks the input as degenerate and SP6/aux return
([], is_ls=True) rather than produce nonsense.
subproblem_dedup: per-joint radian radius (mod 2pi) of the
duplicate search; default 1e-3. It is not a claim that
closer solutions are indistinguishable (#600). A solution is a
distinct exact root: for arms whose roots are isolated (6R
solvers, the 6-DOF LM backstop, the 6R artifacts and their
native twins) two candidates within this radius on every joint
merge only if they are the same root -- their midpoint also
closes FK (ssik.refinement.dedup_same_root). Near a fold,
two genuine roots a fraction of a milliradian apart are both
returned, so a pose can report 8 solutions where it once
reported 4. Pairs farther apart are distinct without that test,
which keeps it off the hot path. Where solutions sample a
continuum instead (a redundant 7R arm's self-motion manifold)
the radius remains the sampling resolution: samples closer than
it merge. SP5's internal root merge also uses it.
Source code in src/ssik/core/tolerances.py
| @dataclass(frozen=True)
class TolerancePolicy:
"""Numeric tolerances for kinematic predicates and subproblem solvers.
Structural-predicate fields are consumed by
:mod:`ssik.kinematics.predicates` and
:func:`ssik.core.topology.describe_topology`.
Subproblem-solver fields are consumed by SP1-SP6 under
:mod:`ssik.subproblems`. Their names capture the *reason* each tolerance
exists so diagnostic messages can say e.g. ``"SP6 sign branch rejected:
closure distance 1.2e-4 > subproblem_numerical 1e-5"`` rather than citing
raw numbers.
Attributes:
axis_parallel: cross-product magnitude below which two unit-vector
axes are considered parallel (or anti-parallel). For unit
vectors ``||a x b|| = sin(theta)`` so the default ``1e-8``
accepts axes differing by up to ~1 microradian.
axis_intersect: perpendicular distance below which two lines in
3D are considered to intersect. Default matches the
``axis_parallel`` tolerance in spirit -- metric-scale chains.
subproblem_feasibility: threshold on residuals that decide whether
a subproblem input admits an *exact* solution or only a
least-squares approximation (``is_ls=True``). Used by SP1's
``|p_perp| vs |q_perp|`` check, SP4's ``|rhs| - R`` boundary,
SP2's sphere mismatch, etc.
subproblem_numerical: threshold for filtering spurious candidates
produced by quartic / ellipse root-finding inside SP5 and SP6.
Unit-circle closure is checked to this tolerance; returned
solutions that fail are dropped.
subproblem_postverify: tighter closure gate for SP5's *refined*
candidates. Gauss-Newton refinement drives every genuine SP5
solution to machine precision (measured <= 6e-16 over ~3e5 calls),
so a refined candidate still above this threshold is a spurious
near-double-quartic-root least-squares point (a non-zero local
minimum, FK ~1e-6), not an IK solution -- it is dropped (#337 /
#159). Far above genuine residuals yet well below
``subproblem_numerical`` (the artifact's FK-closure gate, which
this must not loosen); default ``1e-9``.
subproblem_degeneracy: rank / collinearity threshold. A QR leading
coefficient or sin-of-angle-between-axes below this value
marks the input as degenerate and SP6/aux return
``([], is_ls=True)`` rather than produce nonsense.
subproblem_dedup: per-joint radian radius (mod 2pi) of the
duplicate search; default ``1e-3``. It is *not* a claim that
closer solutions are indistinguishable (#600). A solution is a
distinct exact root: for arms whose roots are isolated (6R
solvers, the 6-DOF LM backstop, the 6R artifacts and their
native twins) two candidates within this radius on every joint
merge only if they are the same root -- their midpoint also
closes FK (``ssik.refinement.dedup_same_root``). Near a fold,
two genuine roots a fraction of a milliradian apart are both
returned, so a pose can report 8 solutions where it once
reported 4. Pairs farther apart are distinct without that test,
which keeps it off the hot path. Where solutions sample a
continuum instead (a redundant 7R arm's self-motion manifold)
the radius remains the sampling resolution: samples closer than
it merge. SP5's internal root merge also uses it.
"""
axis_parallel: float = 1e-8
axis_intersect: float = 1e-8
subproblem_feasibility: float = 1e-9
subproblem_numerical: float = 1e-5
subproblem_postverify: float = 1e-9
subproblem_degeneracy: float = 1e-12
subproblem_dedup: float = 1e-3
|
Self-motion charts: ssik.chart
Charts live on Manipulator, so the canonical entry point for one of the 72 shipped arms is Manipulator.from_prebuilt("panda") — the artifact module exports solve and fk only. solve() on the result is the artifact's own solver, so this is not a slower stand-in for import panda_ik; it is that solver with the chart API attached.
Manipulator.self_motion(T) returns the self-motion manifold at T, a SelfMotionManifold of charts. For redundant 7R arms with a closed-form solver (seven_r.spherical_shoulder: Franka Panda, FR3; seven_r.srs: KUKA iiwa and other exactly-concurrent SRS arms) each chart is one continuous branch q(t) with a branch label (what it promises across poses, per family, is the label contract in the ssik.chart module docstring), its domain in the redundancy coordinate, its in_limits() arcs under joint limits, its tangent(t) as a unit direction and a rate, its frame(t, metric) (tangent plus a metric-orthogonal complement), and its pullback_metric(t, metric) (the metric pulled back to the chart coordinate, g(t) = q'(t)^T G q'(t), exact from the tangent and divergent at a fold); the manifold object has the inverse map locate(q), sheets() (charts glued where they meet at a fold: the connected components a controller can reach by self-motion), gap(a, b) and escape(a, b) (the task-space twist along which two sheets approach fastest, with drift_to_merge() to find where they actually merge), and continue_from(...) for continuation along a pose path; track() follows a branch through a list of poses by label, and Manipulator.solve_path(poses) (track_all) follows every branch at once, one manifold build per pose, reporting fold, collision and unreachable events and, on a closed path, the monodromy as permutation. Chart.sample(n, metric) places points uniformly in arc length rather than in t (which bunches up away from folds), and Chart.length(metric) is the branch's length; metric may be a constant matrix or a batched callable such as the mass matrix. For UR-class 6R arms (ikgeo.three_parallel) the charts are the isolated solutions with the geometric (shoulder, elbow, wrist) labels of three_parallel_label. cuspidality_report() classifies a spherical-shoulder arm's chart slicing (build-time diagnostic). respect_limits="wrap" on Manipulator.solve returns the full geometric set wrapped into joint ranges without dropping anything.
With the native extension (Linux and macOS wheels) a family builds in about 20 µs on a target move and q(t), locate(q) and tangent(t) run in a few µs, so all of it fits a 1 kHz control loop; a chart's domain and in_limits() are computed on first access (well under a millisecond) and cached. native=False selects the pure-Python reference.
Charts of the self-motion manifold for redundant 7R arms.
A 7R arm holding a 6-DOF pose T still has a one-parameter family of
configurations: the self-motion manifold FK^-1(T). The closed-form 7R
solvers compute a chart of it internally, a discrete branch label plus a
redundancy coordinate t, and until now discarded both at the Solution
boundary, returning an unordered sample of q vectors. This module exposes
the chart itself.
A :class:Chart is one continuous branch q(t) of the manifold at one pose:
label is a hashable branch identity built from the closed-form
subproblem roots of the branch. At one pose it tells the charts apart; across
poses it is what makes tracking a branch a lookup instead of a
nearest-neighbour search, within the limits of the label contract below.q(t) evaluates the branch at any t, scalar or batched; tangent(t) is
its derivative as a unit direction and a rate (request B2).in_limits() is the domain under joint limits (request A3): sign-zeros of
smooth per-joint margins, bracketed on a grid the shared resolver refines
until no joint steps or bends more than 0.02 rad, then bisected. Where the
in-limit part is a single point (the margins touch zero without changing
sign) it is a zero-width arc (t, t) at the minimum of the worst violation.domain is the set of t where the branch exists (reachable), exact to
bisection tolerance. Joint limits are not applied here: the chart is the
geometric object, limits are a filter the caller composes on top.frame(t, metric) is the tangent plus a metric-orthogonal complement (D1);
sample(n, metric) / length(metric) place points evenly in arc length
in that metric, not in t (C1); restrict(fn) narrows the domain by any
margin, such as a collision distance.
A :class:SelfMotionManifold is the fibre FK^-1(T) itself, presented as
its charts: every branch at one pose, plus the inverse map
:meth:SelfMotionManifold.locate -- given a configuration on the manifold, which
chart is it on and at what t. Across charts it gives sheets() (the
components self-motion can reach), gap(a, b, limits=...) (distance between
holdable postures, minimised over the windings the joint box admits) and
escape; across poses, continue_from / :func:track / :func:track_all
(Manipulator.solve_path, request C2) follow branches by label and
:func:drift_to_merge finds where two sheets join or touch.
Two solver families are supported, each with its own redundancy coordinate:
seven_r.spherical_shoulder (Franka Panda, FR3)
t is the last joint q6 (He & Liu 2021). Labels are
(elbow, shoulder, wrist, interval): three subproblem sign bits plus the
index of the reachable q6 interval the chart lives in. A branch slot is
continuous within one reachable interval, so the interval index is part of
the identity at a given pose; it is not stable across poses when the
number of reachable intervals changes.
seven_r.srs (KUKA iiwa and other exactly-concurrent SRS arms)
t is the elbow swivel angle psi on the circle [-pi, pi),
periodic. Labels are (elbow, shoulder_sign, wrist_sign) with elbow
the cosine-rule root index and the signs in {+1, -1}. Every chart is
defined on the whole circle.
ikgeo.three_parallel (UR-class 6R: UR5e, Z1, CR5, ...)
A 6R arm has no redundancy: the manifold at a regular pose is up to eight
isolated points, each a zero-dimensional chart (dimension == 0,
parameter == "point", q(t) ignores t). Labels are the classic
(shoulder, elbow, wrist) modes as the signs of the three factors of the
singularity determinant, computed from the geometry
(:func:three_parallel_label), so they change only across a singularity
and :func:track follows a branch by label. UR arms are noncuspidal.
Approximate-class arms (*_polished solvers) and the joint-lock family are
refused: their branches are not closed-form (LM polish) or carry no stable
discrete identity across samples (eigen-solver root ordering).
Label contract. What a label promises, per family; nothing beyond this is
promised. In every supported family the labels of one pose are distinct, and
the same label at a nearby pose is the same branch, except:
seven_r.spherical_shoulder: across a fold, where the branch is
relabelled (continue_from reports "fold"), and in the interval
entry whenever the number of reachable q6 intervals changes.seven_r.srs: across a singularity, where two branches meet
("collision").ikgeo.three_parallel: across a singularity, where a factor of the
singularity determinant changes sign.
Labels are compared only within one family and one arm: they are tuples of
small ints whose entries mean different things per family (the SRS and
three-parallel labels are both 3-tuples). "Nearby" is what
:meth:SelfMotionManifold.continue_from checks, a point of the same-label
chart within max_step of the old one; it reports every exception above as
an event rather than trusting the label. A family added later -- joint-lock,
or a polished solver -- states its own entry, and may promise only the first
half: its label then identifies a chart at one pose and nothing across poses,
and tracking falls back to nearest-point continuation.
Cost model: charts(T) builds the family in microseconds (the Panda's
elbow-reachability arcs are closed form; nothing is scanned). locate and
q(t) never need a domain. A chart's domain is computed on first access,
per reachable arc, and cached; enumerating family.charts computes every
domain, since a chart is listed only when its domain is non-empty.
When the native C++ extension is available (the Linux and macOS wheels), the
family is built and evaluated in C++ -- q(t) and locate(q) take about
2 microseconds, a Panda family builds in about 0.5 ms -- and native=False
forces the pure-Python reference, which the parity tests pin the C++ path to.
:attr:SelfMotionManifold.native says which one you got.
Angles come back as the solver produces them, i.e. principal values from
atan2. Along a chart a joint may therefore jump by 2*pi at a wrap; this
is a representative change, not a geometric one. :meth:Chart.curve unwraps
along t for callers who want a continuous curve.
Example::
import ssik
arm = ssik.Manipulator.from_urdf("panda.urdf", base="panda_link0", ee="panda_link8")
T = arm.fk(q_now)
family = arm.self_motion(T)
chart, t = family.locate(q_now) # the branch the arm is on, and where
(ts, qs), *_ = chart.curve(200) # the branch, continuous in t, per segment
PathTrack
Result of :func:track_all: every tracked branch along a pose path.
tracks maps each starting label (at poses[0]) to its list of
:class:TrackStep, one per pose up to where tracking stopped.
stopped_at is the index of the first pose outside the workspace, where
every track ends with an "unreachable" step, or None when the whole
path was tracked. closed is True when the path returns to its
starting pose, and then :attr:permutation reads off the monodromy.
Source code in src/ssik/chart.py
| class PathTrack:
"""Result of :func:`track_all`: every tracked branch along a pose path.
``tracks`` maps each starting label (at ``poses[0]``) to its list of
:class:`TrackStep`, one per pose up to where tracking stopped.
``stopped_at`` is the index of the first pose outside the workspace, where
every track ends with an ``"unreachable"`` step, or ``None`` when the whole
path was tracked. ``closed`` is ``True`` when the path returns to its
starting pose, and then :attr:`permutation` reads off the monodromy.
"""
__slots__ = ("closed", "stopped_at", "tracks")
def __init__(
self,
tracks: dict[tuple[int, ...], list[TrackStep]],
closed: bool,
stopped_at: int | None = None,
) -> None:
self.tracks = tracks
self.closed = closed
self.stopped_at = stopped_at
@property
def permutation(self) -> dict[tuple[int, ...], tuple[int, ...]] | None:
"""Start label -> end label, on a closed path tracked to its end
(``None`` otherwise, including a path that left the workspace). The
identity when every branch came back to itself. Not necessarily a
bijection: after a ``"collision"`` two tracks can end on one chart --
:meth:`is_bijection` says whether it is one."""
if not self.closed or self.stopped_at is not None:
return None
return {label: steps[-1].label for label, steps in self.tracks.items()}
def is_bijection(self) -> bool:
perm = self.permutation
return perm is not None and sorted(perm.values()) == sorted(perm)
def events(self) -> list[TrackStep]:
"""Every step whose event is not a plain ``"label"`` lookup or the
start: the folds, collisions and the stop outside the workspace, in
pose order."""
steps = [
s for track in self.tracks.values() for s in track if s.event not in ("label", "start")
]
return sorted(steps, key=lambda s: s.index)
def q(self, label: tuple[int, ...]) -> NDArray[np.float64]:
"""The configurations of one track, ``(n_steps, dof)``; the last row is
``NaN`` when tracking stopped outside the workspace."""
return np.stack([s.q for s in self.tracks[label]])
def __repr__(self) -> str:
return (
f"PathTrack({len(self.tracks)} branches, "
f"{len(next(iter(self.tracks.values()), []))} poses, closed={self.closed}, "
f"{len(self.events())} events)"
)
|
permutation
property
permutation: dict[tuple[int, ...], tuple[int, ...]] | None
Start label -> end label, on a closed path tracked to its end
(None otherwise, including a path that left the workspace). The
identity when every branch came back to itself. Not necessarily a
bijection: after a "collision" two tracks can end on one chart --
:meth:is_bijection says whether it is one.
events
events() -> list[TrackStep]
Every step whose event is not a plain "label" lookup or the
start: the folds, collisions and the stop outside the workspace, in
pose order.
Source code in src/ssik/chart.py
| def events(self) -> list[TrackStep]:
"""Every step whose event is not a plain ``"label"`` lookup or the
start: the folds, collisions and the stop outside the workspace, in
pose order."""
steps = [
s for track in self.tracks.values() for s in track if s.event not in ("label", "start")
]
return sorted(steps, key=lambda s: s.index)
|
q
q(label: tuple[int, ...]) -> NDArray[np.float64]
The configurations of one track, (n_steps, dof); the last row is
NaN when tracking stopped outside the workspace.
Source code in src/ssik/chart.py
| def q(self, label: tuple[int, ...]) -> NDArray[np.float64]:
"""The configurations of one track, ``(n_steps, dof)``; the last row is
``NaN`` when tracking stopped outside the workspace."""
return np.stack([s.q for s in self.tracks[label]])
|
SelfMotionManifold
The self-motion manifold FK^-1(T) at one pose, presented as charts.
The object is the fibre, not merely a container of charts: :meth:gap,
:meth:sheets, :meth:escape and :meth:singularity_margin are questions
about the manifold, and the charts are how it is coordinatised. Charts are
created on demand and cached, so :meth:locate never pays for charts it does
not return; :attr:charts enumerates the non-empty ones, which requires
every domain.
Source code in src/ssik/chart.py
| class SelfMotionManifold:
"""The self-motion manifold ``FK^-1(T)`` at one pose, presented as charts.
The object is the fibre, not merely a container of charts: :meth:`gap`,
:meth:`sheets`, :meth:`escape` and :meth:`singularity_margin` are questions
about the manifold, and the charts are how it is coordinatised. Charts are
created on demand and cached, so :meth:`locate` never pays for charts it does
not return; :attr:`charts` enumerates the non-empty ones, which requires
every domain.
"""
def __init__(
self,
*,
parameter: str,
periodic: bool,
param_of: Callable[[NDArray[np.float64]], float],
make_chart: Callable[[int], Chart],
nonempty: Callable[[], list[int]],
locate: Callable[[NDArray[np.float64], float], tuple[int, float]] | None = None,
native: bool = False,
kb: KinBody | None = None,
T: NDArray[np.float64] | None = None,
solver_name: str | None = None,
) -> None:
self.parameter = parameter
self.periodic = periodic
self.dimension = 0 if parameter == "point" else 1
self.native = native
"""True when this manifold is backed by the native C++ extension."""
self._param_of = param_of
self._make_chart = make_chart
self._nonempty = nonempty
self._locate = locate
self._kb = kb
self._T = T
self._solver_name = solver_name
self._cache: dict[int, Chart] = {}
def _chart(self, i: int) -> Chart:
c = self._cache.get(i)
if c is None:
c = self._cache[i] = self._make_chart(i)
return c
@cached_property
def charts(self) -> tuple[Chart, ...]:
"""The non-empty charts, in enumeration order (deterministic per pose)."""
return tuple(self._chart(i) for i in self._nonempty())
def __len__(self) -> int:
return len(self.charts)
def __iter__(self) -> Iterator[Chart]:
return iter(self.charts)
def restrict(
self,
fn: Callable[[NDArray[np.float64]], NDArray[np.float64]],
**kwargs: Any,
) -> SelfMotionManifold:
""":meth:`Chart.restrict` applied to every chart, dropping those it empties.
The manifold-level entry point for planning: hand it one predicate over
configurations and get back the pose's manifold with the forbidden parts
removed, ready for :meth:`locate` and :attr:`charts`. Keyword arguments are
forwarded to :meth:`Chart.restrict` unchanged.
:meth:`locate` on the result still answers for the *restricted* charts
only, so a configuration in a forbidden region locates as ``None`` -- which
is the question a controller is asking.
"""
kept = [c.restrict(fn, **kwargs) for c in self.charts]
kept = [c for c in kept if c.domain]
return SelfMotionManifold(
parameter=self.parameter,
periodic=self.periodic,
param_of=self._param_of,
make_chart=lambda i: kept[i],
nonempty=lambda: list(range(len(kept))),
locate=None, # the native locate knows nothing of the restriction
native=False,
# Carried through so the pose-level methods (gap, escape, drift_to_merge)
# keep working on a restricted family; only the charts are narrower.
kb=self._kb,
T=self._T,
solver_name=self._solver_name,
)
def labels(self) -> list[tuple[int, ...]]:
return [c.label for c in self.charts]
def by_label(self, label: tuple[int, ...]) -> Chart | None:
"""The chart with this label at this pose, or ``None``."""
for c in self.charts:
if c.label == label:
return c
return None
def continue_from(
self, chart: Chart, t: float, q: ArrayLike, max_step: float = 0.5
) -> tuple[Chart, float, str]:
"""Continuation (request B4): the point of this pose's manifold that a
controller on ``chart`` at coordinate ``t`` (configuration ``q``) moves
to. Returns ``(chart, t, event)`` with ``event`` one of
- ``"label"``: the chart with the same label exists here and holds a
point within ``max_step`` (wrap-Linf) of ``q`` at the same ``t`` -- a
lookup, no search. Sound where the locked slice is noncuspidal (see
:func:`cuspidality_report`), which the closed-form decomposition
guarantees for the spherical-shoulder and SRS families.
- ``"fold"``: that chart is gone or too far, but another chart holds a
point within ``max_step``: the branch passed a fold and was relabelled.
- ``"collision"``: nothing within ``max_step`` -- the branch collided
with another and vanished (a singularity crossing); the returned chart
is the nearest one and the caller must decide.
``t`` is kept, so the coordinate stays continuous along the path; on a
periodic chart it is wrapped.
"""
qa = np.asarray(q, dtype=np.float64)
tt = float(_wrap_pi(np.asarray(t))) if self.periodic else float(t)
same = self.by_label(chart.label)
if same is not None:
qc = same.q(tt)
if np.all(np.isfinite(qc)) and _wrap_dist(qc, qa) <= max_step:
return same, tt, "label"
best: tuple[float, Chart] | None = None
for c in self.charts:
qc = c.q(tt)
if not np.all(np.isfinite(qc)):
continue
dist = _wrap_dist(qc, qa)
if best is None or dist < best[0]:
best = (dist, c)
if best is None:
# No chart of this pose exists at t: the reachable range of the
# coordinate moved past the branch (an elbow fold in t). Clamp to the
# nearest domain end of the nearest chart and report the collision.
cand: list[tuple[float, Chart, float]] = []
for c in self.charts:
for lo, hi in c.domain:
tc = min(max(tt, lo), hi)
qc = c.q(tc)
if np.all(np.isfinite(qc)):
cand.append((_wrap_dist(qc, qa), c, tc))
if not cand:
raise ValueError("continue_from: this pose has no chart at all")
dist, c, tc = min(cand, key=lambda x: x[0])
return c, tc, "collision"
return best[1], tt, "fold" if best[0] <= max_step else "collision"
def gap(
self,
a: Chart,
b: Chart,
n_samples: int = 91,
*,
limits: ArrayLike | bool | None = None,
) -> tuple[float, NDArray[np.float64], NDArray[np.float64]]:
"""The joint-space gap between two charts of this pose: the Euclidean
distance between their nearest points, and those points.
A grid of ``n_samples`` per piece brackets the pair; a bounded
minimisation over the two coordinates then refines it.
``limits=None`` measures on the torus (every joint wrapped to
``[-pi, pi)``): right for the geometry, blind to the joint box. With
``limits`` (``True`` for the chain's own, or ``(dof, 2)``) only postures
the arm can hold count: each chart is cut to :meth:`Chart.in_limits`,
and the distance is the plain one between *in-box representatives*,
minimised over the windings ``q_i + 2*pi*k`` that lie in the box. That
one rule gets both cases right that wrap-to-pi gets half wrong: where a
joint's range is under a turn only one winding is holdable and two
points a turn apart are one posture; where it exceeds a turn (UR arms,
``+-2*pi``) both windings are holdable, distinct postures, and the
nearer pair is what counts. The returned points are those in-box
representatives, so ``|qb - qa|`` is the gap. ``inf`` (and ``NaN``
points) when either chart has no posture in the box.
"""
box = None if limits is None or limits is False else _box(a, limits)
pa, ta = _sample_points(a, n_samples, with_t=True, limits=limits)
pb, tb = _sample_points(b, n_samples, with_t=True, limits=limits)
nan = np.full(a.q(0.0).shape[-1], np.nan)
if pa.shape[0] == 0 or pb.shape[0] == 0:
return float("inf"), nan, nan
def dist2(qa: NDArray[np.float64], qb: NDArray[np.float64]) -> NDArray[np.float64]:
if box is None:
return np.asarray(np.sum(_wrap_pi(qb - qa) ** 2, axis=-1))
return _inbox_dist2(qa, qb, box)[0]
d2 = np.stack([dist2(qa[None, :], pb) for qa in pa])
if not np.isfinite(d2).any():
return float("inf"), nan, nan
# Refine from every grid pair within twice the best distance, not from
# the argmin alone. Two charts parameterised by the same joint put
# mirror pairs (ta, tb) and (tb, ta) at one grid distance, and which
# np.argmin keeps is decided by rounding (BLAS kernel, platform); the
# search from one of them found a touching pair 1.4e-3 apart and from
# the other stalled at 0.109 on the same Panda pose.
order = np.argsort(d2, axis=None, kind="stable")
best_d2 = float(d2.flat[order[0]])
starts: list[tuple[int, int]] = []
for k in order[:_GAP_STARTS]:
si, sj = np.unravel_index(int(k), d2.shape)
if d2[si, sj] <= 4.0 * best_d2:
starts.append((int(si), int(sj)))
i, j = starts[0]
qa, qb = pa[i], pb[j]
free_any = a.dimension == 1 or b.dimension == 1
if free_any:
from scipy.optimize import minimize # type: ignore[import-untyped]
def bounds_of(c: Chart, t: float) -> tuple[float, float]:
for lo, hi in c._pieces(limits):
if lo - 1e-9 <= t <= hi + 1e-9:
return lo, hi
return t, t
best_v = best_d2
for i0, j0 in starts:
fixed = (pa[i0], pb[j0])
free = [
(c, t) for c, t in ((a, float(ta[i0])), (b, float(tb[j0]))) if c.dimension == 1
]
def points(
x: NDArray[np.float64], fixed: tuple[NDArray[np.float64], ...] = fixed
) -> tuple[NDArray[np.float64], ...]:
qs = []
k = 0
for c, q_fixed in zip((a, b), fixed, strict=True):
if c.dimension == 1:
qs.append(c._eval_t(np.array([float(x[k])]))[0])
k += 1
else:
qs.append(q_fixed)
return qs[0], qs[1]
def objective(x: NDArray[np.float64], points: Any = points) -> float:
ua, ub = points(x)
if not (np.all(np.isfinite(ua)) and np.all(np.isfinite(ub))):
return 1e6
v = float(dist2(ua[None, :], ub[None, :])[0])
return v if np.isfinite(v) else 1e6
x0 = np.array([t for _c, t in free])
# Tight tolerances: the gradient of a squared distance vanishes with the
# distance, and at scipy's defaults the search stopped ~1e-3 rad short
# of a touching pair -- too coarse for drift_to_merge to call a merge.
res = minimize(
objective,
x0,
method="L-BFGS-B",
bounds=[bounds_of(c, t) for c, t in free],
options={"ftol": 1e-16, "gtol": 1e-14, "maxiter": 500},
)
if res.fun < best_v:
best_v = float(res.fun)
qa, qb = points(res.x)
if box is None:
return float(np.sqrt(np.sum(_wrap_pi(qb - qa) ** 2))), qa, qb
d2v, ra, rb = _inbox_dist2(qa[None, :], qb[None, :], box)
# The nearest pair can sit at a cusp in t narrow enough that the search
# restricted to the in-limits pieces starts in the wrong cell and stops
# short (0.085 where a pair 8.4e-4 apart was in the box, on the iiwa14).
# The torus search's pair, measured in the box, is a second candidate.
_, ta_q, tb_q = self.gap(a, b, n_samples)
if np.all(np.isfinite(ta_q)) and np.all(np.isfinite(tb_q)):
d2t, sa, sb = _inbox_dist2(ta_q[None, :], tb_q[None, :], box)
if d2t[0] < d2v[0]:
d2v, ra, rb = d2t, sa, sb
return float(np.sqrt(d2v[0])), ra[0], rb[0]
def sheets(self, tol: float = 1e-6) -> list[tuple[Chart, ...]]:
"""Connected components of the manifold (request B3): charts glued where
they meet, i.e. whose :meth:`gap` is below ``tol`` (partner slots share a
fold point at a domain end). Each sheet is a tuple of charts; the
arm's self-motion can reach every point of the sheet it is on and no
point of any other. Isolated solutions (6R arms) are one-chart sheets.
"""
cs = list(self.charts)
parent = list(range(len(cs)))
def find(i: int) -> int:
while parent[i] != i:
parent[i] = parent[parent[i]]
i = parent[i]
return i
for i in range(len(cs)):
for j in range(i + 1, len(cs)):
if cs[i].dimension == 1 and self.gap(cs[i], cs[j])[0] <= tol:
parent[find(i)] = find(j)
groups: dict[int, list[Chart]] = {}
for i, c in enumerate(cs):
groups.setdefault(find(i), []).append(c)
return [tuple(g) for g in groups.values()]
def escape(
self, a: Chart, b: Chart, step: float = 1e-4
) -> tuple[NDArray[np.float64], float, NDArray[np.float64], NDArray[np.float64]]:
"""Escape direction between two charts (request D2): the unit task-space
twist along which the two branches approach each other fastest, and a
first-order estimate of how far the pose must drift along it for them to
merge.
Meaningful between charts on different :meth:`sheets`: they can only be
connected by moving the pose to a fold, where two branches coincide
(partner charts on one sheet already touch, gap zero). The direction
is the negative gradient of :meth:`gap` with
respect to the pose, taken by central differences over the six spatial
twist components (``se3_exp(dx) @ T``, the convention of
``ssik.refinement.kinbody_jacobian``), and the magnitude is
``gap / |grad gap|``, the distance to a zero gap if the gap kept closing
at this rate.
**The magnitude overshoots by about a factor of two, and systematically
so.** Two branches annihilate at a fold, where they are the two roots of
a quadratic, so the gap closes like ``C * sqrt(s_c - s)`` rather than
linearly; a first-order step from ``s = 0`` then lands at ``2 * s_c``
exactly. Measured over 204 sheet pairs the closest approach sits at a
median ``0.50`` of this magnitude, with the fraction uncorrelated with
the gap (r = -0.01) -- as it must be, the factor coming from the
exponent alone. So use the direction as returned and line-search the
distance, starting near ``0.5 * magnitude``; the minimum is sharp in
``s`` because ``d(gap)/ds`` diverges at the merge.
:func:`drift_to_merge` walks it. Returns
``(direction, magnitude, q_a, q_b)`` with the nearest pair at ``T``.
Model-free: twelve chart families of neighbouring poses.
"""
if self._kb is None or self._T is None:
raise ValueError("escape needs the manifold's chain and pose")
gap0, qa, qb = self.gap(a, b)
grad = np.zeros(6)
for k in range(6):
g = []
for sgn in (1.0, -1.0):
dx = np.zeros(6)
dx[k] = sgn * step
fam = charts(self._kb, se3_exp(dx) @ self._T, solver_name=self._solver_name)
ca, cb = fam.by_label(a.label), fam.by_label(b.label)
g.append(fam.gap(ca, cb)[0] if ca is not None and cb is not None else np.nan)
grad[k] = (g[0] - g[1]) / (2 * step)
if not np.all(np.isfinite(grad)):
grad = np.where(np.isfinite(grad), grad, 0.0)
n = float(np.linalg.norm(grad))
if n == 0.0:
return np.zeros(6), float("inf"), qa, qb
return -grad / n, gap0 / n, qa, qb
def singularity_margin(self, q: ArrayLike) -> float:
"""Smallest singular value of the chain's 6xN spatial Jacobian at ``q``:
zero exactly at a kinematic singularity, where branches meet. For a 6R
arm this is the margin a controller watches while it tracks a branch."""
from ssik.refinement import kinbody_jacobian
if self._kb is None:
return float("nan")
return float(
np.linalg.svd(
kinbody_jacobian(self._kb, np.asarray(q, dtype=np.float64)), compute_uv=False
).min()
)
def param_of(self, q: ArrayLike) -> float:
"""The redundancy coordinate of a configuration ``q``.
Pure geometry, no branch search: ``q6`` itself for the spherical-shoulder
family, the elbow's swivel angle on the shoulder-wrist circle for SRS.
Meaningful only when ``FK(q)`` is this manifold's pose.
"""
return self._param_of(np.asarray(q, dtype=np.float64))
def locate(self, q: ArrayLike, tol: float = 1e-6) -> tuple[Chart, float] | None:
"""Inverse chart map: the chart ``q`` lies on and its coordinate there.
Evaluates the branches at ``t = param_of(q)`` and returns the first
whose value matches ``q`` to ``tol`` in wrap-to-pi L-infinity distance,
or ``None`` when none does (``q`` is not on this pose's manifold to that
tolerance). One closed-form evaluation; no domain is computed.
"""
qa = np.asarray(q, dtype=np.float64)
if self._locate is not None:
idx, t = self._locate(qa, tol)
return (self._chart(idx), t) if idx >= 0 else None
t = self._param_of(qa)
for chart in self.charts:
if not chart.contains(t):
continue
qc = chart.q(t)
if np.all(np.isfinite(qc)) and _wrap_dist(qc, qa) <= tol:
return chart, t
return None
|
native
instance-attribute
True when this manifold is backed by the native C++ extension.
charts
cached
property
charts: tuple[Chart, ...]
The non-empty charts, in enumeration order (deterministic per pose).
restrict
restrict(
fn: Callable[[NDArray[float64]], NDArray[float64]], **kwargs: Any
) -> SelfMotionManifold
:meth:Chart.restrict applied to every chart, dropping those it empties.
The manifold-level entry point for planning: hand it one predicate over
configurations and get back the pose's manifold with the forbidden parts
removed, ready for :meth:locate and :attr:charts. Keyword arguments are
forwarded to :meth:Chart.restrict unchanged.
:meth:locate on the result still answers for the restricted charts
only, so a configuration in a forbidden region locates as None -- which
is the question a controller is asking.
Source code in src/ssik/chart.py
| def restrict(
self,
fn: Callable[[NDArray[np.float64]], NDArray[np.float64]],
**kwargs: Any,
) -> SelfMotionManifold:
""":meth:`Chart.restrict` applied to every chart, dropping those it empties.
The manifold-level entry point for planning: hand it one predicate over
configurations and get back the pose's manifold with the forbidden parts
removed, ready for :meth:`locate` and :attr:`charts`. Keyword arguments are
forwarded to :meth:`Chart.restrict` unchanged.
:meth:`locate` on the result still answers for the *restricted* charts
only, so a configuration in a forbidden region locates as ``None`` -- which
is the question a controller is asking.
"""
kept = [c.restrict(fn, **kwargs) for c in self.charts]
kept = [c for c in kept if c.domain]
return SelfMotionManifold(
parameter=self.parameter,
periodic=self.periodic,
param_of=self._param_of,
make_chart=lambda i: kept[i],
nonempty=lambda: list(range(len(kept))),
locate=None, # the native locate knows nothing of the restriction
native=False,
# Carried through so the pose-level methods (gap, escape, drift_to_merge)
# keep working on a restricted family; only the charts are narrower.
kb=self._kb,
T=self._T,
solver_name=self._solver_name,
)
|
by_label
by_label(label: tuple[int, ...]) -> Chart | None
The chart with this label at this pose, or None.
Source code in src/ssik/chart.py
| def by_label(self, label: tuple[int, ...]) -> Chart | None:
"""The chart with this label at this pose, or ``None``."""
for c in self.charts:
if c.label == label:
return c
return None
|
continue_from
continue_from(
chart: Chart, t: float, q: ArrayLike, max_step: float = 0.5
) -> tuple[Chart, float, str]
Continuation (request B4): the point of this pose's manifold that a
controller on chart at coordinate t (configuration q) moves
to. Returns (chart, t, event) with event one of
"label": the chart with the same label exists here and holds a
point within max_step (wrap-Linf) of q at the same t -- a
lookup, no search. Sound where the locked slice is noncuspidal (see
:func:cuspidality_report), which the closed-form decomposition
guarantees for the spherical-shoulder and SRS families."fold": that chart is gone or too far, but another chart holds a
point within max_step: the branch passed a fold and was relabelled."collision": nothing within max_step -- the branch collided
with another and vanished (a singularity crossing); the returned chart
is the nearest one and the caller must decide.
t is kept, so the coordinate stays continuous along the path; on a
periodic chart it is wrapped.
Source code in src/ssik/chart.py
| def continue_from(
self, chart: Chart, t: float, q: ArrayLike, max_step: float = 0.5
) -> tuple[Chart, float, str]:
"""Continuation (request B4): the point of this pose's manifold that a
controller on ``chart`` at coordinate ``t`` (configuration ``q``) moves
to. Returns ``(chart, t, event)`` with ``event`` one of
- ``"label"``: the chart with the same label exists here and holds a
point within ``max_step`` (wrap-Linf) of ``q`` at the same ``t`` -- a
lookup, no search. Sound where the locked slice is noncuspidal (see
:func:`cuspidality_report`), which the closed-form decomposition
guarantees for the spherical-shoulder and SRS families.
- ``"fold"``: that chart is gone or too far, but another chart holds a
point within ``max_step``: the branch passed a fold and was relabelled.
- ``"collision"``: nothing within ``max_step`` -- the branch collided
with another and vanished (a singularity crossing); the returned chart
is the nearest one and the caller must decide.
``t`` is kept, so the coordinate stays continuous along the path; on a
periodic chart it is wrapped.
"""
qa = np.asarray(q, dtype=np.float64)
tt = float(_wrap_pi(np.asarray(t))) if self.periodic else float(t)
same = self.by_label(chart.label)
if same is not None:
qc = same.q(tt)
if np.all(np.isfinite(qc)) and _wrap_dist(qc, qa) <= max_step:
return same, tt, "label"
best: tuple[float, Chart] | None = None
for c in self.charts:
qc = c.q(tt)
if not np.all(np.isfinite(qc)):
continue
dist = _wrap_dist(qc, qa)
if best is None or dist < best[0]:
best = (dist, c)
if best is None:
# No chart of this pose exists at t: the reachable range of the
# coordinate moved past the branch (an elbow fold in t). Clamp to the
# nearest domain end of the nearest chart and report the collision.
cand: list[tuple[float, Chart, float]] = []
for c in self.charts:
for lo, hi in c.domain:
tc = min(max(tt, lo), hi)
qc = c.q(tc)
if np.all(np.isfinite(qc)):
cand.append((_wrap_dist(qc, qa), c, tc))
if not cand:
raise ValueError("continue_from: this pose has no chart at all")
dist, c, tc = min(cand, key=lambda x: x[0])
return c, tc, "collision"
return best[1], tt, "fold" if best[0] <= max_step else "collision"
|
gap
gap(
a: Chart, b: Chart, n_samples: int = 91, *, limits: ArrayLike | bool | None = None
) -> tuple[float, NDArray[np.float64], NDArray[np.float64]]
The joint-space gap between two charts of this pose: the Euclidean
distance between their nearest points, and those points.
A grid of n_samples per piece brackets the pair; a bounded
minimisation over the two coordinates then refines it.
limits=None measures on the torus (every joint wrapped to
[-pi, pi)): right for the geometry, blind to the joint box. With
limits (True for the chain's own, or (dof, 2)) only postures
the arm can hold count: each chart is cut to :meth:Chart.in_limits,
and the distance is the plain one between in-box representatives,
minimised over the windings q_i + 2*pi*k that lie in the box. That
one rule gets both cases right that wrap-to-pi gets half wrong: where a
joint's range is under a turn only one winding is holdable and two
points a turn apart are one posture; where it exceeds a turn (UR arms,
+-2*pi) both windings are holdable, distinct postures, and the
nearer pair is what counts. The returned points are those in-box
representatives, so |qb - qa| is the gap. inf (and NaN
points) when either chart has no posture in the box.
Source code in src/ssik/chart.py
| def gap(
self,
a: Chart,
b: Chart,
n_samples: int = 91,
*,
limits: ArrayLike | bool | None = None,
) -> tuple[float, NDArray[np.float64], NDArray[np.float64]]:
"""The joint-space gap between two charts of this pose: the Euclidean
distance between their nearest points, and those points.
A grid of ``n_samples`` per piece brackets the pair; a bounded
minimisation over the two coordinates then refines it.
``limits=None`` measures on the torus (every joint wrapped to
``[-pi, pi)``): right for the geometry, blind to the joint box. With
``limits`` (``True`` for the chain's own, or ``(dof, 2)``) only postures
the arm can hold count: each chart is cut to :meth:`Chart.in_limits`,
and the distance is the plain one between *in-box representatives*,
minimised over the windings ``q_i + 2*pi*k`` that lie in the box. That
one rule gets both cases right that wrap-to-pi gets half wrong: where a
joint's range is under a turn only one winding is holdable and two
points a turn apart are one posture; where it exceeds a turn (UR arms,
``+-2*pi``) both windings are holdable, distinct postures, and the
nearer pair is what counts. The returned points are those in-box
representatives, so ``|qb - qa|`` is the gap. ``inf`` (and ``NaN``
points) when either chart has no posture in the box.
"""
box = None if limits is None or limits is False else _box(a, limits)
pa, ta = _sample_points(a, n_samples, with_t=True, limits=limits)
pb, tb = _sample_points(b, n_samples, with_t=True, limits=limits)
nan = np.full(a.q(0.0).shape[-1], np.nan)
if pa.shape[0] == 0 or pb.shape[0] == 0:
return float("inf"), nan, nan
def dist2(qa: NDArray[np.float64], qb: NDArray[np.float64]) -> NDArray[np.float64]:
if box is None:
return np.asarray(np.sum(_wrap_pi(qb - qa) ** 2, axis=-1))
return _inbox_dist2(qa, qb, box)[0]
d2 = np.stack([dist2(qa[None, :], pb) for qa in pa])
if not np.isfinite(d2).any():
return float("inf"), nan, nan
# Refine from every grid pair within twice the best distance, not from
# the argmin alone. Two charts parameterised by the same joint put
# mirror pairs (ta, tb) and (tb, ta) at one grid distance, and which
# np.argmin keeps is decided by rounding (BLAS kernel, platform); the
# search from one of them found a touching pair 1.4e-3 apart and from
# the other stalled at 0.109 on the same Panda pose.
order = np.argsort(d2, axis=None, kind="stable")
best_d2 = float(d2.flat[order[0]])
starts: list[tuple[int, int]] = []
for k in order[:_GAP_STARTS]:
si, sj = np.unravel_index(int(k), d2.shape)
if d2[si, sj] <= 4.0 * best_d2:
starts.append((int(si), int(sj)))
i, j = starts[0]
qa, qb = pa[i], pb[j]
free_any = a.dimension == 1 or b.dimension == 1
if free_any:
from scipy.optimize import minimize # type: ignore[import-untyped]
def bounds_of(c: Chart, t: float) -> tuple[float, float]:
for lo, hi in c._pieces(limits):
if lo - 1e-9 <= t <= hi + 1e-9:
return lo, hi
return t, t
best_v = best_d2
for i0, j0 in starts:
fixed = (pa[i0], pb[j0])
free = [
(c, t) for c, t in ((a, float(ta[i0])), (b, float(tb[j0]))) if c.dimension == 1
]
def points(
x: NDArray[np.float64], fixed: tuple[NDArray[np.float64], ...] = fixed
) -> tuple[NDArray[np.float64], ...]:
qs = []
k = 0
for c, q_fixed in zip((a, b), fixed, strict=True):
if c.dimension == 1:
qs.append(c._eval_t(np.array([float(x[k])]))[0])
k += 1
else:
qs.append(q_fixed)
return qs[0], qs[1]
def objective(x: NDArray[np.float64], points: Any = points) -> float:
ua, ub = points(x)
if not (np.all(np.isfinite(ua)) and np.all(np.isfinite(ub))):
return 1e6
v = float(dist2(ua[None, :], ub[None, :])[0])
return v if np.isfinite(v) else 1e6
x0 = np.array([t for _c, t in free])
# Tight tolerances: the gradient of a squared distance vanishes with the
# distance, and at scipy's defaults the search stopped ~1e-3 rad short
# of a touching pair -- too coarse for drift_to_merge to call a merge.
res = minimize(
objective,
x0,
method="L-BFGS-B",
bounds=[bounds_of(c, t) for c, t in free],
options={"ftol": 1e-16, "gtol": 1e-14, "maxiter": 500},
)
if res.fun < best_v:
best_v = float(res.fun)
qa, qb = points(res.x)
if box is None:
return float(np.sqrt(np.sum(_wrap_pi(qb - qa) ** 2))), qa, qb
d2v, ra, rb = _inbox_dist2(qa[None, :], qb[None, :], box)
# The nearest pair can sit at a cusp in t narrow enough that the search
# restricted to the in-limits pieces starts in the wrong cell and stops
# short (0.085 where a pair 8.4e-4 apart was in the box, on the iiwa14).
# The torus search's pair, measured in the box, is a second candidate.
_, ta_q, tb_q = self.gap(a, b, n_samples)
if np.all(np.isfinite(ta_q)) and np.all(np.isfinite(tb_q)):
d2t, sa, sb = _inbox_dist2(ta_q[None, :], tb_q[None, :], box)
if d2t[0] < d2v[0]:
d2v, ra, rb = d2t, sa, sb
return float(np.sqrt(d2v[0])), ra[0], rb[0]
|
sheets
sheets(tol: float = 1e-06) -> list[tuple[Chart, ...]]
Connected components of the manifold (request B3): charts glued where
they meet, i.e. whose :meth:gap is below tol (partner slots share a
fold point at a domain end). Each sheet is a tuple of charts; the
arm's self-motion can reach every point of the sheet it is on and no
point of any other. Isolated solutions (6R arms) are one-chart sheets.
Source code in src/ssik/chart.py
| def sheets(self, tol: float = 1e-6) -> list[tuple[Chart, ...]]:
"""Connected components of the manifold (request B3): charts glued where
they meet, i.e. whose :meth:`gap` is below ``tol`` (partner slots share a
fold point at a domain end). Each sheet is a tuple of charts; the
arm's self-motion can reach every point of the sheet it is on and no
point of any other. Isolated solutions (6R arms) are one-chart sheets.
"""
cs = list(self.charts)
parent = list(range(len(cs)))
def find(i: int) -> int:
while parent[i] != i:
parent[i] = parent[parent[i]]
i = parent[i]
return i
for i in range(len(cs)):
for j in range(i + 1, len(cs)):
if cs[i].dimension == 1 and self.gap(cs[i], cs[j])[0] <= tol:
parent[find(i)] = find(j)
groups: dict[int, list[Chart]] = {}
for i, c in enumerate(cs):
groups.setdefault(find(i), []).append(c)
return [tuple(g) for g in groups.values()]
|
escape
escape(
a: Chart, b: Chart, step: float = 0.0001
) -> tuple[NDArray[np.float64], float, NDArray[np.float64], NDArray[np.float64]]
Escape direction between two charts (request D2): the unit task-space
twist along which the two branches approach each other fastest, and a
first-order estimate of how far the pose must drift along it for them to
merge.
Meaningful between charts on different :meth:sheets: they can only be
connected by moving the pose to a fold, where two branches coincide
(partner charts on one sheet already touch, gap zero). The direction
is the negative gradient of :meth:gap with
respect to the pose, taken by central differences over the six spatial
twist components (se3_exp(dx) @ T, the convention of
ssik.refinement.kinbody_jacobian), and the magnitude is
gap / |grad gap|, the distance to a zero gap if the gap kept closing
at this rate.
The magnitude overshoots by about a factor of two, and systematically
so. Two branches annihilate at a fold, where they are the two roots of
a quadratic, so the gap closes like C * sqrt(s_c - s) rather than
linearly; a first-order step from s = 0 then lands at 2 * s_c
exactly. Measured over 204 sheet pairs the closest approach sits at a
median 0.50 of this magnitude, with the fraction uncorrelated with
the gap (r = -0.01) -- as it must be, the factor coming from the
exponent alone. So use the direction as returned and line-search the
distance, starting near 0.5 * magnitude; the minimum is sharp in
s because d(gap)/ds diverges at the merge.
:func:drift_to_merge walks it. Returns
(direction, magnitude, q_a, q_b) with the nearest pair at T.
Model-free: twelve chart families of neighbouring poses.
Source code in src/ssik/chart.py
| def escape(
self, a: Chart, b: Chart, step: float = 1e-4
) -> tuple[NDArray[np.float64], float, NDArray[np.float64], NDArray[np.float64]]:
"""Escape direction between two charts (request D2): the unit task-space
twist along which the two branches approach each other fastest, and a
first-order estimate of how far the pose must drift along it for them to
merge.
Meaningful between charts on different :meth:`sheets`: they can only be
connected by moving the pose to a fold, where two branches coincide
(partner charts on one sheet already touch, gap zero). The direction
is the negative gradient of :meth:`gap` with
respect to the pose, taken by central differences over the six spatial
twist components (``se3_exp(dx) @ T``, the convention of
``ssik.refinement.kinbody_jacobian``), and the magnitude is
``gap / |grad gap|``, the distance to a zero gap if the gap kept closing
at this rate.
**The magnitude overshoots by about a factor of two, and systematically
so.** Two branches annihilate at a fold, where they are the two roots of
a quadratic, so the gap closes like ``C * sqrt(s_c - s)`` rather than
linearly; a first-order step from ``s = 0`` then lands at ``2 * s_c``
exactly. Measured over 204 sheet pairs the closest approach sits at a
median ``0.50`` of this magnitude, with the fraction uncorrelated with
the gap (r = -0.01) -- as it must be, the factor coming from the
exponent alone. So use the direction as returned and line-search the
distance, starting near ``0.5 * magnitude``; the minimum is sharp in
``s`` because ``d(gap)/ds`` diverges at the merge.
:func:`drift_to_merge` walks it. Returns
``(direction, magnitude, q_a, q_b)`` with the nearest pair at ``T``.
Model-free: twelve chart families of neighbouring poses.
"""
if self._kb is None or self._T is None:
raise ValueError("escape needs the manifold's chain and pose")
gap0, qa, qb = self.gap(a, b)
grad = np.zeros(6)
for k in range(6):
g = []
for sgn in (1.0, -1.0):
dx = np.zeros(6)
dx[k] = sgn * step
fam = charts(self._kb, se3_exp(dx) @ self._T, solver_name=self._solver_name)
ca, cb = fam.by_label(a.label), fam.by_label(b.label)
g.append(fam.gap(ca, cb)[0] if ca is not None and cb is not None else np.nan)
grad[k] = (g[0] - g[1]) / (2 * step)
if not np.all(np.isfinite(grad)):
grad = np.where(np.isfinite(grad), grad, 0.0)
n = float(np.linalg.norm(grad))
if n == 0.0:
return np.zeros(6), float("inf"), qa, qb
return -grad / n, gap0 / n, qa, qb
|
singularity_margin
singularity_margin(q: ArrayLike) -> float
Smallest singular value of the chain's 6xN spatial Jacobian at q:
zero exactly at a kinematic singularity, where branches meet. For a 6R
arm this is the margin a controller watches while it tracks a branch.
Source code in src/ssik/chart.py
| def singularity_margin(self, q: ArrayLike) -> float:
"""Smallest singular value of the chain's 6xN spatial Jacobian at ``q``:
zero exactly at a kinematic singularity, where branches meet. For a 6R
arm this is the margin a controller watches while it tracks a branch."""
from ssik.refinement import kinbody_jacobian
if self._kb is None:
return float("nan")
return float(
np.linalg.svd(
kinbody_jacobian(self._kb, np.asarray(q, dtype=np.float64)), compute_uv=False
).min()
)
|
param_of
param_of(q: ArrayLike) -> float
The redundancy coordinate of a configuration q.
Pure geometry, no branch search: q6 itself for the spherical-shoulder
family, the elbow's swivel angle on the shoulder-wrist circle for SRS.
Meaningful only when FK(q) is this manifold's pose.
Source code in src/ssik/chart.py
| def param_of(self, q: ArrayLike) -> float:
"""The redundancy coordinate of a configuration ``q``.
Pure geometry, no branch search: ``q6`` itself for the spherical-shoulder
family, the elbow's swivel angle on the shoulder-wrist circle for SRS.
Meaningful only when ``FK(q)`` is this manifold's pose.
"""
return self._param_of(np.asarray(q, dtype=np.float64))
|
locate
locate(q: ArrayLike, tol: float = 1e-06) -> tuple[Chart, float] | None
Inverse chart map: the chart q lies on and its coordinate there.
Evaluates the branches at t = param_of(q) and returns the first
whose value matches q to tol in wrap-to-pi L-infinity distance,
or None when none does (q is not on this pose's manifold to that
tolerance). One closed-form evaluation; no domain is computed.
Source code in src/ssik/chart.py
| def locate(self, q: ArrayLike, tol: float = 1e-6) -> tuple[Chart, float] | None:
"""Inverse chart map: the chart ``q`` lies on and its coordinate there.
Evaluates the branches at ``t = param_of(q)`` and returns the first
whose value matches ``q`` to ``tol`` in wrap-to-pi L-infinity distance,
or ``None`` when none does (``q`` is not on this pose's manifold to that
tolerance). One closed-form evaluation; no domain is computed.
"""
qa = np.asarray(q, dtype=np.float64)
if self._locate is not None:
idx, t = self._locate(qa, tol)
return (self._chart(idx), t) if idx >= 0 else None
t = self._param_of(qa)
for chart in self.charts:
if not chart.contains(t):
continue
qc = chart.q(t)
if np.all(np.isfinite(qc)) and _wrap_dist(qc, qa) <= tol:
return chart, t
return None
|
Chart
One continuous branch q(t) of the self-motion manifold at one pose.
label is a hashable branch identity (see the module docstring for the
per-family meaning), parameter the redundancy coordinate's name
("q6" or "swivel"), periodic whether t lives on a circle,
and :attr:domain the intervals on which the branch exists, computed on
first access.
Source code in src/ssik/chart.py
| class Chart:
"""One continuous branch ``q(t)`` of the self-motion manifold at one pose.
``label`` is a hashable branch identity (see the module docstring for the
per-family meaning), ``parameter`` the redundancy coordinate's name
(``"q6"`` or ``"swivel"``), ``periodic`` whether ``t`` lives on a circle,
and :attr:`domain` the intervals on which the branch exists, computed on
first access.
"""
def __init__(
self,
*,
label: tuple[int, ...],
parameter: str,
periodic: bool,
eval_fn: Callable[[NDArray[np.float64]], NDArray[np.float64]],
domain_fn: Callable[[], tuple[tuple[float, float], ...]],
deriv_fn: Callable[[NDArray[np.float64]], NDArray[np.float64]] | None = None,
fold_dir_fn: Callable[[NDArray[np.float64]], NDArray[np.float64]] | None = None,
limit_arcs_fn: Callable[[Limits], tuple[tuple[float, float], ...]] | None = None,
default_limits: Limits = (),
) -> None:
self.label = label
self.parameter = parameter
self.periodic = periodic
self.dimension = 0 if parameter == "point" else 1
"""0 for an isolated solution (6R arms), 1 for a one-parameter branch."""
self._eval = eval_fn
self._domain_fn = domain_fn
self._deriv = deriv_fn
self._fold_dir = fold_dir_fn
self._limit_arcs = limit_arcs_fn
self._default_limits = default_limits
self._limit_cache: dict[Limits, tuple[tuple[float, float], ...]] = {}
@cached_property
def domain(self) -> tuple[tuple[float, float], ...]:
"""Intervals ``(t_lo, t_hi)`` on which the branch exists: the full circle
for ``"swivel"``; for ``"q6"`` the exact elbow-fold arcs intersected with
the shoulder and wrist gates, the latter refined to ~1e-10 rad.
Validity of ``q(t)`` extends a tolerance-width (~1e-8 rad) past an
elbow fold, since the subproblem gates carry a feasibility slack."""
return self._domain_fn()
def __repr__(self) -> str:
return f"Chart(label={self.label}, parameter={self.parameter!r})"
def q(self, t: ArrayLike) -> NDArray[np.float64]:
"""Evaluate the branch. Scalar ``t`` -> ``(7,)``; ``(N,)`` -> ``(N, 7)``.
Rows are ``NaN`` where the branch does not exist at ``t``.
Accuracy: the closed form closes FK to ~1e-13 for ``t`` farther than
1e-5 rad from a fold of the branch (a domain end). Within that distance
the residual grows like the square root of the distance, to ~1e-7 at
the fold itself (measured on the Panda: 2.5e-8 at 1e-6, 9e-8 at 1e-9),
because a double root of a subproblem is only determined to
``sqrt(eps)``. A controller that must sit exactly on a fold should
polish with one Newton step; the tangent's ``rate`` diverging is the
same fold seen from the coordinate side.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
out = self._eval(np.atleast_1d(ts))
return out[0] if scalar else out
def in_limits(self, limits: ArrayLike | None = None) -> tuple[tuple[float, float], ...]:
"""Sub-intervals of :attr:`domain` on which every joint is inside its
range (request A3): the chart's domain under joint limits, exact --
boundaries are the sign-zeros of the smooth ``cos(q_i(t) - c_i) -
cos(h_i)`` margins, bracketed on a grid and bisected, the same
computation ssik's in-limits resolvers run internally.
:param limits: ``(7, 2)`` per-joint ``(lower, upper)``; ``None`` uses the
chain's own limits (a limitless joint counts as ``[-pi, pi]``).
Cached per limits. Empty when no point of the branch is in limits.
One connected self-motion sheet can come back as several arcs: the
pieces the limits leave of it, which is what a controller can hold.
Where the in-limit part is a single point or a sliver no grid point
lands in -- typically two joints at their limits at once, so every
margin touches zero without changing sign -- the branch has no
bracketed arc, and the point is returned as a zero-width arc
``(t, t)``: a minimum of the worst-case limit violation along the
chart that lies in limits up to its own error band (#651) and no more
than 1e-4 rad deep (``seven_r._minimax``).
Angles are compared modulo ``2*pi``, and a joint is in limits when
*some* ``q_i + 2*pi*k`` is. ``q(t)`` itself returns one representative
of each joint angle, which may lie on any turn and outside the joint
box: on the Panda a joint limited to ``[-3.07, -0.07]`` comes back as
``+4.37``, which is admissible, being ``-1.91`` a turn away. So an arc
this method reports is *not* an arc whose ``q(t)`` can be compared
against limits directly; move it into the box first, with
``ssik.postprocess.wrap_to_limits`` (which takes solutions) or by
wrapping each joint to the representative nearest its box centre. A chart
is a geometric branch; on a joint whose range exceeds one turn (UR
arms, ``+-2*pi``) the windings ``q_i + 2*pi*k`` are distinct postures
of that branch, and which one the arm can reach from where it is is
the caller's question -- ``ssik.postprocess.rewrap_to_seed`` picks the
winding nearest a seed.
"""
if self._limit_arcs is None:
return ()
lims = (
self._default_limits
if limits is None
else tuple((float(lo), float(hi)) for lo, hi in np.asarray(limits, dtype=np.float64))
)
arcs = self._limit_cache.get(lims)
if arcs is None:
arcs = self._limit_cache[lims] = self._limit_arcs(lims)
return arcs
def _raw_tangent(
self, ts: NDArray[np.float64]
) -> tuple[NDArray[np.float64], NDArray[np.bool_]]:
"""``dq/dt`` rows for :meth:`tangent` and :meth:`frame`, with folds
repaired: rows where the coordinate derivative is not finite but the
branch exists are replaced by the unit limiting direction, and flagged.
A flagged row carries a direction of unit length rather than a rate, so
the normalizing tails see a well-formed tangent and :meth:`tangent`
substitutes the infinite rate afterwards.
"""
assert self._deriv is not None
raw = np.ascontiguousarray(self._deriv(ts), dtype=np.float64)
fold: NDArray[np.bool_] = np.zeros(raw.shape[0], dtype=bool)
if self._fold_dir is None:
return raw, fold
bad = np.flatnonzero(~np.all(np.isfinite(raw), axis=1))
if not bad.size:
return raw, fold
# A fold is where the coordinate fails but the branch does not, so an
# off-branch t (q is NaN there) is left alone: it has no tangent.
qs = np.atleast_2d(self._eval(ts[bad]))
on_branch = np.all(np.isfinite(qs), axis=1)
if not on_branch.any():
return raw, fold
rows = bad[on_branch]
dirs = self._fold_dir(qs[on_branch])
for row, direction in zip(rows, dirs, strict=True):
if not np.all(np.isfinite(direction)):
continue
# The kernel has two unit vectors; the branch picks one by
# continuity, so orient against the tangent just inside the domain.
# Both sides are tried because a fold is an endpoint of the branch
# and only the inward one evaluates.
for offset in _FOLD_PROBE_OFFSETS:
probe = self._deriv(ts[row] + np.array([-offset, offset]))
live = np.flatnonzero(np.all(np.isfinite(probe), axis=1))
if not live.size:
continue
sign = np.sign(float(direction @ probe[live[0]]))
if sign != 0.0:
raw[row] = sign * direction
fold[row] = True
break
return raw, fold
def tangent(self, t: ArrayLike) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
"""The branch's tangent at ``t``: ``(direction, rate)`` with
``dq/dt = rate * direction``, ``direction`` a unit 7-vector oriented by
increasing ``t`` and ``rate = |dq/dt|``. Scalar ``t`` -> ``((7,), ())``;
``(N,)`` -> ``((N, 7), (N,))``.
The split matters at a fold of the joint coordinates (a gimbal lock of
the shoulder or wrist decomposition): the manifold is smooth there but
the coordinate ``t`` has a critical point, so ``rate`` is ``inf`` while
``direction`` stays well defined. Both are ``NaN`` where the branch does
not exist, and that is the only case that gives ``NaN``.
Folds are not rare and they are exactly where a caller lands: an
endpoint of a bounded domain is usually one (an endpoint cut by a
shoulder or wrist gate instead keeps a finite rate), so :meth:`sample`
with ``limits=True`` returns them. ``direction`` there is the kernel of the
chain's ``6xN`` Jacobian (see :func:`_null_tangent`), oriented by the
branch just inside the domain -- exact, not extrapolated, because the
Jacobian is healthy at a fold and only the coordinate normalization is
not.
Exact on both families. SRS: the closed form (rotor derivative of the
swivel orbit pushed through the two 3-axis decompositions).
Spherical-shoulder: implicit differentiation of ``FK(q(t)) = T`` with
``q_6 = t`` -- ``J[:, :6] dq' = -J[:, 6]`` on the chain's spatial
Jacobian, a 6x6 solve, singular exactly at a fold of ``t``, where the
kernel above takes over.
The tail past the raw tangent runs natively where the extension is
present (``_ssik_native.chart_tangent``); :func:`_tangent_tail` is the
reference.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
raw, fold = self._raw_tangent(np.atleast_1d(ts))
# The native tail takes the 7R charts' (N, 7) tangents; a 6R chart's
# (zero-dimensional, all-NaN) ones take the reference.
native = _native_fn("chart_tangent") if raw.shape[1] == 7 else None
if native is None:
direction, rate = _tangent_tail(raw)
else:
out: tuple[NDArray[np.float64], NDArray[np.float64]] = native(raw)
direction, rate = out
rate = np.where(fold, np.inf, rate)
return (direction[0], rate[0]) if scalar else (direction, rate)
def frame(
self, t: ArrayLike, metric: ArrayLike | None = None
) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
"""The chart frame at ``t`` (request D1): the unit tangent ``d`` and a
``(7, 6)`` basis ``V`` of its complement, orthogonal to ``d`` in
``metric``. Scalar ``t`` -> ``((7,), (7, 6))``; ``(N,)`` ->
``((N, 7), (N, 7, 6))``, as :meth:`tangent`.
With ``metric=None`` the complement is ``ker(J)^perp = row(J)``, the
kinematic split. With ``metric=M`` (SPD ``(7, 7)``, e.g. the mass matrix
from gafro) it is the ``M``-orthogonal complement, ``d^T M v = 0``: the
dynamically consistent split, so a task correction commanded in
``span(V)`` does no work along the self-motion. The columns of ``V`` are
Euclidean-orthonormal (not ``M``-orthonormal) and are built by the
Householder reflection carrying ``e_0`` onto ``w = M d / |M d|``, so
``V`` varies continuously along the arc except at the single direction
``w == +e_0``, where ``u = e_0 - w`` vanishes and the limit depends on
the direction of approach (``w == -e_0`` is benign: ``u -> e_0`` from
every side). That defect cannot be removed, only moved: a continuous
``V(w)`` on the whole sphere is a parallelisation of ``S^6``, which does
not exist, and puncturing one point is the best possible -- an arc
misses a codimension-6 point, where the ``sign(w_0)`` variant that fixes
the conditioning would put a codimension-1 equator in its way. Unlike an
SVD's per-evaluation sign flips, this is a measure-zero event. ``NaN``
off the branch. Defined at a fold, on the same recovered tangent
:meth:`tangent` reports.
The tail past the tangent runs natively where the extension is present
(``_ssik_native.chart_frame``); :func:`_frame_tail` is the reference.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
raw, _fold = self._raw_tangent(np.atleast_1d(ts))
m = None if metric is None else np.ascontiguousarray(metric, dtype=np.float64)
native = _native_fn("chart_frame")
if native is None:
d, v = _frame_tail(raw, m)
else:
out: tuple[NDArray[np.float64], NDArray[np.float64]] = native(raw, m)
d, v = out
return (d[0], v[0]) if scalar else (d, v)
def pullback_metric(self, t: ArrayLike, metric: Metric | None = None) -> NDArray[np.float64]:
"""The joint-space metric pulled back to the chart coordinate:
``g(t) = Dφ(t)^T G(q(t)) Dφ(t)`` with ``Dφ = dq/dt``, the ``1x1`` metric
tensor of the branch in ``t``. Scalar ``t`` -> ``()``; ``(N,)`` ->
``(N,)``.
``metric`` is ``G`` as in :meth:`length`: ``None`` (Euclidean, so
``g = |dq/dt|^2``), a constant SPD ``(dof, dof)`` matrix, or a batched
callable ``(N, dof) -> (N, dof, dof)`` such as the mass matrix.
``sqrt(g(t)) dt`` is the arc-length element whose integral
:meth:`length` approximates by chords.
Exact: built on :meth:`tangent`, ``g = rate^2 * d^T G d``. At a fold of
``t`` the rate diverges and so does ``g`` (the coordinate is singular
there, not the manifold; ``d^T G d`` stays finite). ``NaN`` off the
branch. A zero-dimensional chart has no coordinate and raises.
"""
if self.dimension == 0:
raise ValueError("pullback_metric: a zero-dimensional chart has no coordinate")
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
tt = np.atleast_1d(ts)
d, rate = self.tangent(tt)
mfn = _metric_fn(metric)
if mfn is None:
dgd = np.ones(tt.shape[0])
else:
q = self.q(tt)
ok = np.all(np.isfinite(q), axis=1)
dgd = np.full(tt.shape[0], np.nan)
if ok.any():
G = mfn(q[ok])
dgd[ok] = np.einsum("ni,nij,nj->n", d[ok], G, d[ok])
g = np.asarray(rate**2 * dgd, dtype=np.float64)
return g[0] if scalar else g
def restrict(
self,
fn: Callable[[NDArray[np.float64]], NDArray[np.float64]],
*,
clearance: float = 0.0,
tol: float = 1e-10,
samples: int = 181,
conservative: bool | None = None,
) -> Chart:
"""The same branch with its domain narrowed to where ``fn`` allows it.
The chart is the geometric object; a constraint that is not a property of
the kinematics -- self-collision, a task-space obstacle, a keep-out region
-- is a restriction the caller composes on top. This is that composition,
and it returns a :class:`Chart`, so ``q``, ``tangent``, ``contains``,
``curve`` and :meth:`in_limits` all respect it, and restrictions chain::
reachable = chart.restrict(clearance_fn, clearance=0.02)
usable = reachable.restrict(keep_out_fn)
``fn`` is batched: it takes ``(N, dof)`` and returns ``(N,)``. Two forms,
with different accuracy:
* **margin** (float) -- ``>= 0`` feasible. Boundaries are the zeros of
``fn(q(t)) - clearance``, bracketed on the sample grid and bisected to
``tol``, so they are exact to ``tol`` regardless of ``samples``. This is
the form a collision backend already speaks (FCL signed distance,
MuJoCo contact depth), and ``clearance`` buys a safety band for free.
* **boolean** -- ``True`` feasible. Wrapped as a ``+/-1`` margin, so the
bisection still converges, but on a step function: the boundary is only
as good as the assumption that no forbidden region is narrower than the
grid step. ``conservative`` therefore defaults to ``True`` here and
shrinks every kept interval by one step; pass ``False`` to opt out.
``q(t)`` is ``NaN`` off the branch, and ``fn`` is never called there --
those samples are infeasible by construction.
A zero-dimensional chart (a 6R arm's isolated solution) is kept or dropped
whole; an empty result means this branch is entirely forbidden, and
:meth:`SelfMotionManifold.restrict` drops such charts from the manifold.
:param fn: batched margin or predicate over configurations.
:param clearance: required margin, in the margin's own units. Ignored for
a boolean ``fn``.
:param tol: bisection tolerance on the boundary, in ``t``.
:param samples: grid points per ``2*pi`` of parameter used to bracket
boundaries. Raise it when the forbidden regions are narrow.
:param conservative: shrink each kept interval by one grid step on both
sides. Defaults to ``True`` for a boolean ``fn`` and ``False`` for a
margin, where the bisected boundary is already exact.
:returns: a new :class:`Chart`, same ``label`` and same ``q(t)``.
"""
arcs = _restrict_arcs(
self._eval,
self.domain,
fn,
clearance=clearance,
tol=tol,
samples=samples,
conservative=conservative,
periodic=self.periodic,
)
parent_limit_arcs = self._limit_arcs
def limit_arcs(lims: Limits) -> tuple[tuple[float, float], ...]:
if parent_limit_arcs is None:
return ()
from ssik.solvers.seven_r._feasible_param import intersect
return tuple(intersect(list(parent_limit_arcs(lims)), list(arcs)))
return Chart(
label=self.label,
parameter=self.parameter,
periodic=self.periodic,
eval_fn=self._eval,
domain_fn=lambda: arcs,
deriv_fn=self._deriv,
fold_dir_fn=self._fold_dir,
limit_arcs_fn=None if parent_limit_arcs is None else limit_arcs,
default_limits=self._default_limits,
)
def contains(self, t: float, tol: float = 1e-9) -> bool:
"""True iff ``t`` lies in :attr:`domain` (wrapped first when periodic)."""
tt = float(_wrap_pi(np.asarray(t))) if self.periodic else float(t)
return any(lo - tol <= tt <= hi + tol for lo, hi in self.domain)
def curve(self, n: int = 181) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]:
"""Dense sample of the branch, continuous in ``t``, one segment per
domain interval.
Each segment is ``(ts, qs)``: ``ts`` of shape ``(N,)`` with about ``n``
points per full ``2*pi`` of parameter, and ``qs`` of shape ``(N, 7)``
unwrapped along ``t`` so no joint jumps by ``2*pi`` between consecutive
rows. Segments are separate pieces of the branch; their end rows are
not adjacent on the manifold.
"""
segments: list[tuple[NDArray[np.float64], NDArray[np.float64]]] = []
for lo, hi in self.domain:
m = max(round(n * (hi - lo) / _TWO_PI), 2)
ts = np.linspace(lo, hi, m, endpoint=not (self.periodic and hi - lo >= _TWO_PI - 1e-9))
qs = self._eval(ts)
ok = np.all(np.isfinite(qs), axis=1)
ts, qs = ts[ok], qs[ok]
if ts.shape[0] == 0:
continue
segments.append((ts, np.unwrap(qs, axis=0)))
return segments
def length(
self,
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-5,
) -> float:
"""Arc length of the branch, summed over its pieces (request C1).
``metric`` is ``None`` (Euclidean joint space), a constant SPD
``(dof, dof)`` matrix, or a batched callable ``(N, dof) -> (N, dof, dof)``
-- the mass matrix from gafro gives kinetic-energy distance. ``limits``
restricts the branch to :meth:`in_limits` first (``True`` for the chain's
own limits, or a ``(dof, 2)`` array). See :meth:`sample` for ``tol``.
Zero on a zero-dimensional chart.
"""
return float(sum(s[-1] for _, _, s in self._polylines(metric, limits, tol)))
def sample(
self,
n: int,
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-5,
) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]:
"""About ``n`` points spaced uniformly in arc length (request C1).
Uniform in ``t`` is not uniform on the manifold: at a fold of the
coordinate the rate ``|dq/dt|`` diverges (see :meth:`tangent`), so a
``t``-grid bunches up in joint space in the middle of a branch and
leaves gaps at its ends. This places points at equal arc length
instead, in ``metric`` (see :meth:`length`), so a sum over the samples
converges to the curve's value rather than to a sample-density artefact.
Returns one ``(ts, qs)`` per piece, like :meth:`curve` (``qs`` unwrapped
along the piece). The spacing ``h = L / n`` is shared by all pieces; an
open piece gets ``round(L_i / h) + 1`` points (at least two) including
both ends, a closed one ``round(L_i / h)`` (at least three) with the
seam spaced like the rest. Every point is ``q(t)`` evaluated exactly on
the chart; only its ``t`` comes from the arc-length inversion.
The length is the midpoint-metric sum over a polyline of ``q(t)``,
refined by bisection until no chord's midpoint strays more than ``tol``
(rad) from the curve; near a fold ``q`` goes like ``sqrt(t - t_0)`` and
the refinement concentrates there on its own. Spacing is uniform to
about ``tol``. A position-dependent ``metric`` is evaluated once per
chord, batched.
The returned ``qs`` carry whatever turn the branch is on, as
:meth:`in_limits` describes: with ``limits`` set they are on an
admissible arc, which is not the same as being inside the joint box
numerically.
"""
if n < 1:
raise ValueError("sample: n must be >= 1")
lines = self._polylines(metric, limits, tol)
total = sum(s[-1] for _, _, s in lines)
if not lines:
return []
if self.dimension == 0 or total <= 0.0:
return [(ts[:1], qs[:1]) for ts, qs, _ in lines]
h = total / n
out: list[tuple[NDArray[np.float64], NDArray[np.float64]]] = []
for ts, qs, s in lines:
L = s[-1]
closed = _closes(qs)
if closed:
m = max(round(L / h), 3)
targets = np.arange(m) * (L / m)
else:
m = max(round(L / h) + 1, 2)
targets = np.linspace(0.0, L, m)
k = np.clip(np.searchsorted(s, targets, side="right") - 1, 0, s.shape[0] - 2)
ds = s[k + 1] - s[k]
f = np.where(ds > 0.0, (targets - s[k]) / np.where(ds > 0.0, ds, 1.0), 0.0)
t_new = ts[k] + f * (ts[k + 1] - ts[k])
q_new = self._eval_t(t_new)
ok = np.all(np.isfinite(q_new), axis=1)
out.append((t_new[ok], np.unwrap(q_new[ok], axis=0)))
return out
def _eval_t(self, ts: NDArray[np.float64]) -> NDArray[np.float64]:
"""``q(t)`` accepting ``t`` past ``pi`` on a periodic chart (a piece
joined across the seam)."""
return self._eval(_wrap_pi(ts) if self.periodic else ts)
def _pieces(self, limits: ArrayLike | bool | None) -> list[tuple[float, float]]:
"""The intervals to measure: the domain, or its in-limits part; on a
periodic chart two pieces meeting at the ``+-pi`` seam are one piece."""
if limits is None or limits is False:
arcs = list(self.domain)
else:
arcs = list(self.in_limits(None if limits is True else limits))
arcs.sort()
if (
self.periodic
and len(arcs) >= 2
and abs(arcs[0][0] + np.pi) < 1e-9
and abs(arcs[-1][1] - np.pi) < 1e-9
):
first = arcs.pop(0)
lo, _ = arcs.pop()
arcs.append((lo, first[1] + _TWO_PI))
return arcs
def _polylines(
self, metric: Metric | None, limits: ArrayLike | bool | None, tol: float
) -> list[tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]]:
"""Per piece ``(ts, qs, s)``: an adaptive polyline of the branch and the
cumulative metric length along it."""
if self.dimension == 0:
q0 = self.q(0.0)
if not np.all(np.isfinite(q0)):
return []
return [(np.zeros(1), q0[None, :], np.zeros(1))]
mfn = _metric_fn(metric)
lines = []
for lo, hi in self._pieces(limits):
ts, qs = _adaptive_polyline(self._eval_t, lo, hi, tol)
if ts.shape[0] < 2:
continue
dq = _wrap_pi(qs[1:] - qs[:-1])
if mfn is None:
seg = np.linalg.norm(dq, axis=1)
else:
M = mfn(qs[:-1] + 0.5 * dq)
seg = np.sqrt(np.maximum(np.einsum("ni,nij,nj->n", dq, M, dq), 0.0))
lines.append((ts, np.unwrap(qs, axis=0), np.concatenate(([0.0], np.cumsum(seg)))))
return lines
|
dimension
instance-attribute
dimension = 0 if parameter == 'point' else 1
0 for an isolated solution (6R arms), 1 for a one-parameter branch.
domain
cached
property
domain: tuple[tuple[float, float], ...]
Intervals (t_lo, t_hi) on which the branch exists: the full circle
for "swivel"; for "q6" the exact elbow-fold arcs intersected with
the shoulder and wrist gates, the latter refined to ~1e-10 rad.
Validity of q(t) extends a tolerance-width (~1e-8 rad) past an
elbow fold, since the subproblem gates carry a feasibility slack.
q
q(t: ArrayLike) -> NDArray[np.float64]
Evaluate the branch. Scalar t -> (7,); (N,) -> (N, 7).
Rows are NaN where the branch does not exist at t.
Accuracy: the closed form closes FK to ~1e-13 for t farther than
1e-5 rad from a fold of the branch (a domain end). Within that distance
the residual grows like the square root of the distance, to ~1e-7 at
the fold itself (measured on the Panda: 2.5e-8 at 1e-6, 9e-8 at 1e-9),
because a double root of a subproblem is only determined to
sqrt(eps). A controller that must sit exactly on a fold should
polish with one Newton step; the tangent's rate diverging is the
same fold seen from the coordinate side.
Source code in src/ssik/chart.py
| def q(self, t: ArrayLike) -> NDArray[np.float64]:
"""Evaluate the branch. Scalar ``t`` -> ``(7,)``; ``(N,)`` -> ``(N, 7)``.
Rows are ``NaN`` where the branch does not exist at ``t``.
Accuracy: the closed form closes FK to ~1e-13 for ``t`` farther than
1e-5 rad from a fold of the branch (a domain end). Within that distance
the residual grows like the square root of the distance, to ~1e-7 at
the fold itself (measured on the Panda: 2.5e-8 at 1e-6, 9e-8 at 1e-9),
because a double root of a subproblem is only determined to
``sqrt(eps)``. A controller that must sit exactly on a fold should
polish with one Newton step; the tangent's ``rate`` diverging is the
same fold seen from the coordinate side.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
out = self._eval(np.atleast_1d(ts))
return out[0] if scalar else out
|
in_limits
in_limits(limits: ArrayLike | None = None) -> tuple[tuple[float, float], ...]
Sub-intervals of :attr:domain on which every joint is inside its
range (request A3): the chart's domain under joint limits, exact --
boundaries are the sign-zeros of the smooth cos(q_i(t) - c_i) -
cos(h_i) margins, bracketed on a grid and bisected, the same
computation ssik's in-limits resolvers run internally.
Parameters:
| Name |
Type |
Description |
Default |
limits
|
ArrayLike | None
|
(7, 2) per-joint (lower, upper); None uses the
chain's own limits (a limitless joint counts as [-pi, pi]).
Cached per limits. Empty when no point of the branch is in limits.
One connected self-motion sheet can come back as several arcs: the
pieces the limits leave of it, which is what a controller can hold.
Where the in-limit part is a single point or a sliver no grid point
lands in -- typically two joints at their limits at once, so every
margin touches zero without changing sign -- the branch has no
bracketed arc, and the point is returned as a zero-width arc
(t, t): a minimum of the worst-case limit violation along the
chart that lies in limits up to its own error band (#651) and no more
than 1e-4 rad deep (seven_r._minimax).
Angles are compared modulo 2*pi, and a joint is in limits when
some q_i + 2*pi*k is. q(t) itself returns one representative
of each joint angle, which may lie on any turn and outside the joint
box: on the Panda a joint limited to [-3.07, -0.07] comes back as
+4.37, which is admissible, being -1.91 a turn away. So an arc
this method reports is not an arc whose q(t) can be compared
against limits directly; move it into the box first, with
ssik.postprocess.wrap_to_limits (which takes solutions) or by
wrapping each joint to the representative nearest its box centre. A chart
is a geometric branch; on a joint whose range exceeds one turn (UR
arms, +-2*pi) the windings q_i + 2*pi*k are distinct postures
of that branch, and which one the arm can reach from where it is is
the caller's question -- ssik.postprocess.rewrap_to_seed picks the
winding nearest a seed.
|
None
|
Source code in src/ssik/chart.py
| def in_limits(self, limits: ArrayLike | None = None) -> tuple[tuple[float, float], ...]:
"""Sub-intervals of :attr:`domain` on which every joint is inside its
range (request A3): the chart's domain under joint limits, exact --
boundaries are the sign-zeros of the smooth ``cos(q_i(t) - c_i) -
cos(h_i)`` margins, bracketed on a grid and bisected, the same
computation ssik's in-limits resolvers run internally.
:param limits: ``(7, 2)`` per-joint ``(lower, upper)``; ``None`` uses the
chain's own limits (a limitless joint counts as ``[-pi, pi]``).
Cached per limits. Empty when no point of the branch is in limits.
One connected self-motion sheet can come back as several arcs: the
pieces the limits leave of it, which is what a controller can hold.
Where the in-limit part is a single point or a sliver no grid point
lands in -- typically two joints at their limits at once, so every
margin touches zero without changing sign -- the branch has no
bracketed arc, and the point is returned as a zero-width arc
``(t, t)``: a minimum of the worst-case limit violation along the
chart that lies in limits up to its own error band (#651) and no more
than 1e-4 rad deep (``seven_r._minimax``).
Angles are compared modulo ``2*pi``, and a joint is in limits when
*some* ``q_i + 2*pi*k`` is. ``q(t)`` itself returns one representative
of each joint angle, which may lie on any turn and outside the joint
box: on the Panda a joint limited to ``[-3.07, -0.07]`` comes back as
``+4.37``, which is admissible, being ``-1.91`` a turn away. So an arc
this method reports is *not* an arc whose ``q(t)`` can be compared
against limits directly; move it into the box first, with
``ssik.postprocess.wrap_to_limits`` (which takes solutions) or by
wrapping each joint to the representative nearest its box centre. A chart
is a geometric branch; on a joint whose range exceeds one turn (UR
arms, ``+-2*pi``) the windings ``q_i + 2*pi*k`` are distinct postures
of that branch, and which one the arm can reach from where it is is
the caller's question -- ``ssik.postprocess.rewrap_to_seed`` picks the
winding nearest a seed.
"""
if self._limit_arcs is None:
return ()
lims = (
self._default_limits
if limits is None
else tuple((float(lo), float(hi)) for lo, hi in np.asarray(limits, dtype=np.float64))
)
arcs = self._limit_cache.get(lims)
if arcs is None:
arcs = self._limit_cache[lims] = self._limit_arcs(lims)
return arcs
|
tangent
tangent(t: ArrayLike) -> tuple[NDArray[np.float64], NDArray[np.float64]]
The branch's tangent at t: (direction, rate) with
dq/dt = rate * direction, direction a unit 7-vector oriented by
increasing t and rate = |dq/dt|. Scalar t -> ((7,), ());
(N,) -> ((N, 7), (N,)).
The split matters at a fold of the joint coordinates (a gimbal lock of
the shoulder or wrist decomposition): the manifold is smooth there but
the coordinate t has a critical point, so rate is inf while
direction stays well defined. Both are NaN where the branch does
not exist, and that is the only case that gives NaN.
Folds are not rare and they are exactly where a caller lands: an
endpoint of a bounded domain is usually one (an endpoint cut by a
shoulder or wrist gate instead keeps a finite rate), so :meth:sample
with limits=True returns them. direction there is the kernel of the
chain's 6xN Jacobian (see :func:_null_tangent), oriented by the
branch just inside the domain -- exact, not extrapolated, because the
Jacobian is healthy at a fold and only the coordinate normalization is
not.
Exact on both families. SRS: the closed form (rotor derivative of the
swivel orbit pushed through the two 3-axis decompositions).
Spherical-shoulder: implicit differentiation of FK(q(t)) = T with
q_6 = t -- J[:, :6] dq' = -J[:, 6] on the chain's spatial
Jacobian, a 6x6 solve, singular exactly at a fold of t, where the
kernel above takes over.
The tail past the raw tangent runs natively where the extension is
present (_ssik_native.chart_tangent); :func:_tangent_tail is the
reference.
Source code in src/ssik/chart.py
| def tangent(self, t: ArrayLike) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
"""The branch's tangent at ``t``: ``(direction, rate)`` with
``dq/dt = rate * direction``, ``direction`` a unit 7-vector oriented by
increasing ``t`` and ``rate = |dq/dt|``. Scalar ``t`` -> ``((7,), ())``;
``(N,)`` -> ``((N, 7), (N,))``.
The split matters at a fold of the joint coordinates (a gimbal lock of
the shoulder or wrist decomposition): the manifold is smooth there but
the coordinate ``t`` has a critical point, so ``rate`` is ``inf`` while
``direction`` stays well defined. Both are ``NaN`` where the branch does
not exist, and that is the only case that gives ``NaN``.
Folds are not rare and they are exactly where a caller lands: an
endpoint of a bounded domain is usually one (an endpoint cut by a
shoulder or wrist gate instead keeps a finite rate), so :meth:`sample`
with ``limits=True`` returns them. ``direction`` there is the kernel of the
chain's ``6xN`` Jacobian (see :func:`_null_tangent`), oriented by the
branch just inside the domain -- exact, not extrapolated, because the
Jacobian is healthy at a fold and only the coordinate normalization is
not.
Exact on both families. SRS: the closed form (rotor derivative of the
swivel orbit pushed through the two 3-axis decompositions).
Spherical-shoulder: implicit differentiation of ``FK(q(t)) = T`` with
``q_6 = t`` -- ``J[:, :6] dq' = -J[:, 6]`` on the chain's spatial
Jacobian, a 6x6 solve, singular exactly at a fold of ``t``, where the
kernel above takes over.
The tail past the raw tangent runs natively where the extension is
present (``_ssik_native.chart_tangent``); :func:`_tangent_tail` is the
reference.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
raw, fold = self._raw_tangent(np.atleast_1d(ts))
# The native tail takes the 7R charts' (N, 7) tangents; a 6R chart's
# (zero-dimensional, all-NaN) ones take the reference.
native = _native_fn("chart_tangent") if raw.shape[1] == 7 else None
if native is None:
direction, rate = _tangent_tail(raw)
else:
out: tuple[NDArray[np.float64], NDArray[np.float64]] = native(raw)
direction, rate = out
rate = np.where(fold, np.inf, rate)
return (direction[0], rate[0]) if scalar else (direction, rate)
|
frame
frame(
t: ArrayLike, metric: ArrayLike | None = None
) -> tuple[NDArray[np.float64], NDArray[np.float64]]
The chart frame at t (request D1): the unit tangent d and a
(7, 6) basis V of its complement, orthogonal to d in
metric. Scalar t -> ((7,), (7, 6)); (N,) ->
((N, 7), (N, 7, 6)), as :meth:tangent.
With metric=None the complement is ker(J)^perp = row(J), the
kinematic split. With metric=M (SPD (7, 7), e.g. the mass matrix
from gafro) it is the M-orthogonal complement, d^T M v = 0: the
dynamically consistent split, so a task correction commanded in
span(V) does no work along the self-motion. The columns of V are
Euclidean-orthonormal (not M-orthonormal) and are built by the
Householder reflection carrying e_0 onto w = M d / |M d|, so
V varies continuously along the arc except at the single direction
w == +e_0, where u = e_0 - w vanishes and the limit depends on
the direction of approach (w == -e_0 is benign: u -> e_0 from
every side). That defect cannot be removed, only moved: a continuous
V(w) on the whole sphere is a parallelisation of S^6, which does
not exist, and puncturing one point is the best possible -- an arc
misses a codimension-6 point, where the sign(w_0) variant that fixes
the conditioning would put a codimension-1 equator in its way. Unlike an
SVD's per-evaluation sign flips, this is a measure-zero event. NaN
off the branch. Defined at a fold, on the same recovered tangent
:meth:tangent reports.
The tail past the tangent runs natively where the extension is present
(_ssik_native.chart_frame); :func:_frame_tail is the reference.
Source code in src/ssik/chart.py
| def frame(
self, t: ArrayLike, metric: ArrayLike | None = None
) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
"""The chart frame at ``t`` (request D1): the unit tangent ``d`` and a
``(7, 6)`` basis ``V`` of its complement, orthogonal to ``d`` in
``metric``. Scalar ``t`` -> ``((7,), (7, 6))``; ``(N,)`` ->
``((N, 7), (N, 7, 6))``, as :meth:`tangent`.
With ``metric=None`` the complement is ``ker(J)^perp = row(J)``, the
kinematic split. With ``metric=M`` (SPD ``(7, 7)``, e.g. the mass matrix
from gafro) it is the ``M``-orthogonal complement, ``d^T M v = 0``: the
dynamically consistent split, so a task correction commanded in
``span(V)`` does no work along the self-motion. The columns of ``V`` are
Euclidean-orthonormal (not ``M``-orthonormal) and are built by the
Householder reflection carrying ``e_0`` onto ``w = M d / |M d|``, so
``V`` varies continuously along the arc except at the single direction
``w == +e_0``, where ``u = e_0 - w`` vanishes and the limit depends on
the direction of approach (``w == -e_0`` is benign: ``u -> e_0`` from
every side). That defect cannot be removed, only moved: a continuous
``V(w)`` on the whole sphere is a parallelisation of ``S^6``, which does
not exist, and puncturing one point is the best possible -- an arc
misses a codimension-6 point, where the ``sign(w_0)`` variant that fixes
the conditioning would put a codimension-1 equator in its way. Unlike an
SVD's per-evaluation sign flips, this is a measure-zero event. ``NaN``
off the branch. Defined at a fold, on the same recovered tangent
:meth:`tangent` reports.
The tail past the tangent runs natively where the extension is present
(``_ssik_native.chart_frame``); :func:`_frame_tail` is the reference.
"""
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
raw, _fold = self._raw_tangent(np.atleast_1d(ts))
m = None if metric is None else np.ascontiguousarray(metric, dtype=np.float64)
native = _native_fn("chart_frame")
if native is None:
d, v = _frame_tail(raw, m)
else:
out: tuple[NDArray[np.float64], NDArray[np.float64]] = native(raw, m)
d, v = out
return (d[0], v[0]) if scalar else (d, v)
|
pullback_metric
pullback_metric(t: ArrayLike, metric: Metric | None = None) -> NDArray[np.float64]
The joint-space metric pulled back to the chart coordinate:
g(t) = Dφ(t)^T G(q(t)) Dφ(t) with Dφ = dq/dt, the 1x1 metric
tensor of the branch in t. Scalar t -> (); (N,) ->
(N,).
metric is G as in :meth:length: None (Euclidean, so
g = |dq/dt|^2), a constant SPD (dof, dof) matrix, or a batched
callable (N, dof) -> (N, dof, dof) such as the mass matrix.
sqrt(g(t)) dt is the arc-length element whose integral
:meth:length approximates by chords.
Exact: built on :meth:tangent, g = rate^2 * d^T G d. At a fold of
t the rate diverges and so does g (the coordinate is singular
there, not the manifold; d^T G d stays finite). NaN off the
branch. A zero-dimensional chart has no coordinate and raises.
Source code in src/ssik/chart.py
| def pullback_metric(self, t: ArrayLike, metric: Metric | None = None) -> NDArray[np.float64]:
"""The joint-space metric pulled back to the chart coordinate:
``g(t) = Dφ(t)^T G(q(t)) Dφ(t)`` with ``Dφ = dq/dt``, the ``1x1`` metric
tensor of the branch in ``t``. Scalar ``t`` -> ``()``; ``(N,)`` ->
``(N,)``.
``metric`` is ``G`` as in :meth:`length`: ``None`` (Euclidean, so
``g = |dq/dt|^2``), a constant SPD ``(dof, dof)`` matrix, or a batched
callable ``(N, dof) -> (N, dof, dof)`` such as the mass matrix.
``sqrt(g(t)) dt`` is the arc-length element whose integral
:meth:`length` approximates by chords.
Exact: built on :meth:`tangent`, ``g = rate^2 * d^T G d``. At a fold of
``t`` the rate diverges and so does ``g`` (the coordinate is singular
there, not the manifold; ``d^T G d`` stays finite). ``NaN`` off the
branch. A zero-dimensional chart has no coordinate and raises.
"""
if self.dimension == 0:
raise ValueError("pullback_metric: a zero-dimensional chart has no coordinate")
ts = np.asarray(t, dtype=np.float64)
scalar = ts.ndim == 0
tt = np.atleast_1d(ts)
d, rate = self.tangent(tt)
mfn = _metric_fn(metric)
if mfn is None:
dgd = np.ones(tt.shape[0])
else:
q = self.q(tt)
ok = np.all(np.isfinite(q), axis=1)
dgd = np.full(tt.shape[0], np.nan)
if ok.any():
G = mfn(q[ok])
dgd[ok] = np.einsum("ni,nij,nj->n", d[ok], G, d[ok])
g = np.asarray(rate**2 * dgd, dtype=np.float64)
return g[0] if scalar else g
|
restrict
restrict(
fn: Callable[[NDArray[float64]], NDArray[float64]],
*,
clearance: float = 0.0,
tol: float = 1e-10,
samples: int = 181,
conservative: bool | None = None,
) -> Chart
The same branch with its domain narrowed to where fn allows it.
The chart is the geometric object; a constraint that is not a property of
the kinematics -- self-collision, a task-space obstacle, a keep-out region
-- is a restriction the caller composes on top. This is that composition,
and it returns a :class:Chart, so q, tangent, contains,
curve and :meth:in_limits all respect it, and restrictions chain::
reachable = chart.restrict(clearance_fn, clearance=0.02)
usable = reachable.restrict(keep_out_fn)
fn is batched: it takes (N, dof) and returns (N,). Two forms,
with different accuracy:
- margin (float) --
>= 0 feasible. Boundaries are the zeros of
fn(q(t)) - clearance, bracketed on the sample grid and bisected to
tol, so they are exact to tol regardless of samples. This is
the form a collision backend already speaks (FCL signed distance,
MuJoCo contact depth), and clearance buys a safety band for free.
- boolean --
True feasible. Wrapped as a +/-1 margin, so the
bisection still converges, but on a step function: the boundary is only
as good as the assumption that no forbidden region is narrower than the
grid step. conservative therefore defaults to True here and
shrinks every kept interval by one step; pass False to opt out.
q(t) is NaN off the branch, and fn is never called there --
those samples are infeasible by construction.
A zero-dimensional chart (a 6R arm's isolated solution) is kept or dropped
whole; an empty result means this branch is entirely forbidden, and
:meth:SelfMotionManifold.restrict drops such charts from the manifold.
Parameters:
| Name |
Type |
Description |
Default |
fn
|
Callable[[NDArray[float64]], NDArray[float64]]
|
batched margin or predicate over configurations.
|
required
|
clearance
|
float
|
required margin, in the margin's own units. Ignored for
a boolean fn.
|
0.0
|
tol
|
float
|
bisection tolerance on the boundary, in t.
|
1e-10
|
samples
|
int
|
grid points per 2*pi of parameter used to bracket
boundaries. Raise it when the forbidden regions are narrow.
|
181
|
conservative
|
bool | None
|
shrink each kept interval by one grid step on both
sides. Defaults to True for a boolean fn and False for a
margin, where the bisected boundary is already exact.
|
None
|
Returns:
| Type |
Description |
Chart
|
a new :class:Chart, same label and same q(t).
|
Source code in src/ssik/chart.py
| def restrict(
self,
fn: Callable[[NDArray[np.float64]], NDArray[np.float64]],
*,
clearance: float = 0.0,
tol: float = 1e-10,
samples: int = 181,
conservative: bool | None = None,
) -> Chart:
"""The same branch with its domain narrowed to where ``fn`` allows it.
The chart is the geometric object; a constraint that is not a property of
the kinematics -- self-collision, a task-space obstacle, a keep-out region
-- is a restriction the caller composes on top. This is that composition,
and it returns a :class:`Chart`, so ``q``, ``tangent``, ``contains``,
``curve`` and :meth:`in_limits` all respect it, and restrictions chain::
reachable = chart.restrict(clearance_fn, clearance=0.02)
usable = reachable.restrict(keep_out_fn)
``fn`` is batched: it takes ``(N, dof)`` and returns ``(N,)``. Two forms,
with different accuracy:
* **margin** (float) -- ``>= 0`` feasible. Boundaries are the zeros of
``fn(q(t)) - clearance``, bracketed on the sample grid and bisected to
``tol``, so they are exact to ``tol`` regardless of ``samples``. This is
the form a collision backend already speaks (FCL signed distance,
MuJoCo contact depth), and ``clearance`` buys a safety band for free.
* **boolean** -- ``True`` feasible. Wrapped as a ``+/-1`` margin, so the
bisection still converges, but on a step function: the boundary is only
as good as the assumption that no forbidden region is narrower than the
grid step. ``conservative`` therefore defaults to ``True`` here and
shrinks every kept interval by one step; pass ``False`` to opt out.
``q(t)`` is ``NaN`` off the branch, and ``fn`` is never called there --
those samples are infeasible by construction.
A zero-dimensional chart (a 6R arm's isolated solution) is kept or dropped
whole; an empty result means this branch is entirely forbidden, and
:meth:`SelfMotionManifold.restrict` drops such charts from the manifold.
:param fn: batched margin or predicate over configurations.
:param clearance: required margin, in the margin's own units. Ignored for
a boolean ``fn``.
:param tol: bisection tolerance on the boundary, in ``t``.
:param samples: grid points per ``2*pi`` of parameter used to bracket
boundaries. Raise it when the forbidden regions are narrow.
:param conservative: shrink each kept interval by one grid step on both
sides. Defaults to ``True`` for a boolean ``fn`` and ``False`` for a
margin, where the bisected boundary is already exact.
:returns: a new :class:`Chart`, same ``label`` and same ``q(t)``.
"""
arcs = _restrict_arcs(
self._eval,
self.domain,
fn,
clearance=clearance,
tol=tol,
samples=samples,
conservative=conservative,
periodic=self.periodic,
)
parent_limit_arcs = self._limit_arcs
def limit_arcs(lims: Limits) -> tuple[tuple[float, float], ...]:
if parent_limit_arcs is None:
return ()
from ssik.solvers.seven_r._feasible_param import intersect
return tuple(intersect(list(parent_limit_arcs(lims)), list(arcs)))
return Chart(
label=self.label,
parameter=self.parameter,
periodic=self.periodic,
eval_fn=self._eval,
domain_fn=lambda: arcs,
deriv_fn=self._deriv,
fold_dir_fn=self._fold_dir,
limit_arcs_fn=None if parent_limit_arcs is None else limit_arcs,
default_limits=self._default_limits,
)
|
contains
contains(t: float, tol: float = 1e-09) -> bool
True iff t lies in :attr:domain (wrapped first when periodic).
Source code in src/ssik/chart.py
| def contains(self, t: float, tol: float = 1e-9) -> bool:
"""True iff ``t`` lies in :attr:`domain` (wrapped first when periodic)."""
tt = float(_wrap_pi(np.asarray(t))) if self.periodic else float(t)
return any(lo - tol <= tt <= hi + tol for lo, hi in self.domain)
|
curve
curve(n: int = 181) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]
Dense sample of the branch, continuous in t, one segment per
domain interval.
Each segment is (ts, qs): ts of shape (N,) with about n
points per full 2*pi of parameter, and qs of shape (N, 7)
unwrapped along t so no joint jumps by 2*pi between consecutive
rows. Segments are separate pieces of the branch; their end rows are
not adjacent on the manifold.
Source code in src/ssik/chart.py
| def curve(self, n: int = 181) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]:
"""Dense sample of the branch, continuous in ``t``, one segment per
domain interval.
Each segment is ``(ts, qs)``: ``ts`` of shape ``(N,)`` with about ``n``
points per full ``2*pi`` of parameter, and ``qs`` of shape ``(N, 7)``
unwrapped along ``t`` so no joint jumps by ``2*pi`` between consecutive
rows. Segments are separate pieces of the branch; their end rows are
not adjacent on the manifold.
"""
segments: list[tuple[NDArray[np.float64], NDArray[np.float64]]] = []
for lo, hi in self.domain:
m = max(round(n * (hi - lo) / _TWO_PI), 2)
ts = np.linspace(lo, hi, m, endpoint=not (self.periodic and hi - lo >= _TWO_PI - 1e-9))
qs = self._eval(ts)
ok = np.all(np.isfinite(qs), axis=1)
ts, qs = ts[ok], qs[ok]
if ts.shape[0] == 0:
continue
segments.append((ts, np.unwrap(qs, axis=0)))
return segments
|
length
length(
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-05,
) -> float
Arc length of the branch, summed over its pieces (request C1).
metric is None (Euclidean joint space), a constant SPD
(dof, dof) matrix, or a batched callable (N, dof) -> (N, dof, dof)
-- the mass matrix from gafro gives kinetic-energy distance. limits
restricts the branch to :meth:in_limits first (True for the chain's
own limits, or a (dof, 2) array). See :meth:sample for tol.
Zero on a zero-dimensional chart.
Source code in src/ssik/chart.py
| def length(
self,
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-5,
) -> float:
"""Arc length of the branch, summed over its pieces (request C1).
``metric`` is ``None`` (Euclidean joint space), a constant SPD
``(dof, dof)`` matrix, or a batched callable ``(N, dof) -> (N, dof, dof)``
-- the mass matrix from gafro gives kinetic-energy distance. ``limits``
restricts the branch to :meth:`in_limits` first (``True`` for the chain's
own limits, or a ``(dof, 2)`` array). See :meth:`sample` for ``tol``.
Zero on a zero-dimensional chart.
"""
return float(sum(s[-1] for _, _, s in self._polylines(metric, limits, tol)))
|
sample
sample(
n: int,
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-05,
) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]
About n points spaced uniformly in arc length (request C1).
Uniform in t is not uniform on the manifold: at a fold of the
coordinate the rate |dq/dt| diverges (see :meth:tangent), so a
t-grid bunches up in joint space in the middle of a branch and
leaves gaps at its ends. This places points at equal arc length
instead, in metric (see :meth:length), so a sum over the samples
converges to the curve's value rather than to a sample-density artefact.
Returns one (ts, qs) per piece, like :meth:curve (qs unwrapped
along the piece). The spacing h = L / n is shared by all pieces; an
open piece gets round(L_i / h) + 1 points (at least two) including
both ends, a closed one round(L_i / h) (at least three) with the
seam spaced like the rest. Every point is q(t) evaluated exactly on
the chart; only its t comes from the arc-length inversion.
The length is the midpoint-metric sum over a polyline of q(t),
refined by bisection until no chord's midpoint strays more than tol
(rad) from the curve; near a fold q goes like sqrt(t - t_0) and
the refinement concentrates there on its own. Spacing is uniform to
about tol. A position-dependent metric is evaluated once per
chord, batched.
The returned qs carry whatever turn the branch is on, as
:meth:in_limits describes: with limits set they are on an
admissible arc, which is not the same as being inside the joint box
numerically.
Source code in src/ssik/chart.py
| def sample(
self,
n: int,
metric: Metric | None = None,
*,
limits: ArrayLike | bool | None = None,
tol: float = 1e-5,
) -> list[tuple[NDArray[np.float64], NDArray[np.float64]]]:
"""About ``n`` points spaced uniformly in arc length (request C1).
Uniform in ``t`` is not uniform on the manifold: at a fold of the
coordinate the rate ``|dq/dt|`` diverges (see :meth:`tangent`), so a
``t``-grid bunches up in joint space in the middle of a branch and
leaves gaps at its ends. This places points at equal arc length
instead, in ``metric`` (see :meth:`length`), so a sum over the samples
converges to the curve's value rather than to a sample-density artefact.
Returns one ``(ts, qs)`` per piece, like :meth:`curve` (``qs`` unwrapped
along the piece). The spacing ``h = L / n`` is shared by all pieces; an
open piece gets ``round(L_i / h) + 1`` points (at least two) including
both ends, a closed one ``round(L_i / h)`` (at least three) with the
seam spaced like the rest. Every point is ``q(t)`` evaluated exactly on
the chart; only its ``t`` comes from the arc-length inversion.
The length is the midpoint-metric sum over a polyline of ``q(t)``,
refined by bisection until no chord's midpoint strays more than ``tol``
(rad) from the curve; near a fold ``q`` goes like ``sqrt(t - t_0)`` and
the refinement concentrates there on its own. Spacing is uniform to
about ``tol``. A position-dependent ``metric`` is evaluated once per
chord, batched.
The returned ``qs`` carry whatever turn the branch is on, as
:meth:`in_limits` describes: with ``limits`` set they are on an
admissible arc, which is not the same as being inside the joint box
numerically.
"""
if n < 1:
raise ValueError("sample: n must be >= 1")
lines = self._polylines(metric, limits, tol)
total = sum(s[-1] for _, _, s in lines)
if not lines:
return []
if self.dimension == 0 or total <= 0.0:
return [(ts[:1], qs[:1]) for ts, qs, _ in lines]
h = total / n
out: list[tuple[NDArray[np.float64], NDArray[np.float64]]] = []
for ts, qs, s in lines:
L = s[-1]
closed = _closes(qs)
if closed:
m = max(round(L / h), 3)
targets = np.arange(m) * (L / m)
else:
m = max(round(L / h) + 1, 2)
targets = np.linspace(0.0, L, m)
k = np.clip(np.searchsorted(s, targets, side="right") - 1, 0, s.shape[0] - 2)
ds = s[k + 1] - s[k]
f = np.where(ds > 0.0, (targets - s[k]) / np.where(ds > 0.0, ds, 1.0), 0.0)
t_new = ts[k] + f * (ts[k + 1] - ts[k])
q_new = self._eval_t(t_new)
ok = np.all(np.isfinite(q_new), axis=1)
out.append((t_new[ok], np.unwrap(q_new[ok], axis=0)))
return out
|
charts
charts(
kb: KinBody,
T_target: ArrayLike,
*,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
) -> SelfMotionManifold
Enumerate the self-motion charts of kb at pose T_target.
Parameters:
| Name |
Type |
Description |
Default |
kb
|
KinBody
|
POE-normalised 7R :class:~ssik.internals.KinBody.
|
required
|
T_target
|
ArrayLike
|
4x4 SE(3) pose in the base frame.
|
required
|
solver_name
|
str | None
|
the dispatched solver (Manipulator.solver_name or an
artifact's SOLVER_NAME). None runs the dispatcher.
|
None
|
policy
|
TolerancePolicy
|
tolerance policy forwarded to the subproblem feasibility
gates.
|
DEFAULT_TOLERANCE_POLICY
|
native
|
bool
|
use the C++ extension when it is available (a performance
hint, never an error when it is not); False forces the Python
reference.
|
True
|
Raises:
| Type |
Description |
NotImplementedError
|
for a solver family without a closed-form
chart (see the module docstring).
|
TypeError
|
for a T_target of the wrong type, as solve()
(docs/api.md, "Input validation").
|
ValueError
|
for a malformed T_target, as solve().
|
Source code in src/ssik/chart.py
| def charts(
kb: KinBody,
T_target: ArrayLike,
*,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
) -> SelfMotionManifold:
"""Enumerate the self-motion charts of ``kb`` at pose ``T_target``.
:param kb: POE-normalised 7R :class:`~ssik.internals.KinBody`.
:param T_target: 4x4 SE(3) pose in the base frame.
:param solver_name: the dispatched solver (``Manipulator.solver_name`` or an
artifact's ``SOLVER_NAME``). ``None`` runs the dispatcher.
:param policy: tolerance policy forwarded to the subproblem feasibility
gates.
:param native: use the C++ extension when it is available (a performance
hint, never an error when it is not); ``False`` forces the Python
reference.
:raises NotImplementedError: for a solver family without a closed-form
chart (see the module docstring).
:raises TypeError: for a ``T_target`` of the wrong type, as ``solve()``
(``docs/api.md``, "Input validation").
:raises ValueError: for a malformed ``T_target``, as ``solve()``.
"""
from ssik._solve_inputs import check_pose
T = check_pose(T_target, policy=policy)
if solver_name is None:
from ssik.core.dispatcher import dispatch
solver_name = dispatch(kb, policy).solver_name
ext = _native_ext() if native else None
if solver_name == "seven_r.spherical_shoulder":
if ext is not None:
return _spherical_shoulder_family_native(ext, kb, T, policy)
return _spherical_shoulder_family(kb, T, policy)
if solver_name == "seven_r.srs":
if ext is not None:
fam = _srs_family_native(ext, kb, T)
if fam is not None:
return fam
return _srs_family(kb, T, policy)
if solver_name == "ikgeo.three_parallel":
return _three_parallel_family(kb, T, policy, native=ext is not None)
raise NotImplementedError(
f"charts: no closed-form chart for solver {solver_name!r}. Supported: "
f"{sorted(SUPPORTED_SOLVERS)}. Polished (approximate-class) solvers have no "
"closed-form branch; jointlock branches carry no stable label across samples."
)
|
track
track(
kb: KinBody,
poses: ArrayLike,
q0: ArrayLike,
*,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
max_step: float = 0.5,
) -> list[TrackStep]
Follow one branch of the self-motion manifold along a pose path.
poses[0] must be the pose of q0. At each subsequent pose the branch is
continued by :meth:SelfMotionManifold.continue_from: a label lookup while the
label persists, a relabel at a fold, and a "collision" event where the
branch vanished (the step then holds the nearest chart's point). At a pose
with no manifold at all (outside the workspace) tracking stops: the list
ends with one "unreachable" step (q all NaN) and is shorter than
poses. The
coordinate t is held fixed step to step, so this is pure continuation
of the redundancy: a controller would add its own motion along the chart.
Closing a loop and comparing the first and last labels reads off the
monodromy of the loop.
Source code in src/ssik/chart.py
| def track(
kb: KinBody,
poses: ArrayLike,
q0: ArrayLike,
*,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
max_step: float = 0.5,
) -> list[TrackStep]:
"""Follow one branch of the self-motion manifold along a pose path.
``poses[0]`` must be the pose of ``q0``. At each subsequent pose the branch is
continued by :meth:`SelfMotionManifold.continue_from`: a label lookup while the
label persists, a relabel at a fold, and a ``"collision"`` event where the
branch vanished (the step then holds the nearest chart's point). At a pose
with no manifold at all (outside the workspace) tracking stops: the list
ends with one ``"unreachable"`` step (``q`` all ``NaN``) and is shorter than
``poses``. The
coordinate ``t`` is held fixed step to step, so this is *pure* continuation
of the redundancy: a controller would add its own motion along the chart.
Closing a loop and comparing the first and last labels reads off the
monodromy of the loop.
"""
path = track_all(
kb, poses, q0=q0, solver_name=solver_name, policy=policy, native=native, max_step=max_step
)
return next(iter(path.tracks.values()))
|
track_all
track_all(
kb: KinBody,
poses: ArrayLike,
*,
q0: ArrayLike | None = None,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
max_step: float = 0.5,
close_tol: float = 1e-09,
) -> PathTrack
Solve a pose path in one call, carrying chart labels along it (request C2).
With q0=None every chart of poses[0] is tracked, each from the
middle of its longest domain interval; with q0 only the branch through
q0, from where q0 sits (poses[0] must be its pose), and with a
(k, dof) q0 the k branches through those starts, one per chart. The manifold
is built once per pose and every branch is continued on it by
:meth:SelfMotionManifold.continue_from -- a label lookup, no
nearest-neighbour search, with the fold and collision events of
:func:track. t is held fixed per branch, as in :func:track.
Tracking stops at the first pose with no manifold at all (outside the
workspace): nothing connects a branch across that gap, so picking a label
up again on the far side would be a fresh lookup posing as continuation.
Every track then ends with one "unreachable" step (q all NaN)
and :attr:PathTrack.stopped_at names the pose.
When poses[-1] equals poses[0] (to close_tol, max abs entry)
the path is a loop and :attr:PathTrack.permutation is its monodromy.
Source code in src/ssik/chart.py
| def track_all(
kb: KinBody,
poses: ArrayLike,
*,
q0: ArrayLike | None = None,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
max_step: float = 0.5,
close_tol: float = 1e-9,
) -> PathTrack:
"""Solve a pose path in one call, carrying chart labels along it (request C2).
With ``q0=None`` every chart of ``poses[0]`` is tracked, each from the
middle of its longest domain interval; with ``q0`` only the branch through
``q0``, from where ``q0`` sits (``poses[0]`` must be its pose), and with a
``(k, dof)`` ``q0`` the ``k`` branches through those starts, one per chart. The manifold
is built once per pose and every branch is continued on it by
:meth:`SelfMotionManifold.continue_from` -- a label lookup, no
nearest-neighbour search, with the fold and collision events of
:func:`track`. ``t`` is held fixed per branch, as in :func:`track`.
Tracking stops at the first pose with no manifold at all (outside the
workspace): nothing connects a branch across that gap, so picking a label
up again on the far side would be a fresh lookup posing as continuation.
Every track then ends with one ``"unreachable"`` step (``q`` all ``NaN``)
and :attr:`PathTrack.stopped_at` names the pose.
When ``poses[-1]`` equals ``poses[0]`` (to ``close_tol``, max abs entry)
the path is a loop and :attr:`PathTrack.permutation` is its monodromy.
"""
from ssik._solve_inputs import as_real_array, check_joints, check_pose
P = as_real_array(poses, "poses")
if P.ndim != 3 or P.shape[1:] != (4, 4) or P.shape[0] < 1:
raise ValueError(f"track_all: poses must be (N, 4, 4) with N >= 1, got {P.shape}")
for i in range(P.shape[0]):
check_pose(P[i], policy=policy, name=f"poses[{i}]")
if q0 is not None:
starts_arr = np.atleast_2d(as_real_array(q0, "q0"))
if starts_arr.ndim != 2:
raise ValueError(f"q0 must have shape (dof,) or (k, dof), got {starts_arr.shape}")
q0 = np.stack([check_joints(row, len(kb.joints), "q0") for row in starts_arr])
fam = charts(kb, P[0], solver_name=solver_name, policy=policy, native=native)
heads: list[tuple[Chart, float, NDArray[np.float64]]] = []
if q0 is None:
for c in fam.charts:
t = _start_t(c)
heads.append((c, t, c.q(t)))
else:
for q_start in np.atleast_2d(np.asarray(q0, dtype=np.float64)):
located = fam.locate(q_start)
if located is None:
raise ValueError("track: q0 is not on the manifold of poses[0]")
heads.append((located[0], located[1], q_start))
starts = [c.label for c, _, _ in heads]
if len(set(starts)) != len(starts):
raise ValueError(
f"track_all: two starts lie on one chart ({starts}); tracks are keyed by it"
)
tracks = {c.label: [TrackStep(0, c.label, t, q, "start")] for c, t, q in heads}
stopped_at: int | None = None
for i in range(1, P.shape[0]):
fam = charts(kb, P[i], solver_name=solver_name, policy=policy, native=native)
if not fam.charts:
gone = np.full(len(kb.joints), np.nan)
for k, (chart, t, _) in enumerate(heads):
tracks[starts[k]].append(TrackStep(i, chart.label, t, gone, "unreachable"))
stopped_at = i
break
for k, (chart, t, q) in enumerate(heads):
chart, t, event = fam.continue_from(chart, t, q, max_step=max_step)
q = chart.q(t)
heads[k] = (chart, t, q)
tracks[starts[k]].append(TrackStep(i, chart.label, t, q, event))
closed = P.shape[0] > 1 and float(np.max(np.abs(P[-1] - P[0]))) <= close_tol
return PathTrack(tracks, closed, stopped_at)
|
drift_to_merge
drift_to_merge(
kb: KinBody,
T: ArrayLike,
a: Chart,
b: Chart,
direction: ArrayLike,
*,
max_drift: float = ...,
n_steps: int = ...,
merge_tol: float = ...,
limits: ArrayLike | bool | None = ...,
solver_name: str | None = ...,
policy: TolerancePolicy = ...,
native: bool = ...,
explain: Literal[False] = False,
) -> _MergeFound
drift_to_merge(
kb: KinBody,
T: ArrayLike,
a: Chart,
b: Chart,
direction: ArrayLike,
*,
max_drift: float = ...,
n_steps: int = ...,
merge_tol: float = ...,
limits: ArrayLike | bool | None = ...,
solver_name: str | None = ...,
policy: TolerancePolicy = ...,
native: bool = ...,
explain: Literal[True],
) -> _MergeExplained
drift_to_merge(
kb: KinBody,
T: ArrayLike,
a: Chart,
b: Chart,
direction: ArrayLike,
*,
max_drift: float = 1.0,
n_steps: int = 200,
merge_tol: float = 0.001,
limits: ArrayLike | bool | None = None,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
explain: bool = False,
) -> _MergeFound | _MergeExplained
March the pose along a unit twist and report the drift at which the
sheets of charts a and b merge (request D2, the verification of
:meth:SelfMotionManifold.escape).
A sheet is several charts glued at folds (the Panda) or one closed chart
(an SRS arm), so what is followed is the sheet, not a chart: one point of
each sheet is carried along by label continuation -- the same point at
every step, so a nearest point sitting on a fold, where either of two
charts can claim it, never changes which sheet is meant -- and the quantity
watched is the gap between the two sheets, the least :meth:~SelfMotionManifold.gap
over their chart pairs.
Two ways to merge. The sheets can join, becoming one sheet (the Panda):
the drift where that happens is bisected on "same sheet" to ~1e-12. Or
they can touch without joining (the iiwa14, whose sheets are closed
charts): the gap has a square-root cusp there, and each local minimum
the grid brackets is refined by golden section. Either is a merge when
the gap just before it is at most merge_tol. Returns (drift, pose)
at the first merge, or None.
Resolution: the drift of a touch is found to ~1e-8, the gap at it to
~1e-4 rad (the nearest pair is itself cusp-shaped in t there), hence
the default merge_tol. The grid must see a touch before it can be
refined; the gap grows like c*sqrt(|s - s*|) with c ~ 8 rad, so
it is near 1 rad 0.015 away, and n_steps=200 over unit drift resolves
that. limits is passed to :meth:~SelfMotionManifold.gap: only
postures inside the joint box count.
explain=True returns (result, reason), the reason naming the
outcome -- merged, branch lost or no merge -- with the closest
approach found; a bare None cannot tell "never met" from "the pose left
the workspace".
Source code in src/ssik/chart.py
| def drift_to_merge(
kb: KinBody,
T: ArrayLike,
a: Chart,
b: Chart,
direction: ArrayLike,
*,
max_drift: float = 1.0,
n_steps: int = 200,
merge_tol: float = 1e-3,
limits: ArrayLike | bool | None = None,
solver_name: str | None = None,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
native: bool = True,
explain: bool = False,
) -> _MergeFound | _MergeExplained:
"""March the pose along a unit twist and report the drift at which the
*sheets* of charts ``a`` and ``b`` merge (request D2, the verification of
:meth:`SelfMotionManifold.escape`).
A sheet is several charts glued at folds (the Panda) or one closed chart
(an SRS arm), so what is followed is the sheet, not a chart: one point of
each sheet is carried along by label continuation -- the same point at
every step, so a nearest point sitting on a fold, where either of two
charts can claim it, never changes which sheet is meant -- and the quantity
watched is the gap between the two sheets, the least :meth:`~SelfMotionManifold.gap`
over their chart pairs.
Two ways to merge. The sheets can join, becoming one sheet (the Panda):
the drift where that happens is bisected on "same sheet" to ~1e-12. Or
they can touch without joining (the iiwa14, whose sheets are closed
charts): the gap has a square-root cusp there, and each local minimum
the grid brackets is refined by golden section. Either is a merge when
the gap just before it is at most ``merge_tol``. Returns ``(drift, pose)``
at the first merge, or ``None``.
Resolution: the drift of a touch is found to ~1e-8, the gap at it to
~1e-4 rad (the nearest pair is itself cusp-shaped in ``t`` there), hence
the default ``merge_tol``. The grid must see a touch before it can be
refined; the gap grows like ``c*sqrt(|s - s*|)`` with ``c`` ~ 8 rad, so
it is near 1 rad 0.015 away, and ``n_steps=200`` over unit drift resolves
that. ``limits`` is passed to :meth:`~SelfMotionManifold.gap`: only
postures inside the joint box count.
``explain=True`` returns ``(result, reason)``, the reason naming the
outcome -- ``merged``, ``branch lost`` or ``no merge`` -- with the closest
approach found; a bare ``None`` cannot tell "never met" from "the pose left
the workspace".
"""
T0 = np.asarray(T, dtype=np.float64)
d = np.asarray(direction, dtype=np.float64)
def fam_at(s: float) -> SelfMotionManifold:
pose = se3_exp(s * d) @ T0
return charts(kb, pose, solver_name=solver_name, policy=policy, native=native)
Head = tuple[Chart, float, NDArray[np.float64]]
def advance(fam: SelfMotionManifold, h: Head) -> Head:
c, t, _ = fam.continue_from(h[0], h[1], h[2], max_step=0.7)
return c, t, c.q(t)
def sheet_gap(fam: SelfMotionManifold, ha: Head, hb: Head) -> tuple[float, bool]:
"""(gap between the heads' sheets, whether they are one sheet)."""
sa, sb = _glued_sheet(fam, ha[0]), _glued_sheet(fam, hb[0])
if sa & sb:
return 0.0, True
best = float("inf")
for x in fam.charts:
if x.label not in sa:
continue
for y in fam.charts:
if y.label in sb:
best = min(best, fam.gap(x, y, limits=limits)[0])
return best, False
fam0 = fam_at(0.0)
_, qa, qb = fam0.gap(a, b, limits=limits)
la, lb = fam0.locate(_wrap_pi(qa)), fam0.locate(_wrap_pi(qb))
if la is None or lb is None:
raise ValueError("drift_to_merge: the charts' nearest points do not locate")
ha: Head = (la[0], la[1], _wrap_pi(qa))
hb: Head = (lb[0], lb[1], _wrap_pi(qb))
g_start, joined = sheet_gap(fam0, ha, hb)
if joined:
raise ValueError("drift_to_merge: a and b are on one sheet already")
grid: list[tuple[float, Head, Head, float]] = [(0.0, ha, hb, g_start)]
def state_at(k: int, s: float) -> tuple[float, bool] | None:
"""Sheet gap at drift ``s``, the heads continued from grid step ``k``."""
fam = fam_at(s)
if not fam.charts:
return None
_, ha_k, hb_k, _ = grid[k]
return sheet_gap(fam, advance(fam, ha_k), advance(fam, hb_k))
def gap_at(k: int, s: float) -> float:
st = state_at(k, s)
return float("inf") if st is None else st[0]
def golden(k: int, lo: float, hi: float) -> tuple[float, float]:
r = 0.5 * (np.sqrt(5.0) - 1.0)
x1, x2 = hi - r * (hi - lo), lo + r * (hi - lo)
f1, f2 = gap_at(k, x1), gap_at(k, x2)
for _ in range(80):
if hi - lo < 1e-14:
break
if f1 <= f2:
hi, x2, f2 = x2, x1, f1
x1 = hi - r * (hi - lo)
f1 = gap_at(k, x1)
else:
lo, x1, f1 = x1, x2, f2
x2 = lo + r * (hi - lo)
f2 = gap_at(k, x2)
return (x1, f1) if f1 <= f2 else (x2, f2)
def joined_between(k: int, lo: float, hi: float) -> tuple[float, float]:
"""Bisect the drift where the sheets become one, from grid step ``k``;
the gap just before it."""
for _ in range(60):
if hi - lo <= 1e-12:
break
mid = 0.5 * (lo + hi)
st = state_at(k, mid)
if st is not None and st[1]:
hi = mid
else:
lo = mid
return hi, gap_at(k, lo)
def merged(s_min: float, g_min: float, how: str) -> _MergeFound | _MergeExplained:
return _merge_outcome(
(s_min, se3_exp(s_min * d) @ T0),
f"merged at drift {s_min:.8f} ({how}), gap {g_min:.3e} rad",
explain,
)
closest, closest_s = g_start, 0.0
for k in range(1, n_steps + 1):
s = max_drift * k / n_steps
fam = fam_at(s)
if not fam.charts:
s_min, g_min = golden(k - 1, grid[max(k - 2, 0)][0], s)
if g_min <= merge_tol:
return merged(s_min, g_min, "touched before leaving the workspace")
return _merge_outcome(
None,
f"branch lost at drift {s:.4f} (the pose left the workspace); closest "
f"approach {min(closest, g_min):.4g} rad",
explain,
)
ha, hb = advance(fam, grid[-1][1]), advance(fam, grid[-1][2])
g, joined = sheet_gap(fam, ha, hb)
if joined:
s_star, g_before = joined_between(k - 1, grid[-1][0], s)
if g_before <= merge_tol:
return merged(s_star, g_before, "the sheets joined")
return _merge_outcome(
None,
f"branch lost at drift {s_star:.6f}: the sheets became one while "
f"{g_before:.4g} rad apart -- a continuation jumped, not a merge",
explain,
)
grid.append((s, ha, hb, g))
if k >= 2 and grid[k - 1][3] <= grid[k - 2][3] and grid[k - 1][3] < g:
s_min, g_min = golden(k - 1, grid[k - 2][0], s)
if g_min <= merge_tol:
return merged(s_min, g_min, "the sheets touched")
if g_min < closest:
closest, closest_s = g_min, s_min
if g < closest:
closest, closest_s = g, s
return _merge_outcome(
None,
f"no merge within max_drift={max_drift:g}; closest approach {closest:.4g} rad "
f"at drift {closest_s:.6f}",
explain,
)
|
se3_exp
se3_exp(xi: ArrayLike) -> NDArray[np.float64]
4x4 exponential of a spatial twist xi = (v, w) (linear, angular), so
that se3_exp(dx) @ T is the pose T moved by dx, matching the
spatial Jacobian convention T(q + dq) ~ exp([J dq]) T(q).
Source code in src/ssik/chart.py
| def se3_exp(xi: ArrayLike) -> NDArray[np.float64]:
"""4x4 exponential of a spatial twist ``xi = (v, w)`` (linear, angular), so
that ``se3_exp(dx) @ T`` is the pose ``T`` moved by ``dx``, matching the
spatial Jacobian convention ``T(q + dq) ~ exp([J dq]) T(q)``."""
x = np.asarray(xi, dtype=np.float64)
v, w = x[:3], x[3:]
th = float(np.linalg.norm(w))
K = np.array([[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]])
if th < 1e-12:
R, V = np.eye(3), np.eye(3)
else:
R = np.eye(3) + np.sin(th) / th * K + (1 - np.cos(th)) / th**2 * K @ K
V = np.eye(3) + (1 - np.cos(th)) / th**2 * K + (th - np.sin(th)) / th**3 * K @ K
out = np.eye(4)
out[:3, :3] = R
out[:3, 3] = V @ v
return out
|
cuspidality_report
cuspidality_report(
kb: KinBody,
*,
locked_angles: ArrayLike = (-2.0, -0.7, 0.5, 1.5, 2.5),
n_poses: int = 100,
grid: int = 480,
seed: int = 0,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> CuspidalityReport
Classify the chart slicing of a spherical-shoulder arm as cuspidal or
not, by the criterion of Salunkhe, Gupta & Billard (RA-L 2025): lock the
redundancy at each of locked_angles, partition the positioning chain's
(q1, q2) torus into aspects by the singular set of its positional
Jacobian (signed components outside a dead band -- the determinant can touch
zero without changing sign), and count solution pairs of one pose that share
an aspect. A build-time diagnostic (seconds), not a runtime path.
SRS-family charts are noncuspidal by construction (an exact spherical
shoulder and wrist decouple, and the closed form relabels only at a critical
point); this report covers the seven_r.spherical_shoulder family, where
the answer depends on which joint is locked -- ssik locks the last one.
Source code in src/ssik/chart.py
| def cuspidality_report(
kb: KinBody,
*,
locked_angles: ArrayLike = (-2.0, -0.7, 0.5, 1.5, 2.5),
n_poses: int = 100,
grid: int = 480,
seed: int = 0,
policy: TolerancePolicy = DEFAULT_TOLERANCE_POLICY,
) -> CuspidalityReport:
"""Classify the chart slicing of a spherical-shoulder arm as cuspidal or
not, by the criterion of Salunkhe, Gupta & Billard (RA-L 2025): lock the
redundancy at each of ``locked_angles``, partition the positioning chain's
``(q1, q2)`` torus into aspects by the singular set of its positional
Jacobian (signed components outside a dead band -- the determinant can touch
zero without changing sign), and count solution pairs of one pose that share
an aspect. A build-time diagnostic (seconds), not a runtime path.
SRS-family charts are noncuspidal by construction (an exact spherical
shoulder and wrist decouple, and the closed form relabels only at a critical
point); this report covers the ``seven_r.spherical_shoulder`` family, where
the answer depends on which joint is locked -- ssik locks the last one.
"""
from scipy import ndimage # type: ignore[import-untyped]
from ssik.core.dispatcher import dispatch
from ssik.kinematics._scalar3 import _se3_inv
from ssik.kinematics.reverse import reverse_kinematic_chain
from ssik.solvers.ikgeo import spherical_two_intersecting
from ssik.solvers.jointlock.seven_r import _lock_joint
from ssik.solvers.seven_r.srs import _frame_at_joint_batch
name = dispatch(kb, policy).solver_name
if name != "seven_r.spherical_shoulder":
raise NotImplementedError(
f"cuspidality_report: covers seven_r.spherical_shoulder, not {name!r}"
)
rng = np.random.default_rng(seed)
limits = _chain_limits(kb)
g = np.linspace(-np.pi, np.pi, grid, endpoint=False)
step = g[1] - g[0]
q1g, q2g = np.meshgrid(g, g, indexing="ij")
aspects: dict[float, int] = {}
shared: dict[float, int] = {}
for t in np.asarray(locked_angles, dtype=np.float64):
rev = reverse_kinematic_chain(_lock_joint(kb, 6, float(t)))
det = _positioning_det(rev, q1g, q2g, _frame_at_joint_batch)
band = 1e-3 * float(np.abs(det).max())
pos, n_pos = _torus_components(det > band, ndimage)
neg, n_neg = _torus_components(det < -band, ndimage)
aspect = np.where(det > band, pos, np.where(det < -band, -neg, 0))
aspects[float(t)] = n_pos + n_neg
n_shared = 0
for _ in range(n_poses):
q = np.array([rng.uniform(lo, hi) for lo, hi in limits])
from ssik.kinematics.poe_fk import poe_forward_kinematics
sols, _ = spherical_two_intersecting.solve(
rev, _se3_inv(poe_forward_kinematics(kb, q)), policy
)
pts = {(round(float(s.q[1]), 6), round(float(s.q[2]), 6)) for s in sols}
ids: list[int] = []
for a1, a2 in pts:
i = int(np.round((a1 + np.pi) / step)) % grid
j = int(np.round((a2 + np.pi) / step)) % grid
a = int(aspect[i, j])
if a == 0:
ids = []
break
ids.append(a)
n_shared += len(ids) - len(set(ids))
shared[float(t)] = n_shared
return CuspidalityReport(name, aspects, shared)
|
three_parallel_label
three_parallel_label(kb: KinBody, q: ArrayLike) -> tuple[int, int, int]
The (shoulder, elbow, wrist) operation mode of a three-parallel 6R
configuration as three signs, each the sign of one factor of the chain's
singularity determinant, so a label can only change across a singularity:
- shoulder: the wrist point (joint 5's origin) on either side of the plane
spanned by axis 1 and the parallel-trio direction,
(o4 - o0) . (a0 x h);
- elbow: the trio's origins
o1, o2, o3 turning one way or the other,
h . ((o2 - o1) x (o3 - o2)) (zero with the arm stretched or folded);
- wrist: axes 4 and 6 on either side of alignment,
a4 . (a3 x a5).
These are the classic left/right, up/down, flip/no-flip modes computed from
the geometry, not from the order of the quartic roots the solver produces,
which carries no identity. Convention-free: no DH offsets involved.
Source code in src/ssik/chart.py
| def three_parallel_label(kb: KinBody, q: ArrayLike) -> tuple[int, int, int]:
"""The (shoulder, elbow, wrist) operation mode of a three-parallel 6R
configuration as three signs, each the sign of one factor of the chain's
singularity determinant, so a label can only change across a singularity:
- shoulder: the wrist point (joint 5's origin) on either side of the plane
spanned by axis 1 and the parallel-trio direction, ``(o4 - o0) . (a0 x h)``;
- elbow: the trio's origins ``o1, o2, o3`` turning one way or the other,
``h . ((o2 - o1) x (o3 - o2))`` (zero with the arm stretched or folded);
- wrist: axes 4 and 6 on either side of alignment, ``a4 . (a3 x a5)``.
These are the classic left/right, up/down, flip/no-flip modes computed from
the geometry, not from the order of the quartic roots the solver produces,
which carries no identity. Convention-free: no DH offsets involved.
"""
from ssik.solvers.seven_r.srs import _frame_at_joint_batch
qa = np.asarray(q, dtype=np.float64)[None, :]
axes = []
origins = []
for i in range(6):
R, o = _frame_at_joint_batch(kb, qa, i)
axes.append(R[0] @ np.asarray(kb.joints[i].axis, dtype=np.float64))
origins.append(o[0])
h = axes[1]
shoulder = float((origins[4] - origins[0]) @ np.cross(axes[0], h))
elbow = float(h @ np.cross(origins[2] - origins[1], origins[3] - origins[2]))
wrist = float(axes[4] @ np.cross(axes[3], axes[5]))
return (int(np.sign(shoulder)), int(np.sign(elbow)), int(np.sign(wrist)))
|
Streaming IK: Tracker
Manipulator.solve_path tracks a pose list offline. A teleoperated arm sees
its poses one at a time, from a VR controller, a SpaceMouse, a mocap stream or
a transform gizmo. A Tracker holds the state that takes: the branch it is
on, the configuration it last commanded, and the time of the last pose.
import ssik
arm = ssik.Manipulator.from_prebuilt("ur5e") # or from_urdf(...)
tracker = arm.tracker(q_robot, max_joint_speed=2.0)
for T, t in source.poses(): # any PoseSource
step = tracker.update(T, t) # one pose in
robot.command(step.q) # one configuration out
Tracker lives on Manipulator, so a shipped arm gets it through
Manipulator.from_prebuilt, whose solve is the artifact's own. It adds no
cost to solve().
Continuation. Every update continues the branch the tracker is following,
never a fresh solve that could land on another one:
- On a redundant 7R arm with a closed-form chart (
seven_r.spherical_shoulder:
Franka Panda, FR3; seven_r.srs: KUKA iiwa and the other exactly concurrent
SRS arms) the redundancy coordinate is held fixed, as solve_path holds it,
and the candidate is the chart point there nearest the followed one (every
joint compared on the circle). Where the label contract holds, that is the
chart solve_path continues to by label, so a tracked stretch returns
solve_path's configurations up to a 2*pi representative. Near a fold of
the spherical-shoulder charts the old label can still name a point within
max_step that is not the continuation; the tracker takes the nearer point.
- On every other arm, and on a 7R chart arm when the held point is further
than
jump_threshold or outside the joint limits, the candidate is
solve(T, q_seed=..., max_solutions=1, allow_rescue=False) under the
tracker's respect_limits: the nearest configuration in ssik's seed metric.
At a singular pose that is the point of the continuum nearest the seed
(Singular continua), so the continuation is defined
through a singularity. The T-perturbation rescue is off: it is for a
measure-zero ridge where the analytical path finds nothing, and it costs
milliseconds native and seconds in Python on every update it runs, which
would be every update the target is out of reach. At such a ridge the
tracker holds for one update instead.
The candidate is reported in the representative solve() reports, the one
nearest the followed configuration, inside the joint limits when they are
respected.
Distance. Every distance is ssik's seed metric (wrap_linf): the largest
single-joint move, with continuous joints (no limits) compared on the circle
and every other joint by its coordinate, since a finite joint cannot turn
through its stop.
Statuses. With branch_distance the distance from the configuration being
followed to the candidate:
| Status |
When |
q |
OK |
a candidate within jump_threshold, reachable this update |
the candidate; FK closes to the target within the solver's tolerance |
LIMITED |
as OK, but some joint would move more than max_joint_speed * dt |
moved toward the candidate, the step scaled down (direction kept) until no joint exceeds its limit |
HELD |
no candidate, or one further than jump_threshold with allow_jump=False |
the previous q, unchanged |
JUMPED |
a candidate further than jump_threshold with allow_jump=True, or next_branch() |
the candidate, rate limited like any move |
HELD carries a reason:
"unreachable": no configuration reaches the target, within the limits or
not (step.reachable is False);"limits": the followed branch continues only outside the joint limits, or
no in-limit configuration reaches the target;"jump": the nearest configuration is further than jump_threshold, a
branch switch.
While held the tracker keeps following the branch it was on, so it resumes
OK when the target comes back within reach of that branch.
Thresholds. The two limits answer different questions.
jump_threshold (default 0.5 rad, solve_path's max_step) decides what
is the same branch. It is a property of the branch geometry, independent of
time: a fast hand never triggers it and a slow flip always does.max_joint_speed (rad/s, or m/s for a prismatic joint; one number or one
per joint; default none) bounds how fast the arm follows. It applies to an
update with a timestamp t after an earlier timestamp (the constructor's
t0, reset's t, or a previous update), with dt their difference.
Timestamps must not decrease. While LIMITED, q does not close FK to the
target: fk_residual is the honest ||FK(q) - T||_F and lag the
distance still to go. Later updates keep closing the gap, and the update
that arrives is OK.
Step fields. TrackerStep is a frozen record: q (read-only), status,
reason, fk_residual (against this update's target, whatever the status),
moved (from the previous step's q), branch_distance (nan when there
was no candidate), lag, and t.
Threads. A Tracker is not thread-safe. Every call reads and changes its
state (the branch followed, the configuration, the last timestamp), so call it
from one thread at a time.
Branches. solutions(max_solutions=None) returns every configuration at
the target the tracker last accepted (tracker.target), nearest the followed
branch first, one per geometric branch (enumerate_windings=False); on a
redundant 7R arm these are solve()'s samples of the self-motion. They are
for rendering the other branches. next_branch() switches to the next branch
at that target, cycling in a fixed order (on a 7R chart arm by chart label at
the held coordinate, otherwise by configuration), and returns a JUMPED
step, or None when there is no other branch. Without a rate limit its q is
on the new branch; with one the arm does not move there, the new branch
becomes the one followed, and later timestamped updates carry the arm there
(LIMITED until it arrives). reset(q, t=None) restarts from a
configuration the arm is at.
Self-motion. On a redundant 7R arm with a closed-form chart the tracker
can move the elbow while the hand stays at the target ("keep my hand here,
swing the elbow"):
red = tracker.redundancy # ssik.Redundancy, or None
red.label, red.parameter, red.t # the followed chart, "q6" or "swivel", its coordinate
lo, hi = red.arc # the stretch a slide can cover
step = tracker.set_redundancy(0.3, t) # slide to coordinate 0.3 at tracker.target
redundancy is the followed chart at tracker.target (the
chart a solve_path would follow), its
coordinate t, and arc, the Chart.in_limits() arc that contains t
(the chart's domain interval with respect_limits=False). A fold or a
branch junction is an end of the arc, and an in-limit single point is a
zero-width arc (t, t). On the periodic swivel the pieces meeting at
+-pi are one arc, shifted by 2*pi where needed so that lo <= t <= hi,
and the whole circle is (-pi, pi); t itself is reported in [-pi, pi).
It is None on an arm without a closed-form chart (6R arms,
seven_r.srs_polished such as the Kinova Gen3, jointlock.seven_r), and on
a chart arm whose followed point no chart locates (a branch junction). It is
computed when read, so update() costs the same as before.set_redundancy(value, t=None) slides along the followed chart, from its
coordinate to value, at the target the tracker holds; t is a timestamp,
as for update(). The statuses are update()'s:OK: value is on arc (on the swivel, some value + 2*pi*k is; with
the whole circle in limits the shorter way round is taken). q is the
chart point there, followed along the arc from the current one (so a
joint keeps its winding), inside the joint limits, and FK closes to the
target. Within about 1e-5 rad of a fold the closed form closes FK only
to 1e-9..1e-7 (Chart.q), so such a point gets a Gauss-Newton step
on FK. An arc end that in_limits() placed a hair past its joint limit
is stepped in (at most 1e-6 in t) to the first point the limits
admit without clamping it onto the limit.LIMITED (max_joint_speed and timestamps): the slide goes along the
chart only as far as no joint moves more than max_joint_speed * dt, so
the hand stays on the target while the elbow lags. That point becomes the
one followed and lag is the distance still to go. The rest is not
queued: sending value again on later ticks, as a slider or a teleop
loop does, continues the slide, and the call that arrives is OK. An
arm still behind its followed point from an earlier rate limit first
closes that gap, as update() does.HELD: q unchanged. reason is "limits" when value is on the chart
but outside the arc, and "jump" when it is past a fold or a branch
junction (going on would mean leaving the chart for another branch) or
redundancy is None. A slide never switches chart or branch.- On an arm without a closed-form chart it raises
NotImplementedError, as
Manipulator.self_motion does: that is a property of the arm, not of
the stream.
A slide stays on one branch however long it is, so moved can exceed
jump_threshold; max_joint_speed is what bounds the motion per tick.
- update(T) holds the redundancy coordinate fixed, so after
set_redundancy it keeps the chosen coordinate as the target moves, while
the point there is within jump_threshold and inside the limits. Where it
is not, update() takes the seeded solve as usual and redundancy reports
the coordinate it landed on. A chart's label may renumber its reachable
interval across poses (the label contract in ssik.chart); the coordinate
is what is held.
Streaming, stateful IK on one :class:~ssik.Manipulator.
::
tracker = arm.tracker(q_robot, max_joint_speed=2.0)
for T, t in source.poses():
step = tracker.update(T, t)
robot.command(step.q)
Parameters:
| Name |
Type |
Description |
Default |
arm
|
Manipulator
|
the arm. A shipped arm is Manipulator.from_prebuilt(name),
whose solve is the artifact's own.
|
required
|
q0
|
ArrayLike
|
the arm's current configuration, (dof,).
|
required
|
max_joint_speed
|
float | ArrayLike | None
|
optional joint-speed limit in rad/s (m/s for a
prismatic joint), one number for every joint or one per joint. It
applies only to updates that carry a timestamp t after an earlier
one: the move is then scaled down, direction kept, until no joint moves
more than max_joint_speed * dt. None (default): no limit.
|
None
|
jump_threshold
|
float
|
the largest seed-metric distance (radians) from the
branch being followed to the candidate that still counts as the same
branch. Default 0.5, solve_path's max_step. It is a property
of the branch geometry, not of time: a fast hand never triggers it,
and a slow flip always does.
|
0.5
|
allow_jump
|
bool
|
when False (default) a candidate beyond
jump_threshold is refused (HELD, reason "jump"); when
True the tracker switches to it and reports JUMPED.
|
False
|
respect_limits
|
bool
|
keep every commanded configuration within the joint
limits (default). False follows the geometric branch regardless.
|
True
|
native
|
bool
|
use the C++ extension when it is available (default);
False forces the pure-Python reference.
|
True
|
t0
|
float | None
|
optional timestamp of q0, so the first update is rate
limited too.
|
None
|
Raises:
| Type |
Description |
TypeError
|
if q0 is not an array of real numbers, or a numeric
option is not a number.
|
ValueError
|
if q0 is not a finite (dof,) vector, or
max_joint_speed / jump_threshold is not finite and positive.
|
Source code in src/ssik/tracker.py
| class Tracker:
"""Streaming, stateful IK on one :class:`~ssik.Manipulator`.
::
tracker = arm.tracker(q_robot, max_joint_speed=2.0)
for T, t in source.poses():
step = tracker.update(T, t)
robot.command(step.q)
:param arm: the arm. A shipped arm is ``Manipulator.from_prebuilt(name)``,
whose solve is the artifact's own.
:param q0: the arm's current configuration, ``(dof,)``.
:param max_joint_speed: optional joint-speed limit in rad/s (m/s for a
prismatic joint), one number for every joint or one per joint. It
applies only to updates that carry a timestamp ``t`` after an earlier
one: the move is then scaled down, direction kept, until no joint moves
more than ``max_joint_speed * dt``. ``None`` (default): no limit.
:param jump_threshold: the largest seed-metric distance (radians) from the
branch being followed to the candidate that still counts as the same
branch. Default ``0.5``, ``solve_path``'s ``max_step``. It is a property
of the branch geometry, not of time: a fast hand never triggers it,
and a slow flip always does.
:param allow_jump: when ``False`` (default) a candidate beyond
``jump_threshold`` is refused (``HELD``, reason ``"jump"``); when
``True`` the tracker switches to it and reports ``JUMPED``.
:param respect_limits: keep every commanded configuration within the joint
limits (default). ``False`` follows the geometric branch regardless.
:param native: use the C++ extension when it is available (default);
``False`` forces the pure-Python reference.
:param t0: optional timestamp of ``q0``, so the first update is rate
limited too.
:raises TypeError: if ``q0`` is not an array of real numbers, or a numeric
option is not a number.
:raises ValueError: if ``q0`` is not a finite ``(dof,)`` vector, or
``max_joint_speed`` / ``jump_threshold`` is not finite and positive.
"""
__slots__ = (
"_allow_jump",
"_arm",
"_chart_family",
"_circular",
"_goal",
"_head",
"_jump",
"_kb",
"_native",
"_policy",
"_q",
"_respect",
"_speed",
"_t_last",
"_target",
)
def __init__(
self,
arm: Manipulator,
q0: ArrayLike,
*,
max_joint_speed: float | ArrayLike | None = None,
jump_threshold: float = 0.5,
allow_jump: bool = False,
respect_limits: bool = True,
native: bool = True,
t0: float | None = None,
) -> None:
from ssik.postprocess import _circular_mask
self._arm = arm
self._kb = arm.kinbody
self._policy = arm._policy
self._speed = (
None
if max_joint_speed is None
else _check_positive(max_joint_speed, "max_joint_speed", arm.dof)
)
jt = _check_positive(jump_threshold, "jump_threshold")
assert isinstance(jt, float)
self._jump = jt
self._allow_jump = bool(allow_jump)
self._respect = bool(respect_limits)
self._native = bool(native)
self._chart_family = arm.solver_name in _CHART_FAMILIES
self._circular = np.asarray(_circular_mask(self._kb, arm.dof), dtype=bool)
self.reset(q0, t=t0)
# ------------------------------------------------------------------
# State
# ------------------------------------------------------------------
@property
def q(self) -> NDArray[np.float64]:
"""The configuration last returned (``q0`` before any update), read-only."""
return _readonly(self._q)
@property
def target(self) -> NDArray[np.float64]:
"""The pose of the branch point being followed: the last target the
tracker accepted (``FK(q0)`` before any update), read-only."""
return _readonly(self._target)
def reset(self, q: ArrayLike, *, t: float | None = None) -> None:
"""Restart from configuration ``q`` (the arm is there now), with
optional timestamp ``t``. The branch is whichever ``q`` is on.
:raises TypeError: if ``q`` is not an array of real numbers or ``t`` is
not a number.
:raises ValueError: if ``q`` is not a finite ``(dof,)`` vector or ``t``
is not finite.
"""
from ssik._solve_inputs import check_joints
q_arr = check_joints(q, self._arm.dof, "q0").copy()
self._t_last = _check_time(t)
self._q = q_arr
self._goal = q_arr.copy()
self._target = self._arm.fk(q_arr)
self._head: tuple[Chart, float] | None = None
if self._chart_family:
self._head = self._locate(self._target, q_arr)
def __repr__(self) -> str:
return (
f"<Tracker on {self._arm.solver_name}: jump_threshold={self._jump}, "
f"max_joint_speed={self._speed}, allow_jump={self._allow_jump}>"
)
# ------------------------------------------------------------------
# Metric
# ------------------------------------------------------------------
def _delta(self, a: NDArray[np.float64], b: NDArray[np.float64]) -> NDArray[np.float64]:
"""``b - a`` per joint, continuous joints the short way round."""
d = b - a
c = self._circular
if c.any():
d[c] = (d[c] + np.pi) % (2.0 * np.pi) - np.pi
return d
def _dist(self, a: NDArray[np.float64], b: NDArray[np.float64]) -> float:
"""ssik's seed metric (``wrap_linf``): the largest single-joint move."""
return float(np.max(np.abs(self._delta(a, b))))
# ------------------------------------------------------------------
# Candidates
# ------------------------------------------------------------------
def _charts(self, T: NDArray[np.float64]) -> Any:
from ssik.chart import charts
return charts(
self._kb,
T,
solver_name=self._arm.solver_name,
policy=self._policy,
native=self._native,
)
def _locate(self, T: NDArray[np.float64], q: NDArray[np.float64]) -> tuple[Chart, float] | None:
located: tuple[Chart, float] | None = self._charts(T).locate(q)
return located
def _seeded(self, T: NDArray[np.float64], respect_limits: bool) -> list[Solution]:
kwargs: dict[str, Any] = {}
if self._arm._prebuilt is not None:
# Only an artifact's solve takes the backend switch; the live solve
# is the Python one.
kwargs["native"] = self._native
# No T-perturbation rescue: it exists for a measure-zero ridge where the
# analytical path finds nothing, and costs tens of milliseconds native
# and seconds in Python on every update it runs -- which, with the
# rescue on, is every update the target is out of limits or out of
# reach. A tracker holds one update at such a ridge instead.
sols: list[Solution] = self._arm.solve(
T,
q_seed=self._goal,
max_solutions=1,
respect_limits=respect_limits,
allow_rescue=False,
**kwargs,
)
return sols
def _admit(
self,
q: NDArray[np.float64],
T: NDArray[np.float64],
seed: NDArray[np.float64] | None = None,
) -> NDArray[np.float64] | None:
"""A chart point as a commanded configuration: the representative
nearest ``seed`` (default: the followed branch point), inside the
limits when they are respected (``None`` if it cannot be), as
``solve()`` reports one."""
from ssik.postprocess import respect_limits, rewrap_to_seed, wrap_to_limits
res = float(np.linalg.norm(self._arm.fk(q) - T))
sols = [Solution(q=np.asarray(q, dtype=np.float64), fk_residual=res)]
if self._respect:
sols = respect_limits(wrap_to_limits(sols, self._kb, T_target=T), self._kb, T_target=T)
if not sols:
return None
sols = rewrap_to_seed(sols, self._kb, self._goal if seed is None else seed, T_target=T)
return np.asarray(sols[0].q, dtype=np.float64)
def _held_point(self, fam: Any) -> tuple[Chart, float, NDArray[np.float64]] | None:
"""The followed branch at a new pose: of every chart's point at the held
redundancy coordinate, the one nearest the followed point (every joint
compared on the circle), or ``None`` when none is within
``jump_threshold``.
This is :meth:`~ssik.chart.SelfMotionManifold.continue_from` with the
label lookup replaced by the nearest point it is meant to find. The two
agree wherever the label contract holds; near a fold of the
spherical-shoulder charts the old label can still name a point within
``max_step`` that is not the continuation (the relabelled chart is
nearer), and the lookup would detour through it.
"""
from ssik.chart import _wrap_dist, _wrap_pi
assert self._head is not None
t = self._head[1]
if fam.periodic:
t = float(_wrap_pi(np.asarray(t)))
best: tuple[float, Chart, NDArray[np.float64]] | None = None
for chart in fam.charts:
qc = chart.q(t)
if not np.all(np.isfinite(qc)):
continue
d = _wrap_dist(qc, self._goal)
if best is None or d < best[0]:
best = (d, chart, qc)
if best is None or best[0] > self._jump:
return None
return best[1], t, best[2]
def _candidate(
self, T: NDArray[np.float64]
) -> tuple[NDArray[np.float64] | None, tuple[Chart, float] | None, bool]:
"""The continuation of the followed branch at ``T``, its chart location
(7R chart arms), and whether the branch's own continuation was found
but lies outside the joint limits. ``(None, None, ...)`` when nothing
reaches ``T`` within the limits."""
fam = None
blocked = False
if self._chart_family:
fam = self._charts(T)
if self._head is not None:
held = self._held_point(fam)
if held is not None:
chart, t, qc = held
admitted = self._admit(qc, T)
if admitted is not None:
return admitted, (chart, t), False
blocked = True
sols = self._seeded(T, self._respect)
if not sols:
return None, None, blocked
q = np.asarray(sols[0].q, dtype=np.float64)
head = fam.locate(q) if fam is not None else None
return q, head, blocked
def _hold_reason(self, T: NDArray[np.float64], best: float, blocked: bool) -> HoldReason:
"""Why the tracker holds, when its best in-limit candidate is ``best``
away (``inf`` for none) and ``blocked`` says the branch's continuation
was found outside the limits. Runs only on the hold path."""
if blocked:
return "limits"
if not self._respect:
return "unreachable" if math.isinf(best) else "jump"
raw = self._seeded(T, respect_limits=False)
if not raw:
return "unreachable"
if math.isinf(best) or self._dist(self._goal, np.asarray(raw[0].q)) <= self._jump:
return "limits"
return "jump"
# ------------------------------------------------------------------
# Motion
# ------------------------------------------------------------------
def _toward(
self, goal: NDArray[np.float64], dt: float | None
) -> tuple[NDArray[np.float64], bool]:
"""Move from the last commanded configuration toward ``goal``, scaled
down (direction kept) to the rate limit; ``True`` when it clamped."""
if self._speed is None or dt is None:
return goal.copy(), False
delta = self._delta(self._q, goal)
cap = np.broadcast_to(np.asarray(self._speed, dtype=np.float64) * dt, delta.shape)
mag = np.abs(delta)
over = mag > cap
if not over.any():
return goal.copy(), False
scale = float(np.min(np.where(over, cap / np.where(over, mag, 1.0), 1.0)))
return self._q + scale * delta, True
def _step(
self,
q_new: NDArray[np.float64],
status: TrackStatus,
T: NDArray[np.float64],
t: float | None,
reason: HoldReason | None = None,
branch_distance: float = math.nan,
lag_to: NDArray[np.float64] | None = None,
) -> TrackerStep:
moved = self._dist(self._q, q_new)
self._q = q_new
return TrackerStep(
q=_readonly(q_new),
status=status,
reason=reason,
fk_residual=float(np.linalg.norm(self._arm.fk(q_new) - T)),
moved=moved,
branch_distance=branch_distance,
lag=self._dist(q_new, self._goal if lag_to is None else lag_to),
t=t,
)
def _advance_time(self, t: Any) -> tuple[float | None, float | None]:
"""Validate timestamp ``t`` against the last one; return it and the
``dt`` since the last one (``None`` without both), and record it."""
tt = _check_time(t)
if tt is not None and self._t_last is not None and tt < self._t_last:
raise ValueError(f"t must not decrease: got {tt} after {self._t_last}")
dt = None if tt is None or self._t_last is None else tt - self._t_last
if tt is not None:
self._t_last = tt
return tt, dt
def update(self, T: ArrayLike, t: float | None = None) -> TrackerStep:
"""Follow the branch to target ``T`` (timestamp ``t``, seconds).
- ``OK``: the continuation is within ``jump_threshold`` and, under a
rate limit, reachable this tick. ``q`` closes FK to ``T``.
- ``LIMITED``: same branch, but the move was clamped to
``max_joint_speed * dt``. Later updates keep closing the gap.
- ``HELD``: the previous ``q`` is returned unchanged; ``reason`` is
``"unreachable"``, ``"limits"`` or ``"jump"``. The tracker keeps
following the branch it was on, so it resumes when the target
comes back within reach.
- ``JUMPED``: with ``allow_jump=True``, a candidate beyond
``jump_threshold`` was taken (rate-limited like any move).
:param T: 4x4 target pose in the arm's base frame (the flange, as for
``solve()``).
:param t: optional timestamp. Timestamps must not decrease.
:raises TypeError: if ``T`` is not an array of real numbers or ``t`` is
not a number.
:raises ValueError: if ``T`` is not a finite rigid ``(4, 4)`` transform
(``docs/api.md``, "Input validation"), or ``t`` is not finite or is
earlier than the previous timestamp.
"""
from ssik._solve_inputs import check_pose
T_arr = check_pose(T, policy=self._policy, name="T")
tt, dt = self._advance_time(t)
cand, head, blocked = self._candidate(T_arr)
dist = math.inf if cand is None else self._dist(self._goal, cand)
if cand is None or (dist > self._jump and not self._allow_jump):
reason = self._hold_reason(T_arr, dist, blocked)
reported = math.nan if cand is None else dist
return self._step(self._q.copy(), TrackStatus.HELD, T_arr, tt, reason, reported)
assert cand is not None
jumped = dist > self._jump
self._goal = cand
self._target = T_arr
if self._chart_family:
self._head = head
q_new, limited = self._toward(cand, dt)
if jumped:
status = TrackStatus.JUMPED
else:
status = TrackStatus.LIMITED if limited else TrackStatus.OK
return self._step(q_new, status, T_arr, tt, branch_distance=dist)
# ------------------------------------------------------------------
# Branches
# ------------------------------------------------------------------
def solutions(self, max_solutions: int | None = None) -> list[Solution]:
"""Every configuration at :attr:`target`, nearest the followed branch
first: ``solve(target, q_seed=..., enumerate_windings=False)`` under
the tracker's ``respect_limits``. On a redundant 7R arm these are
``solve()``'s samples of the self-motion. For rendering the other
branches ("ghost arms"); :meth:`next_branch` switches to one.
"""
kwargs: dict[str, Any] = {}
if self._arm._prebuilt is not None:
kwargs["native"] = self._native
sols: list[Solution] = self._arm.solve(
self._target,
q_seed=self._goal,
max_solutions=max_solutions,
respect_limits=self._respect,
enumerate_windings=False,
**kwargs,
)
return sols
def _branch_points(self) -> list[tuple[NDArray[np.float64], tuple[Chart, float] | None]]:
"""The branches at :attr:`target` in a fixed order, each as the
configuration the tracker would follow on it."""
if self._chart_family and self._head is not None:
fam = self._charts(self._target)
t = self._head[1]
if fam.periodic:
t = float((t + np.pi) % (2.0 * np.pi) - np.pi)
out: list[tuple[NDArray[np.float64], tuple[Chart, float] | None]] = []
for chart in sorted(fam.charts, key=lambda c: c.label):
tc = t
if not chart.contains(tc):
lo, hi = max(chart.domain, key=lambda d: d[1] - d[0])
tc = min(max(t, lo), hi)
qc = chart.q(tc)
if not np.all(np.isfinite(qc)):
continue
admitted = self._admit(qc, self._target)
if admitted is not None:
out.append((admitted, (chart, tc)))
return out
sols = self.solutions()
qs = sorted(
(np.asarray(s.q, dtype=np.float64) for s in sols),
key=lambda q: tuple(round(float(v), 9) for v in q),
)
return [(q, None) for q in qs]
def next_branch(self) -> TrackerStep | None:
"""Switch to the next branch at :attr:`target`, cycling through them
in a fixed order (on a 7R chart arm: by chart label, at the held
redundancy coordinate; otherwise: by configuration). Returns the
``JUMPED`` step, or ``None`` when there is no other branch.
Without a rate limit the returned ``q`` is on the new branch. With
``max_joint_speed`` set the arm does not move here: the new branch
becomes the one followed, and later timestamped updates carry the arm
there at the joint-speed limit (``LIMITED`` until it arrives).
"""
points = self._branch_points()
if len(points) < 2:
return None
cur = min(range(len(points)), key=lambda i: self._dist(self._goal, points[i][0]))
q_next, head = points[(cur + 1) % len(points)]
if self._dist(self._goal, q_next) == 0.0:
return None
dist = self._dist(self._goal, q_next)
self._goal = q_next
if self._chart_family:
self._head = head
q_new = q_next.copy() if self._speed is None else self._q.copy()
return self._step(
q_new, TrackStatus.JUMPED, self._target, self._t_last, branch_distance=dist
)
# ------------------------------------------------------------------
# Redundancy
# ------------------------------------------------------------------
def _arc(self, chart: Chart, t: float) -> tuple[float, float]:
"""The stretch of ``chart`` containing ``t`` that a slide may cover:
its in-limits arc (its domain interval when limits are not respected),
``(t, t)`` when none contains ``t``. On a periodic chart the pieces
meeting at ``+-pi`` are one arc, shifted by ``2*pi`` to contain ``t``."""
pieces = list(chart.in_limits() if self._respect else chart.domain)
if chart.periodic:
low = [p for p in pieces if p[0] <= -math.pi + _COORD_TOL]
high = [p for p in pieces if p[1] >= math.pi - _COORD_TOL]
if low and high and low[0] is not high[0]:
pieces = [p for p in pieces if p is not low[0] and p is not high[0]]
pieces.append((high[0][0], low[0][1] + _TWO_PI))
for lo, hi in pieces:
if hi - lo >= _TWO_PI - _COORD_TOL:
return (-math.pi, math.pi)
for shift in (0.0, -_TWO_PI):
if lo + shift - _COORD_TOL <= t <= hi + shift + _COORD_TOL:
return (lo + shift, hi + shift)
return (t, t)
for lo, hi in pieces:
if lo - _COORD_TOL <= t <= hi + _COORD_TOL:
return (lo, hi)
return (t, t)
@property
def redundancy(self) -> Redundancy | None:
"""Where the tracker is on the self-motion: the followed chart, its
coordinate, and the arc :meth:`set_redundancy` can slide along.
``None`` on an arm without a closed-form chart (6R arms, and 7R arms
other than ``seven_r.spherical_shoulder`` / ``seven_r.srs``), and on a
chart arm whose followed point no chart locates (a branch junction).
Computed when read (the chart's :meth:`~ssik.chart.Chart.in_limits`),
so :meth:`update` pays nothing for it.
"""
if not self._chart_family or self._head is None:
return None
chart, t = self._head
if chart.periodic:
t = _wrap(t)
return Redundancy(
chart=chart,
label=chart.label,
parameter=chart.parameter,
periodic=chart.periodic,
t=float(t),
arc=self._arc(chart, t),
)
def _on_circle(self, d: NDArray[np.float64]) -> NDArray[np.float64]:
"""Every joint difference wrapped to ``[-pi, pi)``: a chart returns one
representative of each angle, so its values are compared on the circle
before they are followed."""
return np.asarray((d + np.pi) % _TWO_PI - np.pi, dtype=np.float64)
@staticmethod
def _snap(qs: NDArray[np.float64], near: NDArray[np.float64]) -> NDArray[np.float64]:
"""``qs`` moved by whole turns to the representative nearest ``near``."""
return np.asarray(qs + _TWO_PI * np.round((near - qs) / _TWO_PI), dtype=np.float64)
def _chart_q(self, chart: Chart, t: float, near: NDArray[np.float64]) -> NDArray[np.float64]:
q = chart.q(_wrap(t) if chart.periodic else t)
return self._snap(np.asarray(q, dtype=np.float64), near)
def _slide_path(
self, chart: Chart, t0: float, t1: float
) -> tuple[NDArray[np.float64], NDArray[np.float64]] | None:
"""``(ts, qs)``: the chart from ``t0`` to ``t1``, sampled finely enough
that no joint steps more than ``_SLIDE_MAX_STEP``, each row in the
winding continuous with the followed point. ``None`` if the chart does
not exist along the way."""
n = max(2, math.ceil(abs(t1 - t0) / _SLIDE_DT) + 1)
while True:
ts = np.linspace(t0, t1, n)
qs = chart.q((ts + np.pi) % _TWO_PI - np.pi if chart.periodic else ts)
if not np.all(np.isfinite(qs)):
return None
steps = self._on_circle(np.diff(qs, axis=0))
if n > _SLIDE_MAX_SAMPLES or float(np.max(np.abs(steps))) <= _SLIDE_MAX_STEP:
break
n = 4 * n
start = self._goal + self._on_circle(qs[0] - self._goal)
cont = start + np.vstack([np.zeros((1, qs.shape[1])), np.cumsum(steps, axis=0)])
return ts, self._snap(qs, cont)
def _polish(self, q: NDArray[np.float64], T: NDArray[np.float64]) -> NDArray[np.float64]:
"""Gauss-Newton steps on FK (two at most) for a chart point near a fold
(``_POLISH_ABOVE``); ``q`` itself when it already closes."""
from ssik.refinement import kinbody_jacobian, se3_log_residual
best, best_res = q, float(np.linalg.norm(self._arm.fk(q) - T))
for _ in range(2):
if best_res <= _POLISH_ABOVE:
break
err = se3_log_residual(T @ np.linalg.inv(self._arm.fk(best)))
dq = np.linalg.lstsq(kinbody_jacobian(self._kb, best), err, rcond=None)[0]
cand = best + dq
res = float(np.linalg.norm(self._arm.fk(cand) - T))
if not res < best_res:
break
best, best_res = cand, res
return best
def _closes(self, q: NDArray[np.float64], T: NDArray[np.float64]) -> bool:
return float(np.linalg.norm(self._arm.fk(q) - T)) <= _POLISH_ABOVE
def _resolve(
self, chart: Chart, t0: float, arc: tuple[float, float], value: float
) -> float | HoldReason:
"""The coordinate to slide to for a requested ``value``: the
representative reached from ``t0`` without leaving ``arc``, or why
there is none."""
lo, hi = arc
if chart.periodic:
if hi - lo >= _TWO_PI - _COORD_TOL: # the whole circle: the shorter way
return t0 + float(self._on_circle(np.asarray(value - t0)))
base = value + _TWO_PI * round((t0 - value) / _TWO_PI)
inside = [
r
for r in (base - _TWO_PI, base, base + _TWO_PI)
if lo - _COORD_TOL <= r <= hi + _COORD_TOL
]
if inside:
nearest: float = min(inside, key=lambda r: abs(r - t0))
return min(max(nearest, lo), hi)
return "limits" if chart.contains(value) else "jump"
if lo - _COORD_TOL <= value <= hi + _COORD_TOL:
return min(max(value, lo), hi)
same_piece = any(
lo_d - _COORD_TOL <= t0 <= hi_d + _COORD_TOL
and lo_d - _COORD_TOL <= value <= hi_d + _COORD_TOL
for lo_d, hi_d in chart.domain
)
return "limits" if same_piece else "jump"
def set_redundancy(self, value: float, t: float | None = None) -> TrackerStep:
"""Slide along the self-motion at the held target: the hand stays at
:attr:`target` while the followed chart's redundancy coordinate (``q6``
or the swivel angle, :attr:`redundancy`) moves to ``value``.
- ``OK``: ``value`` is on :attr:`redundancy`'s ``arc`` (on the swivel,
some ``value + 2*pi*k`` is; with the whole circle in limits, the
shorter way round is taken). ``q`` is the chart point there, followed
along the arc from the current one, and closes FK to the target.
- ``LIMITED`` (``max_joint_speed`` and timestamps): the slide goes
along the chart only as far as no joint moves more than
``max_joint_speed * dt``, so the hand stays on the target. That
point becomes the one followed, and ``lag`` is the distance still to
go. The rest is not queued: sending ``value`` again on later ticks
continues the slide, and the call that arrives is ``OK``.
- ``HELD``: ``q`` unchanged. ``reason`` is ``"limits"`` when ``value``
is on the chart but outside the arc, and ``"jump"`` when it is past a
fold or a branch junction (going on would mean leaving the chart for
another branch) or :attr:`redundancy` is ``None``.
A slide stays on one branch however long it is, so ``moved`` can
exceed ``jump_threshold`` without a rate limit; ``max_joint_speed``
bounds the motion per tick. :meth:`update` then holds the coordinate
set here as the target moves.
:param value: the redundancy coordinate to slide to.
:param t: optional timestamp, as for :meth:`update`.
:raises NotImplementedError: on an arm without a closed-form chart, as
``Manipulator.self_motion`` raises (:attr:`redundancy` is always
``None`` there).
:raises TypeError: if ``value`` or ``t`` is not a real number.
:raises ValueError: if ``value`` or ``t`` is not finite, or ``t`` is
earlier than the previous timestamp.
"""
if isinstance(value, bool) or not isinstance(value, numbers.Real):
raise TypeError(f"value must be a real number, got {type(value).__name__}")
v = float(value)
if not math.isfinite(v):
raise ValueError(f"value must be finite, got {v}")
_check_time(t)
if not self._chart_family:
raise NotImplementedError(
f"set_redundancy: {self._arm.solver_name} has no closed-form chart, so a "
"Tracker on it has no redundancy coordinate"
)
tt, dt = self._advance_time(t)
T = self._target
red = self.redundancy
if red is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "jump")
chart, t0 = red.chart, red.t
t1 = self._resolve(chart, t0, red.arc, v)
if isinstance(t1, str):
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, t1)
path = self._slide_path(chart, t0, t1)
if path is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "jump")
ts, qs = path
requested = self._admit(self._polish(qs[-1], T), T, seed=qs[-1])
lo, hi = red.arc
inward = 1.0 if t1 - lo < hi - t1 else -1.0
t_end = t1
for inset in _END_INSETS:
# An arc end found by in_limits() can sit a few 1e-8 rad past the
# limit it marks, where the limits either refuse the point or
# clamp it onto the limit (which costs FK closure ~1e-9). Step
# in from it to the first point admitted as it is.
if requested is not None and self._closes(requested, T):
break
t_in = t1 + inward * inset
if not lo <= t_in <= hi:
break
q_in = self._chart_q(chart, t_in, qs[-1])
inner = self._admit(self._polish(q_in, T), T, seed=q_in)
if inner is not None and (requested is None or self._closes(inner, T)):
requested, t_end = inner, t_in
t1 = t_end
if requested is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "limits")
if self._speed is not None and dt is not None:
cap = np.broadcast_to(np.asarray(self._speed, dtype=np.float64) * dt, qs[-1].shape)
def within(q: NDArray[np.float64]) -> bool:
return bool(np.all(np.abs(self._delta(self._q, q)) <= cap))
first_out = next((i for i in range(len(ts)) if not within(qs[i])), None)
if first_out == 0:
# The arm is still behind the followed point (an earlier rate
# limit): close that gap first, as update() does.
q_new, _ = self._toward(self._goal, dt)
return self._step(q_new, TrackStatus.LIMITED, T, tt, lag_to=requested)
if first_out is not None:
# Narrow down the furthest point of the slide the limit
# reaches, one batched chart evaluation per round.
a, b = float(ts[first_out - 1]), float(ts[first_out])
q_a = qs[first_out - 1]
for _ in range(_NARROW_ROUNDS):
sub = np.linspace(a, b, _NARROW_POINTS)[1:-1]
q_sub = chart.q((sub + np.pi) % _TWO_PI - np.pi if chart.periodic else sub)
for tm, qm in zip(sub, q_sub, strict=True):
qm = self._snap(qm, q_a)
if not within(qm):
b = float(tm)
break
a, q_a = float(tm), qm
reached = self._admit(self._polish(q_a, T), T, seed=q_a)
if reached is not None and within(reached):
self._goal = reached
self._head = (chart, _wrap(a) if chart.periodic else a)
return self._step(reached.copy(), TrackStatus.LIMITED, T, tt, lag_to=requested)
return self._step(self._q.copy(), TrackStatus.LIMITED, T, tt, lag_to=requested)
self._goal = requested
self._head = (chart, _wrap(t1) if chart.periodic else t1)
return self._step(requested.copy(), TrackStatus.OK, T, tt)
|
q
property
The configuration last returned (q0 before any update), read-only.
target
property
The pose of the branch point being followed: the last target the
tracker accepted (FK(q0) before any update), read-only.
redundancy
property
redundancy: Redundancy | None
Where the tracker is on the self-motion: the followed chart, its
coordinate, and the arc :meth:set_redundancy can slide along.
None on an arm without a closed-form chart (6R arms, and 7R arms
other than seven_r.spherical_shoulder / seven_r.srs), and on a
chart arm whose followed point no chart locates (a branch junction).
Computed when read (the chart's :meth:~ssik.chart.Chart.in_limits),
so :meth:update pays nothing for it.
update
update(T: ArrayLike, t: float | None = None) -> TrackerStep
Follow the branch to target T (timestamp t, seconds).
OK: the continuation is within jump_threshold and, under a
rate limit, reachable this tick. q closes FK to T.LIMITED: same branch, but the move was clamped to
max_joint_speed * dt. Later updates keep closing the gap.HELD: the previous q is returned unchanged; reason is
"unreachable", "limits" or "jump". The tracker keeps
following the branch it was on, so it resumes when the target
comes back within reach.JUMPED: with allow_jump=True, a candidate beyond
jump_threshold was taken (rate-limited like any move).
Parameters:
| Name |
Type |
Description |
Default |
T
|
ArrayLike
|
4x4 target pose in the arm's base frame (the flange, as for
solve()).
|
required
|
t
|
float | None
|
optional timestamp. Timestamps must not decrease.
|
None
|
Raises:
| Type |
Description |
TypeError
|
if T is not an array of real numbers or t is
not a number.
|
ValueError
|
if T is not a finite rigid (4, 4) transform
(docs/api.md, "Input validation"), or t is not finite or is
earlier than the previous timestamp.
|
Source code in src/ssik/tracker.py
| def update(self, T: ArrayLike, t: float | None = None) -> TrackerStep:
"""Follow the branch to target ``T`` (timestamp ``t``, seconds).
- ``OK``: the continuation is within ``jump_threshold`` and, under a
rate limit, reachable this tick. ``q`` closes FK to ``T``.
- ``LIMITED``: same branch, but the move was clamped to
``max_joint_speed * dt``. Later updates keep closing the gap.
- ``HELD``: the previous ``q`` is returned unchanged; ``reason`` is
``"unreachable"``, ``"limits"`` or ``"jump"``. The tracker keeps
following the branch it was on, so it resumes when the target
comes back within reach.
- ``JUMPED``: with ``allow_jump=True``, a candidate beyond
``jump_threshold`` was taken (rate-limited like any move).
:param T: 4x4 target pose in the arm's base frame (the flange, as for
``solve()``).
:param t: optional timestamp. Timestamps must not decrease.
:raises TypeError: if ``T`` is not an array of real numbers or ``t`` is
not a number.
:raises ValueError: if ``T`` is not a finite rigid ``(4, 4)`` transform
(``docs/api.md``, "Input validation"), or ``t`` is not finite or is
earlier than the previous timestamp.
"""
from ssik._solve_inputs import check_pose
T_arr = check_pose(T, policy=self._policy, name="T")
tt, dt = self._advance_time(t)
cand, head, blocked = self._candidate(T_arr)
dist = math.inf if cand is None else self._dist(self._goal, cand)
if cand is None or (dist > self._jump and not self._allow_jump):
reason = self._hold_reason(T_arr, dist, blocked)
reported = math.nan if cand is None else dist
return self._step(self._q.copy(), TrackStatus.HELD, T_arr, tt, reason, reported)
assert cand is not None
jumped = dist > self._jump
self._goal = cand
self._target = T_arr
if self._chart_family:
self._head = head
q_new, limited = self._toward(cand, dt)
if jumped:
status = TrackStatus.JUMPED
else:
status = TrackStatus.LIMITED if limited else TrackStatus.OK
return self._step(q_new, status, T_arr, tt, branch_distance=dist)
|
solutions
solutions(max_solutions: int | None = None) -> list[Solution]
Every configuration at :attr:target, nearest the followed branch
first: solve(target, q_seed=..., enumerate_windings=False) under
the tracker's respect_limits. On a redundant 7R arm these are
solve()'s samples of the self-motion. For rendering the other
branches ("ghost arms"); :meth:next_branch switches to one.
Source code in src/ssik/tracker.py
| def solutions(self, max_solutions: int | None = None) -> list[Solution]:
"""Every configuration at :attr:`target`, nearest the followed branch
first: ``solve(target, q_seed=..., enumerate_windings=False)`` under
the tracker's ``respect_limits``. On a redundant 7R arm these are
``solve()``'s samples of the self-motion. For rendering the other
branches ("ghost arms"); :meth:`next_branch` switches to one.
"""
kwargs: dict[str, Any] = {}
if self._arm._prebuilt is not None:
kwargs["native"] = self._native
sols: list[Solution] = self._arm.solve(
self._target,
q_seed=self._goal,
max_solutions=max_solutions,
respect_limits=self._respect,
enumerate_windings=False,
**kwargs,
)
return sols
|
next_branch
next_branch() -> TrackerStep | None
Switch to the next branch at :attr:target, cycling through them
in a fixed order (on a 7R chart arm: by chart label, at the held
redundancy coordinate; otherwise: by configuration). Returns the
JUMPED step, or None when there is no other branch.
Without a rate limit the returned q is on the new branch. With
max_joint_speed set the arm does not move here: the new branch
becomes the one followed, and later timestamped updates carry the arm
there at the joint-speed limit (LIMITED until it arrives).
Source code in src/ssik/tracker.py
| def next_branch(self) -> TrackerStep | None:
"""Switch to the next branch at :attr:`target`, cycling through them
in a fixed order (on a 7R chart arm: by chart label, at the held
redundancy coordinate; otherwise: by configuration). Returns the
``JUMPED`` step, or ``None`` when there is no other branch.
Without a rate limit the returned ``q`` is on the new branch. With
``max_joint_speed`` set the arm does not move here: the new branch
becomes the one followed, and later timestamped updates carry the arm
there at the joint-speed limit (``LIMITED`` until it arrives).
"""
points = self._branch_points()
if len(points) < 2:
return None
cur = min(range(len(points)), key=lambda i: self._dist(self._goal, points[i][0]))
q_next, head = points[(cur + 1) % len(points)]
if self._dist(self._goal, q_next) == 0.0:
return None
dist = self._dist(self._goal, q_next)
self._goal = q_next
if self._chart_family:
self._head = head
q_new = q_next.copy() if self._speed is None else self._q.copy()
return self._step(
q_new, TrackStatus.JUMPED, self._target, self._t_last, branch_distance=dist
)
|
reset
reset(q: ArrayLike, *, t: float | None = None) -> None
Restart from configuration q (the arm is there now), with
optional timestamp t. The branch is whichever q is on.
Raises:
| Type |
Description |
TypeError
|
if q is not an array of real numbers or t is
not a number.
|
ValueError
|
if q is not a finite (dof,) vector or t
is not finite.
|
Source code in src/ssik/tracker.py
| def reset(self, q: ArrayLike, *, t: float | None = None) -> None:
"""Restart from configuration ``q`` (the arm is there now), with
optional timestamp ``t``. The branch is whichever ``q`` is on.
:raises TypeError: if ``q`` is not an array of real numbers or ``t`` is
not a number.
:raises ValueError: if ``q`` is not a finite ``(dof,)`` vector or ``t``
is not finite.
"""
from ssik._solve_inputs import check_joints
q_arr = check_joints(q, self._arm.dof, "q0").copy()
self._t_last = _check_time(t)
self._q = q_arr
self._goal = q_arr.copy()
self._target = self._arm.fk(q_arr)
self._head: tuple[Chart, float] | None = None
if self._chart_family:
self._head = self._locate(self._target, q_arr)
|
set_redundancy
set_redundancy(value: float, t: float | None = None) -> TrackerStep
Slide along the self-motion at the held target: the hand stays at
:attr:target while the followed chart's redundancy coordinate (q6
or the swivel angle, :attr:redundancy) moves to value.
OK: value is on :attr:redundancy's arc (on the swivel,
some value + 2*pi*k is; with the whole circle in limits, the
shorter way round is taken). q is the chart point there, followed
along the arc from the current one, and closes FK to the target.LIMITED (max_joint_speed and timestamps): the slide goes
along the chart only as far as no joint moves more than
max_joint_speed * dt, so the hand stays on the target. That
point becomes the one followed, and lag is the distance still to
go. The rest is not queued: sending value again on later ticks
continues the slide, and the call that arrives is OK.HELD: q unchanged. reason is "limits" when value
is on the chart but outside the arc, and "jump" when it is past a
fold or a branch junction (going on would mean leaving the chart for
another branch) or :attr:redundancy is None.
A slide stays on one branch however long it is, so moved can
exceed jump_threshold without a rate limit; max_joint_speed
bounds the motion per tick. :meth:update then holds the coordinate
set here as the target moves.
Parameters:
| Name |
Type |
Description |
Default |
value
|
float
|
the redundancy coordinate to slide to.
|
required
|
t
|
float | None
|
optional timestamp, as for :meth:update.
|
None
|
Raises:
| Type |
Description |
NotImplementedError
|
on an arm without a closed-form chart, as
Manipulator.self_motion raises (:attr:redundancy is always
None there).
|
TypeError
|
if value or t is not a real number.
|
ValueError
|
if value or t is not finite, or t is
earlier than the previous timestamp.
|
Source code in src/ssik/tracker.py
| def set_redundancy(self, value: float, t: float | None = None) -> TrackerStep:
"""Slide along the self-motion at the held target: the hand stays at
:attr:`target` while the followed chart's redundancy coordinate (``q6``
or the swivel angle, :attr:`redundancy`) moves to ``value``.
- ``OK``: ``value`` is on :attr:`redundancy`'s ``arc`` (on the swivel,
some ``value + 2*pi*k`` is; with the whole circle in limits, the
shorter way round is taken). ``q`` is the chart point there, followed
along the arc from the current one, and closes FK to the target.
- ``LIMITED`` (``max_joint_speed`` and timestamps): the slide goes
along the chart only as far as no joint moves more than
``max_joint_speed * dt``, so the hand stays on the target. That
point becomes the one followed, and ``lag`` is the distance still to
go. The rest is not queued: sending ``value`` again on later ticks
continues the slide, and the call that arrives is ``OK``.
- ``HELD``: ``q`` unchanged. ``reason`` is ``"limits"`` when ``value``
is on the chart but outside the arc, and ``"jump"`` when it is past a
fold or a branch junction (going on would mean leaving the chart for
another branch) or :attr:`redundancy` is ``None``.
A slide stays on one branch however long it is, so ``moved`` can
exceed ``jump_threshold`` without a rate limit; ``max_joint_speed``
bounds the motion per tick. :meth:`update` then holds the coordinate
set here as the target moves.
:param value: the redundancy coordinate to slide to.
:param t: optional timestamp, as for :meth:`update`.
:raises NotImplementedError: on an arm without a closed-form chart, as
``Manipulator.self_motion`` raises (:attr:`redundancy` is always
``None`` there).
:raises TypeError: if ``value`` or ``t`` is not a real number.
:raises ValueError: if ``value`` or ``t`` is not finite, or ``t`` is
earlier than the previous timestamp.
"""
if isinstance(value, bool) or not isinstance(value, numbers.Real):
raise TypeError(f"value must be a real number, got {type(value).__name__}")
v = float(value)
if not math.isfinite(v):
raise ValueError(f"value must be finite, got {v}")
_check_time(t)
if not self._chart_family:
raise NotImplementedError(
f"set_redundancy: {self._arm.solver_name} has no closed-form chart, so a "
"Tracker on it has no redundancy coordinate"
)
tt, dt = self._advance_time(t)
T = self._target
red = self.redundancy
if red is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "jump")
chart, t0 = red.chart, red.t
t1 = self._resolve(chart, t0, red.arc, v)
if isinstance(t1, str):
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, t1)
path = self._slide_path(chart, t0, t1)
if path is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "jump")
ts, qs = path
requested = self._admit(self._polish(qs[-1], T), T, seed=qs[-1])
lo, hi = red.arc
inward = 1.0 if t1 - lo < hi - t1 else -1.0
t_end = t1
for inset in _END_INSETS:
# An arc end found by in_limits() can sit a few 1e-8 rad past the
# limit it marks, where the limits either refuse the point or
# clamp it onto the limit (which costs FK closure ~1e-9). Step
# in from it to the first point admitted as it is.
if requested is not None and self._closes(requested, T):
break
t_in = t1 + inward * inset
if not lo <= t_in <= hi:
break
q_in = self._chart_q(chart, t_in, qs[-1])
inner = self._admit(self._polish(q_in, T), T, seed=q_in)
if inner is not None and (requested is None or self._closes(inner, T)):
requested, t_end = inner, t_in
t1 = t_end
if requested is None:
return self._step(self._q.copy(), TrackStatus.HELD, T, tt, "limits")
if self._speed is not None and dt is not None:
cap = np.broadcast_to(np.asarray(self._speed, dtype=np.float64) * dt, qs[-1].shape)
def within(q: NDArray[np.float64]) -> bool:
return bool(np.all(np.abs(self._delta(self._q, q)) <= cap))
first_out = next((i for i in range(len(ts)) if not within(qs[i])), None)
if first_out == 0:
# The arm is still behind the followed point (an earlier rate
# limit): close that gap first, as update() does.
q_new, _ = self._toward(self._goal, dt)
return self._step(q_new, TrackStatus.LIMITED, T, tt, lag_to=requested)
if first_out is not None:
# Narrow down the furthest point of the slide the limit
# reaches, one batched chart evaluation per round.
a, b = float(ts[first_out - 1]), float(ts[first_out])
q_a = qs[first_out - 1]
for _ in range(_NARROW_ROUNDS):
sub = np.linspace(a, b, _NARROW_POINTS)[1:-1]
q_sub = chart.q((sub + np.pi) % _TWO_PI - np.pi if chart.periodic else sub)
for tm, qm in zip(sub, q_sub, strict=True):
qm = self._snap(qm, q_a)
if not within(qm):
b = float(tm)
break
a, q_a = float(tm), qm
reached = self._admit(self._polish(q_a, T), T, seed=q_a)
if reached is not None and within(reached):
self._goal = reached
self._head = (chart, _wrap(a) if chart.periodic else a)
return self._step(reached.copy(), TrackStatus.LIMITED, T, tt, lag_to=requested)
return self._step(self._q.copy(), TrackStatus.LIMITED, T, tt, lag_to=requested)
self._goal = requested
self._head = (chart, _wrap(t1) if chart.periodic else t1)
return self._step(requested.copy(), TrackStatus.OK, T, tt)
|
Where a :class:Tracker is on its arm's self-motion (:attr:Tracker.redundancy).
Parameters:
| Name |
Type |
Description |
Default |
chart
|
Chart
|
the followed chart, at :attr:Tracker.target.
|
required
|
label
|
tuple[int, ...]
|
its branch label (chart.label).
|
required
|
parameter
|
str
|
the redundancy coordinate's name: "q6"
(seven_r.spherical_shoulder) or "swivel" (seven_r.srs).
|
required
|
periodic
|
bool
|
whether the coordinate lives on a circle (the swivel).
|
required
|
t
|
float
|
the coordinate of the followed point (the point the arm is at,
unless a rate limit holds it back).
|
required
|
arc
|
tuple[float, float]
|
(lo, hi) with lo <= t <= hi: the stretch of the chart
:meth:Tracker.set_redundancy can slide along. It is the
:meth:~ssik.chart.Chart.in_limits arc containing t (the chart's
domain interval when the tracker does not respect limits); a fold or a
branch junction is an end, and a single in-limit point is (t, t).
On the periodic swivel the pieces that meet at +-pi are one arc,
shifted by 2*pi where needed to contain t; (-pi, pi) when
the whole circle is in limits.
|
required
|
Source code in src/ssik/tracker.py
| @dataclass(frozen=True, eq=False)
class Redundancy:
"""Where a :class:`Tracker` is on its arm's self-motion (:attr:`Tracker.redundancy`).
:param chart: the followed chart, at :attr:`Tracker.target`.
:param label: its branch label (``chart.label``).
:param parameter: the redundancy coordinate's name: ``"q6"``
(``seven_r.spherical_shoulder``) or ``"swivel"`` (``seven_r.srs``).
:param periodic: whether the coordinate lives on a circle (the swivel).
:param t: the coordinate of the followed point (the point the arm is at,
unless a rate limit holds it back).
:param arc: ``(lo, hi)`` with ``lo <= t <= hi``: the stretch of the chart
:meth:`Tracker.set_redundancy` can slide along. It is the
:meth:`~ssik.chart.Chart.in_limits` arc containing ``t`` (the chart's
domain interval when the tracker does not respect limits); a fold or a
branch junction is an end, and a single in-limit point is ``(t, t)``.
On the periodic swivel the pieces that meet at ``+-pi`` are one arc,
shifted by ``2*pi`` where needed to contain ``t``; ``(-pi, pi)`` when
the whole circle is in limits.
"""
chart: Chart
label: tuple[int, ...]
parameter: str
periodic: bool
t: float
arc: tuple[float, float]
|
Bases: Enum
What one :meth:Tracker.update did.
Source code in src/ssik/tracker.py
| class TrackStatus(enum.Enum):
"""What one :meth:`Tracker.update` did."""
#: The configuration is the branch's continuation at the new target.
OK = "ok"
#: Same branch, but the move was clamped to ``max_joint_speed * dt``; the
#: configuration lags the target and catches up over later updates.
LIMITED = "limited"
#: The last configuration was kept; :attr:`TrackerStep.reason` says why.
HELD = "held"
#: The tracker switched branch (``allow_jump=True``, or
#: :meth:`Tracker.next_branch`).
JUMPED = "jumped"
|
One :meth:Tracker.update (or :meth:Tracker.next_branch) result.
Parameters:
| Name |
Type |
Description |
Default |
q
|
NDArray[float64]
|
the configuration to command, (dof,), read-only. Under
HELD it is the previous one, unchanged.
|
required
|
status
|
TrackStatus
|
|
required
|
reason
|
HoldReason | None
|
under HELD, why: "unreachable" (no configuration
reaches the target), "limits" (the branch continues only outside
the joint limits, or no in-limit configuration exists) or "jump"
(the nearest configuration is further than jump_threshold: a branch
switch). None otherwise.
|
required
|
fk_residual
|
float
|
||FK(q) - T||_F against the target of this update.
Within the solver's tolerance under OK and JUMPED without a rate
limit; larger under LIMITED (the arm is still on its way) and
HELD (the arm did not follow).
|
required
|
moved
|
float
|
how far q moved from the previous step's q, in ssik's
seed metric (the largest single-joint move, continuous joints compared
on the circle).
|
required
|
branch_distance
|
float
|
the seed-metric distance from the branch point the
tracker was following to the candidate at this target; nan when
there was none. Compared against jump_threshold.
|
required
|
lag
|
float
|
the seed-metric distance from q to the branch point being
followed; 0 unless a rate limit is holding the arm back.
|
required
|
t
|
float | None
|
the timestamp passed to this update, or None.
|
required
|
Source code in src/ssik/tracker.py
| @dataclass(frozen=True, eq=False)
class TrackerStep:
"""One :meth:`Tracker.update` (or :meth:`Tracker.next_branch`) result.
:param q: the configuration to command, ``(dof,)``, read-only. Under
``HELD`` it is the previous one, unchanged.
:param status: the :class:`TrackStatus`.
:param reason: under ``HELD``, why: ``"unreachable"`` (no configuration
reaches the target), ``"limits"`` (the branch continues only outside
the joint limits, or no in-limit configuration exists) or ``"jump"``
(the nearest configuration is further than ``jump_threshold``: a branch
switch). ``None`` otherwise.
:param fk_residual: ``||FK(q) - T||_F`` against the target of this update.
Within the solver's tolerance under ``OK`` and ``JUMPED`` without a rate
limit; larger under ``LIMITED`` (the arm is still on its way) and
``HELD`` (the arm did not follow).
:param moved: how far ``q`` moved from the previous step's ``q``, in ssik's
seed metric (the largest single-joint move, continuous joints compared
on the circle).
:param branch_distance: the seed-metric distance from the branch point the
tracker was following to the candidate at this target; ``nan`` when
there was none. Compared against ``jump_threshold``.
:param lag: the seed-metric distance from ``q`` to the branch point being
followed; ``0`` unless a rate limit is holding the arm back.
:param t: the timestamp passed to this update, or ``None``.
"""
q: NDArray[np.float64]
status: TrackStatus
reason: HoldReason | None
fk_residual: float
moved: float
branch_distance: float
lag: float
t: float | None
@property
def reachable(self) -> bool:
"""``False`` only when no configuration reaches the target."""
return self.reason != "unreachable"
|
reachable
property
False only when no configuration reaches the target.
Teleoperation frames: ssik.teleop
The steps between a device and Tracker.update are rigid-transform
compositions, collected in ssik.teleop. ssik ships no device code: a device
is anything with a poses() method yielding (T, t), the PoseSource
protocol. examples/06_teleop.py wires a scripted source through every step.
Conventions. A pose is a 4x4 homogeneous rigid transform a_T_b, the pose
of frame b in frame a; it maps b-coordinates to a-coordinates and
composes as a_T_c = a_T_b @ b_T_c. Every pose argument is checked as
solve() checks T_target (Input validation), with the
argument's name in the message.
| Helper |
Returns |
invert(T) |
[R^T, -R^T p] |
calibration_from(T_device, T_robot) |
base_T_tracking = T_robot @ T_device^-1, the calibration that maps this device reading onto this robot pose |
apply_calibration(calibration, T_device) |
calibration @ T_device: a reading tracking_T_device in the arm's base frame |
tcp_to_flange(T_tcp, flange_T_tcp) |
T_tcp @ flange_T_tcp^-1: the flange pose that puts the tool centre point at T_tcp (what IK solves for) |
flange_to_tcp(T_flange, flange_T_tcp) |
T_flange @ flange_T_tcp: the TCP pose at a flange pose |
scale_about(T, scale, anchor) |
position a + scale * (p - a) about the anchor point a (a (3,) point, or a pose's position, in T's frame), rotation unchanged |
Calibration. A device reports its poses in its own fixed frame, the
tracking frame. The calibration is base_T_tracking, the tracking frame in
the arm's base frame. When the arm is mounted in a world frame of its own (a
frame aligned with the room, with the arm at a shoulder), it is
base_T_world @ world_T_tracking: world_T_tracking comes from the device
setup and base_T_world = invert(world_T_base) from the arm's mounting.
Clutch. Relative teleoperation: the arm follows the device's motion since
the grip was pressed, not its absolute pose. engage(T_device, T_robot)
stores the device anchor A_d = [R_ad, p_ad] and the arm anchor
A_r = [R_ar, p_ar], both in one frame: the arm's base frame, after
apply_calibration. While engaged, a device pose D = [R_d, p_d] gives a
target by one of two conventions, chosen by Clutch(scale=1.0, frame=...).
frame="world" (default): the device's motion, measured in the base frame,
is applied in the base frame.
p = p_ar + scale * (p_d - p_ad)
R = (R_d @ R_ad^T) @ R_ar
The rotation since engaging, R_d @ R_ad^T, turns the tool about its own
point, with the axis taken in the base frame. A pure device translation Δ
moves the tool by scale * Δ in the base frame, whatever the device or the
tool is pointing at. Use it when the operator watches the arm and moves a
hand-held device (a VR controller, a mocap marker, a gizmo): moving the hand
toward the arm's +x moves the tool toward +x.
frame="tool": the device's motion, measured in its own frame at engagement,
is applied in the tool's frame at engagement.
target(D) = A_r @ S(A_d^-1 @ D)
A_d^-1 @ D is the device's displacement in the device's frame at engagement;
S multiplies its translation by scale. With scale = 1 this is
(A_r @ A_d^-1) @ D, a fixed rigid transform of the device's pose, so relative
motion is reproduced exactly and the device's axes act as the tool's axes:
pushing a SpaceMouse cap forward moves the tool along its own approach axis.
Use it for jogging along the tool's axes.
In both conventions:
target(A_d) == A_r: engaging never moves the target.- Rotation is never scaled, and scaling equals
scale_about(D, scale, A_d)
followed by the unscaled clutch.
release() makes target return None (the caller holds). The next
engage re-anchors both frames, so the operator can reposition the device
without moving the arm. Engaging again at the current device pose and
target continues exactly where the clutch was.- The two agree when the device and arm anchors have the same orientation.
- Both commute with a common rigid change of frame
X: engaging at X @ A_d
and X @ A_r and reading X @ D gives X @ target(D). Clutching in the
base frame after calibration therefore equals clutching in the world frame,
so with frame="world" a hand moved along the room's +x moves the tool
along the room's +x, wherever the arm is mounted.
Engage with the arm's actual pose, flange_to_tcp(arm.fk(tracker.q), tool),
so a rate-limited arm that is still catching up is anchored where it is.
Frame helpers for teleoperation: calibration, tool offset, scaling, clutch.
A teleoperation loop turns device poses into arm targets and hands them to a
:class:~ssik.Tracker. The device (a VR controller, a SpaceMouse, a mocap
marker, a transform gizmo) reports poses in its own frame; the arm's IK wants
the flange in the arm's base frame. Everything between the two is a handful of
rigid-transform compositions, collected here so they are written once and
tested for the invariants that matter. ssik ships no device integration: a
device is anything that yields poses, the :class:PoseSource protocol.
Conventions. Every pose is a 4x4 homogeneous rigid transform a_T_b: the
pose of frame b expressed in frame a, so it maps b-coordinates to
a-coordinates and composes as a_T_c = a_T_b @ b_T_c. Every function
checks each pose against the rigid-transform rule of solve() (default
tolerance; docs/api.md, "Input validation") and raises TypeError or
ValueError as it does.
A typical chain, device reading D (tracking_T_device, in the device's
own tracking frame) to IK target::
D_base = apply_calibration(base_T_tracking, D) # into the arm's base frame
T_tcp = clutch.target(D_base) # relative motion, while held
T = tcp_to_flange(T_tcp, flange_T_tcp) # the pose ssik solves for
step = tracker.update(T, t)
The calibration is base_T_tracking. An arm mounted in a world frame of its
own (a room frame, with the arm at a shoulder) has
base_T_tracking = invert(world_T_base) @ world_T_tracking: the mounting
gives world_T_base, the device setup world_T_tracking.
docs/api.md ("Teleoperation frames") is the normative statement of these
conventions.
PoseSource
Bases: Protocol
Anything that streams poses: the one interface a device adapter needs.
poses() yields (T, t) pairs: T a 4x4 rigid transform (the
device's pose in its own fixed frame) and t a timestamp in seconds that
never decreases. The iterator ends when the stream does. ssik ships no
adapters; examples/06_teleop.py has a scripted source and a stub showing
where a VR or SpaceMouse reader plugs in.
Source code in src/ssik/teleop.py
| @runtime_checkable
class PoseSource(Protocol):
"""Anything that streams poses: the one interface a device adapter needs.
``poses()`` yields ``(T, t)`` pairs: ``T`` a 4x4 rigid transform (the
device's pose in its own fixed frame) and ``t`` a timestamp in seconds that
never decreases. The iterator ends when the stream does. ssik ships no
adapters; ``examples/06_teleop.py`` has a scripted source and a stub showing
where a VR or SpaceMouse reader plugs in.
"""
def poses(self) -> Iterator[tuple[NDArray[np.float64], float]]: ...
|
Clutch
Relative ("clutched") teleoperation: the arm follows the device's motion
since the clutch engaged, not the device's absolute pose.
::
clutch = Clutch(scale=0.5)
clutch.engage(T_device, T_robot) # grip pressed
T = clutch.target(T_device) # each frame while held
clutch.release() # grip released
On :meth:engage the clutch stores the device pose A_d = [R_ad, p_ad]
and the arm pose A_r = [R_ar, p_ar], both in one frame: the arm's base
frame, after :func:apply_calibration. While engaged, for a device pose
D = [R_d, p_d], frame picks how the device's motion since engaging
is applied to the arm:
frame="world" (default)::
p = p_ar + scale * (p_d - p_ad)
R = (R_d @ R_ad^T) @ R_ar
The device's translation and rotation since engaging, both expressed in
the base frame, are applied to the arm in the base frame, the rotation
about the arm's tool point. Moving the hand 10 cm along the base x axis
moves the tool scale * 10 cm along the base x axis, whatever either is
pointing at. This is what an operator looking at the arm expects.
frame="tool"::
target(D) = A_r @ S(A_d^-1 @ D)
A_d^-1 @ D is the device's displacement in the device's own frame at
engagement; S multiplies its translation by scale; the arm makes
that displacement in its tool frame at the anchor. With scale = 1 this
is the fixed rigid transform (A_r @ A_d^-1) @ D of the device's pose:
the device's axes act as the tool's axes, which suits jogging along the
tool's own axes (a SpaceMouse held like the tool).
In both modes the target does not jump at engagement
(target(A_d) == A_r), rotation is never scaled, and releasing and
engaging again re-anchors both frames, so the operator can reposition the
device without moving the arm (indexing). The two modes agree when the
device and arm anchors have the same orientation.
Source code in src/ssik/teleop.py
| class Clutch:
"""Relative ("clutched") teleoperation: the arm follows the device's motion
since the clutch engaged, not the device's absolute pose.
::
clutch = Clutch(scale=0.5)
clutch.engage(T_device, T_robot) # grip pressed
T = clutch.target(T_device) # each frame while held
clutch.release() # grip released
On :meth:`engage` the clutch stores the device pose ``A_d = [R_ad, p_ad]``
and the arm pose ``A_r = [R_ar, p_ar]``, both in one frame: the arm's base
frame, after :func:`apply_calibration`. While engaged, for a device pose
``D = [R_d, p_d]``, ``frame`` picks how the device's motion since engaging
is applied to the arm:
``frame="world"`` (default)::
p = p_ar + scale * (p_d - p_ad)
R = (R_d @ R_ad^T) @ R_ar
The device's translation and rotation since engaging, both expressed in
the base frame, are applied to the arm in the base frame, the rotation
about the arm's tool point. Moving the hand 10 cm along the base x axis
moves the tool ``scale * 10`` cm along the base x axis, whatever either is
pointing at. This is what an operator looking at the arm expects.
``frame="tool"``::
target(D) = A_r @ S(A_d^-1 @ D)
``A_d^-1 @ D`` is the device's displacement in the device's own frame at
engagement; ``S`` multiplies its translation by ``scale``; the arm makes
that displacement in its tool frame at the anchor. With ``scale = 1`` this
is the fixed rigid transform ``(A_r @ A_d^-1) @ D`` of the device's pose:
the device's axes act as the tool's axes, which suits jogging along the
tool's own axes (a SpaceMouse held like the tool).
In both modes the target does not jump at engagement
(``target(A_d) == A_r``), rotation is never scaled, and releasing and
engaging again re-anchors both frames, so the operator can reposition the
device without moving the arm (indexing). The two modes agree when the
device and arm anchors have the same orientation.
"""
__slots__ = ("_anchor_device", "_anchor_robot", "_frame", "_scale")
def __init__(self, *, scale: float = 1.0, frame: Literal["world", "tool"] = "world") -> None:
if frame not in ("world", "tool"):
raise ValueError(f"frame must be 'world' or 'tool', got {frame!r}")
self._scale = _scale(scale)
self._frame = frame
self._anchor_device: NDArray[np.float64] | None = None
self._anchor_robot: NDArray[np.float64] | None = None
@property
def scale(self) -> float:
"""The translation scale applied to the device's displacement."""
return self._scale
@property
def frame(self) -> Literal["world", "tool"]:
"""``"world"`` or ``"tool"``: where the device's motion is applied."""
return self._frame
@property
def engaged(self) -> bool:
"""Whether the clutch is engaged."""
return self._anchor_robot is not None
def engage(self, T_device: ArrayLike, T_robot: ArrayLike) -> None:
"""Anchor: the device is at ``T_device`` and the arm at ``T_robot``
now. Engaging while engaged re-anchors."""
A_d = _pose(T_device, "T_device")
A_r = _pose(T_robot, "T_robot")
self._anchor_device = A_d.copy()
self._anchor_robot = A_r.copy()
def release(self) -> None:
"""Disengage. :meth:`target` returns ``None`` until the next engage."""
self._anchor_device = None
self._anchor_robot = None
def target(self, T_device: ArrayLike) -> NDArray[np.float64] | None:
"""The arm target for device pose ``T_device``, or ``None`` while
released (the arm should hold)."""
D = _pose(T_device, "T_device")
A_d, A_r = self._anchor_device, self._anchor_robot
if A_r is None or A_d is None:
return None
if self._frame == "tool":
rel = _inv(A_d) @ D
rel[:3, 3] *= self._scale
out: NDArray[np.float64] = A_r @ rel
return out
out = np.eye(4)
out[:3, :3] = (D[:3, :3] @ A_d[:3, :3].T) @ A_r[:3, :3]
out[:3, 3] = A_r[:3, 3] + self._scale * (D[:3, 3] - A_d[:3, 3])
return out
|
scale
property
The translation scale applied to the device's displacement.
frame
property
frame: Literal['world', 'tool']
"world" or "tool": where the device's motion is applied.
engaged
property
Whether the clutch is engaged.
engage
engage(T_device: ArrayLike, T_robot: ArrayLike) -> None
Anchor: the device is at T_device and the arm at T_robot
now. Engaging while engaged re-anchors.
Source code in src/ssik/teleop.py
| def engage(self, T_device: ArrayLike, T_robot: ArrayLike) -> None:
"""Anchor: the device is at ``T_device`` and the arm at ``T_robot``
now. Engaging while engaged re-anchors."""
A_d = _pose(T_device, "T_device")
A_r = _pose(T_robot, "T_robot")
self._anchor_device = A_d.copy()
self._anchor_robot = A_r.copy()
|
release
Disengage. :meth:target returns None until the next engage.
Source code in src/ssik/teleop.py
| def release(self) -> None:
"""Disengage. :meth:`target` returns ``None`` until the next engage."""
self._anchor_device = None
self._anchor_robot = None
|
target
target(T_device: ArrayLike) -> NDArray[np.float64] | None
The arm target for device pose T_device, or None while
released (the arm should hold).
Source code in src/ssik/teleop.py
| def target(self, T_device: ArrayLike) -> NDArray[np.float64] | None:
"""The arm target for device pose ``T_device``, or ``None`` while
released (the arm should hold)."""
D = _pose(T_device, "T_device")
A_d, A_r = self._anchor_device, self._anchor_robot
if A_r is None or A_d is None:
return None
if self._frame == "tool":
rel = _inv(A_d) @ D
rel[:3, 3] *= self._scale
out: NDArray[np.float64] = A_r @ rel
return out
out = np.eye(4)
out[:3, :3] = (D[:3, :3] @ A_d[:3, :3].T) @ A_r[:3, :3]
out[:3, 3] = A_r[:3, 3] + self._scale * (D[:3, 3] - A_d[:3, 3])
return out
|
calibration_from
calibration_from(T_device: ArrayLike, T_robot: ArrayLike) -> NDArray[np.float64]
The calibration that maps a device reading onto a known robot pose.
Hold the device at a pose the arm also holds (or define the correspondence
you want), read T_device (tracking_T_device) and T_robot
(base_T_tcp), and this returns
base_T_tracking = T_robot @ T_device^-1, so that
apply_calibration(calibration, T_device) == T_robot.
Source code in src/ssik/teleop.py
| def calibration_from(T_device: ArrayLike, T_robot: ArrayLike) -> NDArray[np.float64]:
"""The calibration that maps a device reading onto a known robot pose.
Hold the device at a pose the arm also holds (or define the correspondence
you want), read ``T_device`` (``tracking_T_device``) and ``T_robot``
(``base_T_tcp``), and this returns
``base_T_tracking = T_robot @ T_device^-1``, so that
``apply_calibration(calibration, T_device) == T_robot``.
"""
return _pose(T_robot, "T_robot") @ _inv(_pose(T_device, "T_device"))
|
apply_calibration
apply_calibration(calibration: ArrayLike, T_device: ArrayLike) -> NDArray[np.float64]
A device reading tracking_T_device in the arm's base frame:
calibration @ T_device with calibration = base_T_tracking.
Source code in src/ssik/teleop.py
| def apply_calibration(calibration: ArrayLike, T_device: ArrayLike) -> NDArray[np.float64]:
"""A device reading ``tracking_T_device`` in the arm's base frame:
``calibration @ T_device`` with ``calibration = base_T_tracking``."""
return _pose(calibration, "calibration") @ _pose(T_device, "T_device")
|
tcp_to_flange
tcp_to_flange(T_tcp: ArrayLike, flange_T_tcp: ArrayLike) -> NDArray[np.float64]
The flange pose that puts the tool centre point at T_tcp.
T_tcp is base_T_tcp; flange_T_tcp is the tool offset (the TCP
in the flange frame). Returns base_T_flange = T_tcp @ flange_T_tcp^-1,
the pose to pass to solve() or Tracker.update().
Source code in src/ssik/teleop.py
| def tcp_to_flange(T_tcp: ArrayLike, flange_T_tcp: ArrayLike) -> NDArray[np.float64]:
"""The flange pose that puts the tool centre point at ``T_tcp``.
``T_tcp`` is ``base_T_tcp``; ``flange_T_tcp`` is the tool offset (the TCP
in the flange frame). Returns ``base_T_flange = T_tcp @ flange_T_tcp^-1``,
the pose to pass to ``solve()`` or ``Tracker.update()``.
"""
return _pose(T_tcp, "T_tcp") @ _inv(_pose(flange_T_tcp, "flange_T_tcp"))
|
flange_to_tcp
flange_to_tcp(T_flange: ArrayLike, flange_T_tcp: ArrayLike) -> NDArray[np.float64]
Where the tool centre point is when the flange is at T_flange:
T_flange @ flange_T_tcp (FK(q) in, TCP pose out). The inverse of
:func:tcp_to_flange.
Source code in src/ssik/teleop.py
| def flange_to_tcp(T_flange: ArrayLike, flange_T_tcp: ArrayLike) -> NDArray[np.float64]:
"""Where the tool centre point is when the flange is at ``T_flange``:
``T_flange @ flange_T_tcp`` (``FK(q)`` in, TCP pose out). The inverse of
:func:`tcp_to_flange`."""
return _pose(T_flange, "T_flange") @ _pose(flange_T_tcp, "flange_T_tcp")
|
scale_about
scale_about(T: ArrayLike, scale: float, anchor: ArrayLike) -> NDArray[np.float64]
Workspace scaling: T with its position scaled by scale about
anchor, its rotation unchanged.
The position becomes a + scale * (p - a), with a the anchor point:
a (3,) point, or a 4x4 pose whose position is used. Both are in the
frame T is expressed in. scale < 1 makes a large hand motion a small
arm motion. Rotation is never scaled: a scaled rotation is not a rotation.
Source code in src/ssik/teleop.py
| def scale_about(T: ArrayLike, scale: float, anchor: ArrayLike) -> NDArray[np.float64]:
"""Workspace scaling: ``T`` with its position scaled by ``scale`` about
``anchor``, its rotation unchanged.
The position becomes ``a + scale * (p - a)``, with ``a`` the anchor point:
a ``(3,)`` point, or a 4x4 pose whose position is used. Both are in the
frame ``T`` is expressed in. ``scale < 1`` makes a large hand motion a small
arm motion. Rotation is never scaled: a scaled rotation is not a rotation.
"""
P = _pose(T, "T")
s = _scale(scale)
from ssik._solve_inputs import as_real_array
a = as_real_array(anchor, "anchor")
if a.shape == (4, 4):
a = _pose(a, "anchor")[:3, 3]
elif a.shape != (3,):
raise ValueError(f"anchor must be a (3,) point or a (4, 4) pose, got {a.shape}")
elif not all(map(math.isfinite, a.tolist())):
raise ValueError("anchor must be finite (no NaN or inf)")
out = P.copy()
out[:3, 3] = a + s * (P[:3, 3] - a)
return out
|
invert
invert(T: ArrayLike) -> NDArray[np.float64]
The inverse of a rigid transform: b_T_a from a_T_b, exactly
[R^T, -R^T p] (no general matrix inverse).
Source code in src/ssik/teleop.py
| def invert(T: ArrayLike) -> NDArray[np.float64]:
"""The inverse of a rigid transform: ``b_T_a`` from ``a_T_b``, exactly
``[R^T, -R^T p]`` (no general matrix inverse)."""
return _inv(_pose(T, "T"))
|
Postprocess helpers
The solve() pipeline already applies these by default (when respect_limits=True); they're exposed for callers who want a different order, an extra filter step, or to compose with collision/dexterity scoring.
Drop solutions where any joint's q value is outside its reachable range.
Joints with limits=None are unconstrained (continuous joints, or
fixtures that don't supply limits) and never reject a solution. Joints
with limits=(lo, hi) reject any solution where q[i] is outside
[lo, hi] by more than the limit band: round-off (1e-9 rad), widened
to the solution's own angular error estimate when T_target is given
(see docs/api.md#joint-limits). A value within that band of a limit, on
either side, is at the limit: the solution is kept, with that joint set to
exactly the limit.
Parameters:
| Name |
Type |
Description |
Default |
sols
|
list[Solution]
|
candidate solutions (e.g. output of an ssik solver's
solve()).
|
required
|
kb
|
KinBody
|
the same :class:KinBody used for the IK call. Joint limits
come from kb.joints[i].limits.
|
required
|
T_target
|
NDArray[float64] | None
|
the IK target. When given, the band is the solution's
error band (#651), and a solution the clamp moved has its
fk_residual re-measured at the returned q; without it the band
is round-off and the solver's residual is kept.
|
None
|
Returns:
| Type |
Description |
list[Solution]
|
filtered solutions; preserves input order. Every returned value
lies within its limits exactly.
|
Source code in src/ssik/postprocess.py
| def respect_limits(
sols: list[Solution], kb: KinBody, *, T_target: NDArray[np.float64] | None = None
) -> list[Solution]:
"""Drop solutions where any joint's q value is outside its reachable range.
Joints with ``limits=None`` are unconstrained (continuous joints, or
fixtures that don't supply limits) and never reject a solution. Joints
with ``limits=(lo, hi)`` reject any solution where ``q[i]`` is outside
``[lo, hi]`` by more than the limit band: round-off (``1e-9`` rad), widened
to the solution's own angular error estimate when ``T_target`` is given
(see ``docs/api.md#joint-limits``). A value within that band of a limit, on
either side, is at the limit: the solution is kept, with that joint set to
exactly the limit.
:param sols: candidate solutions (e.g. output of an ssik solver's
``solve()``).
:param kb: the same :class:`KinBody` used for the IK call. Joint limits
come from ``kb.joints[i].limits``.
:param T_target: the IK target. When given, the band is the solution's
error band (#651), and a solution the clamp moved has its
``fk_residual`` re-measured at the returned ``q``; without it the band
is round-off and the solver's residual is kept.
:returns: filtered solutions; preserves input order. Every returned value
lies within its limits exactly.
"""
n_joints = len(kb.joints)
kept: list[Solution] = []
for sol in sols:
if len(sol.q) != n_joints:
raise ValueError(f"solution q-length {len(sol.q)} doesn't match kb DOF {n_joints}")
q_new: NDArray[np.float64] | None = None
within = True
band = _band_of(sol, kb, T_target)
for i, joint in enumerate(kb.joints):
if joint.limits is None:
continue
lo, hi = joint.limits
q_i = float(sol.q[i])
v = _onto_limits(q_i, lo, hi, band)
if v is None:
within = False
break
if v != q_i:
if q_new is None:
q_new = np.asarray(sol.q, dtype=np.float64).copy()
q_new[i] = v
if within:
# Any change here is a clamp: a move.
kept.append(sol if q_new is None else _placed(sol, q_new, True, kb, T_target))
return kept
|
Try wrapping each joint's q value by ±2*pi integer multiples to
bring it into the joint's reachable range.
A revolute joint at q = 3.0 with limits [-pi, pi] is FK-equivalent
to q - 2*pi ≈ -3.28, which is also outside the range, so neither
wrap fits and the solution stays at q = 3.0 (and would be dropped by
:func:respect_limits if called next). A joint at q = 4.0 with
limits [-pi, pi] wraps to q - 2*pi ≈ -2.28 which is in range:
we keep the wrapped value.
Joints with limits=None are left unchanged (no constraint to wrap
into). Prismatic joints are left unchanged (no rotational periodicity).
Search is over k ∈ {-2, -1, 0, +1, +2} integer multiples of 2*pi;
that covers any joint whose limits span up to ±5*pi (more than enough for
any commercial arm). The smallest-|k| wrap that lands in range wins,
biasing toward the original value. A wrap within the limit band of a limit,
on either side, lands exactly on the limit, as in :func:respect_limits.
Parameters:
| Name |
Type |
Description |
Default |
sols
|
list[Solution]
|
|
required
|
kb
|
KinBody
|
the same :class:KinBody used for the IK call.
|
required
|
T_target
|
NDArray[float64] | None
|
the IK target. When given, the band is the solution's
error band (#651), and a solution the band moved has its
fk_residual re-measured at the returned q; a 2*pi wrap
alone keeps it.
|
None
|
Returns:
| Type |
Description |
list[Solution]
|
solutions with each q-vector adjusted joint-wise; preserves
input order; returns Solution instances with the wrapped q
and other fields unchanged.
|
Source code in src/ssik/postprocess.py
| def wrap_to_limits(
sols: list[Solution], kb: KinBody, *, T_target: NDArray[np.float64] | None = None
) -> list[Solution]:
"""Try wrapping each joint's q value by ``±2*pi`` integer multiples to
bring it into the joint's reachable range.
A revolute joint at ``q = 3.0`` with limits ``[-pi, pi]`` is FK-equivalent
to ``q - 2*pi ≈ -3.28``, which is *also* outside the range, so neither
wrap fits and the solution stays at ``q = 3.0`` (and would be dropped by
:func:`respect_limits` if called next). A joint at ``q = 4.0`` with
limits ``[-pi, pi]`` wraps to ``q - 2*pi ≈ -2.28`` which is in range:
we keep the wrapped value.
Joints with ``limits=None`` are left unchanged (no constraint to wrap
into). Prismatic joints are left unchanged (no rotational periodicity).
Search is over ``k ∈ {-2, -1, 0, +1, +2}`` integer multiples of ``2*pi``;
that covers any joint whose limits span up to ±5*pi (more than enough for
any commercial arm). The smallest-|k| wrap that lands in range wins,
biasing toward the original value. A wrap within the limit band of a limit,
on either side, lands exactly on the limit, as in :func:`respect_limits`.
:param sols: candidate solutions.
:param kb: the same :class:`KinBody` used for the IK call.
:param T_target: the IK target. When given, the band is the solution's
error band (#651), and a solution the band moved has its
``fk_residual`` re-measured at the returned ``q``; a ``2*pi`` wrap
alone keeps it.
:returns: solutions with each q-vector adjusted joint-wise; preserves
input order; returns ``Solution`` instances with the wrapped q
and other fields unchanged.
"""
n_joints = len(kb.joints)
out: list[Solution] = []
for sol in sols:
if len(sol.q) != n_joints:
raise ValueError(f"solution q-length {len(sol.q)} doesn't match kb DOF {n_joints}")
q_new = np.asarray(sol.q, dtype=np.float64).copy()
moved = False
band = _band_of(sol, kb, T_target)
for i, joint in enumerate(kb.joints):
if joint.limits is None or joint.joint_type != "revolute":
continue
lo, hi = joint.limits
q_i = float(q_new[i])
# Smallest |k| first; a fit within the band of a limit lands on it.
for k in (0, 1, -1, 2, -2):
cand = q_i + 2.0 * np.pi * k
v = _onto_limits(cand, lo, hi, band)
if v is not None:
q_new[i] = v
moved = moved or v != cand
break
out.append(_placed(sol, q_new, moved, kb, T_target))
return out
|
Sort solutions by joint-space distance to a reference configuration.
The "wrap-to-pi" distance treats angle differences modulo 2*pi, so
e.g. q=3.0 and q_seed=-3.0 are at distance
|wrap(3.0 - (-3.0))| = |wrap(6.0)| = |6.0 - 2*pi| ≈ 0.28, not 6.0.
This is the right metric for revolute-joint similarity.
Parameters:
| Name |
Type |
Description |
Default |
sols
|
list[Solution]
|
|
required
|
q_seed
|
NDArray[float64]
|
reference joint configuration (length matches the chain's
DOF).
|
required
|
metric
|
str
|
one of "wrap_l2" (default; sum-of-squares of
wrap-to-pi differences) or "wrap_linf" (max wrap-to-pi
difference). wrap_l2 is smooth and prefers configurations that
are uniformly close; wrap_linf is hard-cap and prefers
configurations whose worst-joint deviation is small.
|
'wrap_l2'
|
kb
|
KinBody | None
|
when given, differences are wrapped modulo 2*pi only for
continuous joints; finite revolute and prismatic joints use ordinary
coordinate distance (see :func:_circular_mask). Required for correct
ranking of winding representatives (#562) -- without it, a branch and
its 2*pi lift tie. None wraps every joint (pre-#562 behaviour).
|
None
|
Returns:
| Type |
Description |
list[Solution]
|
solutions sorted by ascending distance to q_seed. Equal
distances are broken deterministically by :func:_rank_key, so the
order depends only on the solutions themselves.
|
Source code in src/ssik/postprocess.py
| def nearest_to_seed(
sols: list[Solution],
q_seed: NDArray[np.float64],
*,
metric: str = "wrap_l2",
kb: KinBody | None = None,
) -> list[Solution]:
"""Sort solutions by joint-space distance to a reference configuration.
The "wrap-to-pi" distance treats angle differences modulo ``2*pi``, so
e.g. ``q=3.0`` and ``q_seed=-3.0`` are at distance
``|wrap(3.0 - (-3.0))| = |wrap(6.0)| = |6.0 - 2*pi| ≈ 0.28``, not 6.0.
This is the right metric for revolute-joint similarity.
:param sols: candidate solutions.
:param q_seed: reference joint configuration (length matches the chain's
DOF).
:param metric: one of ``"wrap_l2"`` (default; sum-of-squares of
wrap-to-pi differences) or ``"wrap_linf"`` (max wrap-to-pi
difference). ``wrap_l2`` is smooth and prefers configurations that
are uniformly close; ``wrap_linf`` is hard-cap and prefers
configurations whose worst-joint deviation is small.
:param kb: when given, differences are wrapped modulo ``2*pi`` only for
continuous joints; finite revolute and prismatic joints use ordinary
coordinate distance (see :func:`_circular_mask`). Required for correct
ranking of winding representatives (#562) -- without it, a branch and
its ``2*pi`` lift tie. ``None`` wraps every joint (pre-#562 behaviour).
:returns: solutions sorted by ascending distance to ``q_seed``. Equal
distances are broken deterministically by :func:`_rank_key`, so the
order depends only on the solutions themselves.
"""
if metric not in ("wrap_l2", "wrap_linf"):
raise ValueError(f"unknown metric {metric!r}; expected 'wrap_l2' or 'wrap_linf'")
seed = np.asarray(q_seed, dtype=np.float64)
circular = _circular_mask(kb, len(seed))
return sorted(sols, key=lambda s: _rank_key(_seed_deltas(s.q, seed, circular), metric, s.q))
|
Keep only solutions within a per-joint deviation of a reference config.
A solution passes when every joint is within tolerance of the seed --
max_i |d_i| <= tolerance, the L-infinity (max-joint-move) bound. This is
the hard guarantee for trajectory tracking ("no joint jumps more than
tolerance"), as opposed to :func:nearest_to_seed, which only ranks. The
result may be empty when no in-tolerance branch exists -- itself the useful
signal that smooth continuation is not possible at this pose. Compose with
:func:nearest_to_seed + :func:take_first to rank and cap the survivors.
Parameters:
| Name |
Type |
Description |
Default |
sols
|
list[Solution]
|
|
required
|
q_seed
|
NDArray[float64]
|
reference joint configuration (length matches the chain's
DOF).
|
required
|
tolerance
|
float
|
maximum allowed per-joint deviation, radians.
|
required
|
kb
|
KinBody | None
|
when given, the bound is measured the way the joint actually
moves -- modulo 2*pi for continuous joints only, ordinary distance
for finite revolute and prismatic joints (#562). This makes the
guarantee honest: a finite joint that must travel 6 radians to reach the
solution is no longer admitted by a 0.5-radian bound just because the
endpoints happen to wrap close. None wraps every joint (pre-#562).
|
None
|
Returns:
| Type |
Description |
list[Solution]
|
the subset of sols within tolerance of q_seed, in
input order.
|
Source code in src/ssik/postprocess.py
| def within_seed_tolerance(
sols: list[Solution],
q_seed: NDArray[np.float64],
tolerance: float,
*,
kb: KinBody | None = None,
) -> list[Solution]:
"""Keep only solutions within a per-joint deviation of a reference config.
A solution passes when *every* joint is within ``tolerance`` of the seed --
``max_i |d_i| <= tolerance``, the L-infinity (max-joint-move) bound. This is
the hard guarantee for trajectory tracking ("no joint jumps more than
``tolerance``"), as opposed to :func:`nearest_to_seed`, which only ranks. The
result may be empty when no in-tolerance branch exists -- itself the useful
signal that smooth continuation is not possible at this pose. Compose with
:func:`nearest_to_seed` + :func:`take_first` to rank and cap the survivors.
:param sols: candidate solutions.
:param q_seed: reference joint configuration (length matches the chain's
DOF).
:param tolerance: maximum allowed per-joint deviation, radians.
:param kb: when given, the bound is measured the way the joint actually
moves -- modulo ``2*pi`` for continuous joints only, ordinary distance
for finite revolute and prismatic joints (#562). This makes the
guarantee honest: a finite joint that must travel 6 radians to reach the
solution is no longer admitted by a 0.5-radian bound just because the
endpoints happen to wrap close. ``None`` wraps every joint (pre-#562).
:returns: the subset of ``sols`` within ``tolerance`` of ``q_seed``, in
input order.
"""
seed = np.asarray(q_seed, dtype=np.float64)
circular = _circular_mask(kb, len(seed))
def within(sol: Solution) -> bool:
return all(abs(d) <= tolerance for d in _seed_deltas(sol.q, seed, circular))
return [s for s in sols if within(s)]
|
Truncate to the first k solutions.
Use after :func:nearest_to_seed (or any other ranking) to keep only
the top-k matches. k <= 0 returns an empty list.
Renamed from max_solutions in v1.0 to avoid name collision with
the max_solutions kwarg on Manipulator.solve / artifact
solve() -- they have different shapes (kwarg is an int passed in;
this function takes (sols, k)).
Parameters:
| Name |
Type |
Description |
Default |
sols
|
list[Solution]
|
candidate solutions, typically already sorted.
|
required
|
k
|
int
|
maximum number of solutions to keep.
|
required
|
Returns:
Source code in src/ssik/postprocess.py
| def take_first(sols: list[Solution], k: int) -> list[Solution]:
"""Truncate to the first ``k`` solutions.
Use after :func:`nearest_to_seed` (or any other ranking) to keep only
the top-``k`` matches. ``k <= 0`` returns an empty list.
Renamed from ``max_solutions`` in v1.0 to avoid name collision with
the ``max_solutions`` kwarg on ``Manipulator.solve`` / artifact
``solve()`` -- they have different shapes (kwarg is an int passed in;
this function takes ``(sols, k)``).
:param sols: candidate solutions, typically already sorted.
:param k: maximum number of solutions to keep.
:returns: ``sols[:max(k, 0)]``.
"""
return list(sols[: max(k, 0)])
|
Rewrap each revolute joint to the q_i + 2*pi*k representative nearest
the seed, staying within the joint's finite limits (#562, step 1).
A wide-limit joint (limits spanning > 2pi, e.g. UR [-2*pi, 2*pi]) admits
several in-limit windings of one geometric branch; a seeded solve should
return the one nearest q_seed rather than the principal value, so
solve(T, q_seed=q_current, max_solutions=1) never commands a gratuitous
2pi turn. Per joint:
- finite limits, value in range: among all
q_i + 2*pi*k in
[lo, hi], choose the one nearest seed_i (ties -> smaller value,
deterministic). Representatives are derived from the limits, so a
boundary principal like 0 with [-2*pi, 2*pi] correctly considers
{-2*pi, 0, 2*pi}. An admissible value never leaves its limits.
- finite limits, value out of range: only reachable via
respect_limits=False, where the caller wants the raw geometric set.
The nearest turn to seed_i, unclamped. Forcing such a value into
limits used to return a representative a full turn from the seed --
exactly the motion this function exists to prevent (shipped in v5.2.0).
- continuous (
limits is None): the nearest turn to seed_i
(k = round((seed_i - q_i) / 2*pi)), no clamp.
- prismatic: unchanged (no rotational periodicity).
The choice is made per value rather than from a caller flag, because the
flag would have to mean "the user wanted limits", which is not what the
pipeline's own respect_limits says by the time the ranking pass runs.
Only the returned coordinate changes; FK is identical. Runs only when a seed
is supplied (the unseeded path pays nothing).
Parameters:
| Name |
Type |
Description |
Default |
continuous_only
|
bool
|
restrict rewrapping to continuous joints. Set when
winding enumeration is active: enumeration already emits every in-limit
representative of a finite joint, so collapsing them onto the
seed-nearest one would destroy the very set being returned. Continuous
joints are never enumerated (infinite family), so they still need the
nearest-turn choice.
|
False
|
T_target
|
NDArray[float64] | None
|
the IK target. When given, the limit band is the
solution's error band (#651), and a solution whose chosen
representative the band moved onto a limit has its fk_residual
re-measured.
|
None
|
Source code in src/ssik/postprocess.py
| def rewrap_to_seed(
sols: list[Solution],
kb: KinBody,
q_seed: NDArray[np.float64],
*,
continuous_only: bool = False,
T_target: NDArray[np.float64] | None = None,
) -> list[Solution]:
"""Rewrap each revolute joint to the ``q_i + 2*pi*k`` representative nearest
the seed, staying within the joint's finite limits (#562, step 1).
A wide-limit joint (limits spanning > 2*pi, e.g. UR ``[-2*pi, 2*pi]``) admits
several in-limit windings of one geometric branch; a seeded solve should
return the one nearest ``q_seed`` rather than the principal value, so
``solve(T, q_seed=q_current, max_solutions=1)`` never commands a gratuitous
2*pi turn. Per joint:
- **finite limits, value in range**: among all ``q_i + 2*pi*k`` in
``[lo, hi]``, choose the one nearest ``seed_i`` (ties -> smaller value,
deterministic). Representatives are derived from the limits, so a
boundary principal like ``0`` with ``[-2*pi, 2*pi]`` correctly considers
``{-2*pi, 0, 2*pi}``. An admissible value never leaves its limits.
- **finite limits, value out of range**: only reachable via
``respect_limits=False``, where the caller wants the raw geometric set.
The nearest turn to ``seed_i``, unclamped. Forcing such a value *into*
limits used to return a representative a full turn from the seed --
exactly the motion this function exists to prevent (shipped in v5.2.0).
- **continuous** (``limits is None``): the nearest turn to ``seed_i``
(``k = round((seed_i - q_i) / 2*pi)``), no clamp.
- **prismatic**: unchanged (no rotational periodicity).
The choice is made per value rather than from a caller flag, because the
flag would have to mean "the user wanted limits", which is not what the
pipeline's own ``respect_limits`` says by the time the ranking pass runs.
Only the returned coordinate changes; FK is identical. Runs only when a seed
is supplied (the unseeded path pays nothing).
:param continuous_only: restrict rewrapping to continuous joints. Set when
winding enumeration is active: enumeration already emits every in-limit
representative of a finite joint, so collapsing them onto the
seed-nearest one would destroy the very set being returned. Continuous
joints are never enumerated (infinite family), so they still need the
nearest-turn choice.
:param T_target: the IK target. When given, the limit band is the
solution's error band (#651), and a solution whose chosen
representative the band moved onto a limit has its ``fk_residual``
re-measured.
"""
seed = np.asarray(q_seed, dtype=np.float64)
out: list[Solution] = []
for sol in sols:
q_new = np.asarray(sol.q, dtype=np.float64).copy()
moved = False
band = _band_of(sol, kb, T_target)
for i, joint in enumerate(kb.joints):
if joint.joint_type != "revolute":
continue
q_i, s_i = float(q_new[i]), float(seed[i])
if joint.limits is None:
q_new[i] = q_i + _TWO_PI * round((s_i - q_i) / _TWO_PI)
continue
lo, hi = joint.limits
if not (lo <= q_i <= hi):
# Out of limits already (respect_limits=False): take the nearest
# turn without dragging it into limits the caller waived.
q_new[i] = q_i + _TWO_PI * round((s_i - q_i) / _TWO_PI)
continue
if continuous_only: # enumeration emits this joint's representatives
continue
cands = _reps(q_i, lo, hi, band)
q_new[i] = v = min(cands, key=lambda c: (abs(c - s_i), c))
moved = moved or _is_move(q_i, v)
out.append(_placed(sol, q_new, moved, kb, T_target))
return out
|
Angle representatives
Every solve() result, on either backend and under every respect_limits
mode, reports each angle in one canonical coordinate. It is chosen before the
limit pass, seed ranking and winding enumeration, so all of them see the same
values:
- A continuous revolute joint (
limits=None) is reported in (-pi, pi].
An angle within round-off (1e-9 rad) of the cut is reported as exactly
+pi. Either side of the cut is the same configuration of a continuous
joint, so compare such joints on the circle.
- A finite revolute joint whose limits admit more than one representative
of
pi ([-pi, pi] to within the 1e-9 limit band, or
the UR family's [-2*pi, 2*pi]) reports an angle at the cut on the
+pi side: the 2*pi shift nearest the in-limit representative of pi
closest to +pi. If that shift lies beyond a limit that sits at +pi, as
on a [-pi, pi] joint, the angle is reported as exactly the limit. An angle
is at the cut when it lies within its solution's
error band of pi (modulo 2*pi), or within 1e-14 rad of
it.
- Every other angle keeps the solver's value, then
wrap_to_limits, winding
enumeration and rewrap_to_seed apply as documented below.
The coordinate moves by a multiple of 2*pi, which is the same configuration.
The only other movements are the 1e-9 snap of a continuous joint and the
clamp of a shifted angle onto a limit at +pi, which moves it by at most its
error band, and never more than 1e-6 rad. For an accurate solution that band
is far below round-off, so an accurate angle near pi stays where the solver
put it. The band is wider where the solver cannot place the angle better, such
as a folded elbow at the cut, a double root known only to a few 1e-7 rad.
There both backends report the angle on the same side of the cut. The
derivation is at ssik.postprocess._CUT_SNAP and ssik.postprocess._BAND_GAIN.
A clamp moves the configuration, so the returned fk_residual is measured
again at the clamped value (see Joint limits).
Joint limits
A limit is inclusive up to the solution's own error. With
respect_limits=True or "wrap", on either backend:
- an angle outside a joint limit by at most the solution's limit band is
at the limit: the solution is kept and that joint is reported as exactly the
limit. The limit band is the larger of round-off (
1e-9 rad) and the
solution's error band;
- an angle inside a limit by at most round-off (
1e-9 rad) is also reported as
exactly the limit;
- any other angle inside its limits keeps its value, and any angle further
outside drops the solution.
This applies to the limit pass, the redundant-7R in-limits resolvers, and every
winding representative, so a branch at a limit has the same lifts on both
backends. A configuration exactly at a hard stop is therefore returned on both
backends, and a seeded solve from it returns its own branch, although the
solvers compute its angle on either side of the limit. Every returned angle
lies within its limits exactly. respect_limits=False leaves the solver's
values as they are.
The 1e-9 floor covers the closed-form families at a regular or moderately
conditioned pose, which land within 1.2e-10 rad of an exact limit, and
general_6r, whose accepted solutions are polished
(Polished general_6r solutions). Near a
singularity or a fold an angle is a multiple root, known only to about the
square root of round-off, and an angle at an exact limit can land up to a few
1e-7 rad past it. The error band covers that. The band is capped at 1e-6
rad, so a clamp never moves a configuration further. A configuration further
than that across a limit is dropped, even when its error estimate is larger.
Such a configuration is typically a different point of a singular family,
which moving one joint cannot bring back without breaking FK. The derivation
is at ssik.postprocess._LIMIT_BAND and ssik.postprocess._BAND_GAIN.
Solution.fk_residual always describes the returned q. The solver measures
it, and when this clamp or an angle-representative
snap or clamp moves a value, it is measured again at the moved value, on both
backends: ||FK(q) - T_target||_F on the arm's chain, the solvers' own metric.
A 2*pi shift (a winding representative, a seed rewrap) is the same
configuration and keeps the solver's value. The re-measurement costs one FK per
moved solution, and solutions nothing moved pay nothing.
On a redundant 7R arm the in-limit part of the self-motion can be a single
point or a sliver, typically where two or more joints are at their limits at
once. There every joint's margin touches zero without changing sign, so
bracketing the sign changes finds no in-limits arc. When the in-limits
resolver finds no arc on any branch, it looks for the point where the
worst-case limit violation along each branch is smallest. On a closed-form
chart (seven_r.srs, seven_r.spherical_shoulder) that point is the
solution. On the polished families it is a starting point, from which the
resolver walks along the arm's true self-motion curve to the minimum. The
point counts as in limits when its worst violation is within its limit band.
Such a point is clamped onto the limits and its fk_residual measured again,
as above. Only a contact whose deepest margin is below 1e-4 rad is accepted
this way. A deeper in-limits stretch with no arc around it is left to the
rescue, as before. Chart.in_limits() returns such a point as a zero-width
arc (t, t). None of this runs unless the resolver's own search comes back
empty. The jointlock.seven_r arms (Rizon 4 and 10, Kassow KR810) have no
in-limits resolver, so this does not apply to them.
Error band
Whether a value is at a joint limit or at the +-pi cut is decided against
that solution's own angular error. For a solution q of the target T, both
backends compute
error band = min(1e-6, 10 * ||FK(q) - T||_F / sigma_min(J(q)))
where J is the spatial Jacobian of the arm's chain at q and sigma_min
its smallest singular value. To first order, an error dq leaves a residual
of at least sigma_min * ||dq||, so the ratio bounds every joint's error.
Across the release pose set (uniform, near-limit and near-singular poses of
every 6R arm, both backends), every actual error up to the 1e-6 cap was at
most 5.9 times that ratio, and at most 2.1 times it for errors below 1e-9.
The factor 10 covers both. An accurate solution at a regular pose has a
residual near 1e-15 and a band far below round-off. A solution near a
singularity has a small sigma_min and a wide band. The 1e-6 cap is a tenth
of the default FK acceptance gate (TolerancePolicy.subproblem_numerical), so
a moved solution stays inside that gate. It is fixed: a tighter
subproblem_numerical does not narrow it.
The band is computed only for a solution with a value outside a limit, or near
the cut, by between the floor and 1e-6 rad, so an ordinary solve does not pay
for it. The standalone filters (respect_limits, wrap_to_limits,
expand_windings, rewrap_to_seed, count_windings) use it when given
T_target, and otherwise use only the round-off floors.
Winding representatives
A revolute joint whose limits span more than one turn (the UR family's
[-2*pi, 2*pi]) has several in-limit
q + 2*pi*k representatives of the same geometric branch. They reach the
same pose but are different admissible configurations, at different distances
and with different motions available from where the robot is now. solve()
returns all of them by default; pass enumerate_windings=False for one
representative per geometric branch.
These are finite-limit lifts, not additional geometric branches, and
Diagnostic.geometric_branches / Diagnostic.winding_representatives report
the two counts separately.
The joints that admit more than one in-limit winding (#562).
A revolute joint with finite limits spanning more than 2*pi (UR-family
[-2*pi, 2*pi]) has several distinct
joint-coordinate representatives of the same geometric branch. Those are
finite-limit lifts, not new IK branches, but they are distinct admissible
configurations with different distances and feasible motions.
Excluded, by construction: prismatic joints (no rotational periodicity),
continuous joints (limits is None -- the lift family is infinite and no
representative is privileged), and finite joints spanning at most 2*pi
(at most one representative, except at an exact boundary).
Returns:
| Type |
Description |
list[tuple[int, float, float]]
|
[(joint_index, lo, hi)], in joint order.
|
Source code in src/ssik/postprocess.py
| def winding_joints(kb: KinBody) -> list[tuple[int, float, float]]:
"""The joints that admit more than one in-limit winding (#562).
A revolute joint with finite limits spanning more than ``2*pi`` (UR-family
``[-2*pi, 2*pi]``) has several distinct
joint-coordinate representatives of the *same* geometric branch. Those are
finite-limit lifts, not new IK branches, but they are distinct admissible
configurations with different distances and feasible motions.
Excluded, by construction: prismatic joints (no rotational periodicity),
continuous joints (``limits is None`` -- the lift family is infinite and no
representative is privileged), and finite joints spanning at most ``2*pi``
(at most one representative, except at an exact boundary).
:returns: ``[(joint_index, lo, hi)]``, in joint order.
"""
out: list[tuple[int, float, float]] = []
for i, joint in enumerate(kb.joints):
if joint.joint_type != "revolute" or joint.limits is None:
continue
lo, hi = joint.limits
if hi - lo > _TWO_PI + _SPAN_TOL:
out.append((i, float(lo), float(hi)))
return out
|
Expand each solution into every in-limit winding representative (#562).
Takes the Cartesian product across :func:winding_joints, since each
combination is a distinct point of the bounded joint-coordinate domain. FK
is identical for every representative; only the coordinates differ. Arms
with no wide-limit joint are returned unchanged (26 of the 72 shipped arms),
so they pay nothing.
Output order is branch-major, and within a branch the ascending-value
Cartesian product -- the stable order the ranking and truncation rules are
defined against.
Parameters:
| Name |
Type |
Description |
Default |
limit
|
int | None
|
stop once this many configurations exist. Only sound when the
caller keeps a prefix of the expansion (i.e. unseeded truncation);
a ranked truncation must not pass it.
|
None
|
T_target
|
NDArray[float64] | None
|
the IK target. When given, the limit band is the
solution's error band (#651), and a representative the band moved onto
a limit has its fk_residual re-measured.
|
None
|
Source code in src/ssik/postprocess.py
| def expand_windings(
sols: list[Solution],
kb: KinBody,
*,
limit: int | None = None,
T_target: NDArray[np.float64] | None = None,
) -> list[Solution]:
"""Expand each solution into every in-limit winding representative (#562).
Takes the Cartesian product across :func:`winding_joints`, since each
combination is a distinct point of the bounded joint-coordinate domain. FK
is identical for every representative; only the coordinates differ. Arms
with no wide-limit joint are returned unchanged (26 of the 72 shipped arms),
so they pay nothing.
Output order is branch-major, and within a branch the ascending-value
Cartesian product -- the stable order the ranking and truncation rules are
defined against.
:param limit: stop once this many configurations exist. Only sound when the
caller keeps a *prefix* of the expansion (i.e. unseeded truncation);
a ranked truncation must not pass it.
:param T_target: the IK target. When given, the limit band is the
solution's error band (#651), and a representative the band moved onto
a limit has its ``fk_residual`` re-measured.
"""
wind = winding_joints(kb)
if not wind:
return list(sols)
idxs = [i for i, _, _ in wind]
out: list[Solution] = []
for sol in sols:
q = np.asarray(sol.q, dtype=np.float64)
band = _band_of(sol, kb, T_target)
opts = [_reps(float(q[i]), lo, hi, band) for i, lo, hi in wind]
# Per winding joint, the representatives the limit band moved (#645):
# nearly always none, and then no lift needs checking.
moves = [
{v for v in reps if _is_move(float(q[i]), v)}
for (i, _, _), reps in zip(wind, opts, strict=True)
]
any_move = any(moves)
res, ref = sol.fk_residual, sol.refinement_used
for combo in itertools.product(*opts):
q_new = q.copy()
for t, v in enumerate(combo):
q_new[idxs[t]] = v
if any_move and any(v in moves[t] for t, v in enumerate(combo)):
out.append(_placed(sol, q_new, True, kb, T_target))
else:
out.append(Solution(q_new, res, ref))
if limit is not None and len(out) >= limit:
return out
return out
|
How many configurations :func:expand_windings would produce, without
building them. Used for diagnostics so a truncated or pruned solve can still
report the true size of the complete in-limit set. Pass the same
T_target as to :func:expand_windings: it sets the limit band.
Source code in src/ssik/postprocess.py
| def count_windings(
sols: list[Solution], kb: KinBody, *, T_target: NDArray[np.float64] | None = None
) -> int:
"""How many configurations :func:`expand_windings` would produce, without
building them. Used for diagnostics so a truncated or pruned solve can still
report the true size of the complete in-limit set. Pass the same
``T_target`` as to :func:`expand_windings`: it sets the limit band.
"""
wind = winding_joints(kb)
if not wind:
return len(sols)
total = 0
for sol in sols:
n = 1
band = _band_of(sol, kb, T_target)
for i, lo, hi in wind:
n *= len(_reps(float(sol.q[i]), lo, hi, band))
total += n
return total
|
Every wheel ships the header-only C++ solvers (ssik_cpp/) and a CMake package
for them. ssik.get_include() and ssik.get_cmake_dir() locate them.
ssik.cpp.solver_data(arm) returns the input of ssik::Solver, the one C++
entry point for every family in ssik.cpp.NATIVE_FAMILIES; what it returns is
the contract in The native backend, and its arrays are
specified in Solver data, schema 1.
ssik.cpp.joint_data(arm) returns the arrays a C++ caller builds
ssik::JointConsts<N> and ssik::JointLimits<N> from, for
three_parallel_artifact_solve. The consumer guide is
cpp/README.md;
the C++ names covered by semver are listed in the semver policy.
The header-only C++ solvers that ship with ssik, for native consumers.
Every wheel carries the ssik_cpp headers (cpp/include/ssik_cpp in the
repository) and a relocatable CMake package for them, so a C++ project, or a
Python extension built with scikit-build-core, can call the family solvers
directly with no Python in the loop::
cmake -DCMAKE_PREFIX_PATH="$(python -c 'import ssik; print(ssik.get_cmake_dir())')" ...
find_package(ssik_cpp CONFIG REQUIRED)
target_link_libraries(my_target PRIVATE ssik::ssik_cpp)
Eigen stays the consumer's responsibility: the package find_dependencys
Eigen3, and ssik's own builds use the release pinned in
scripts/fetch_eigen.py (3.4.0). The per-arm generated headers
(cpp/gen/<arm>_ik.hpp) are not in the wheel. Build the solver input at
runtime instead: :func:solver_data for ssik::Solver, which serves every
native family, or :func:joint_data for three_parallel_artifact_solve.
From a source checkout, scripts/cpp_emit.py emits a per-arm header.
SOLVER_DATA_SCHEMA
module-attribute
SOLVER_DATA_SCHEMA: int = _SOLVER_DATA_SCHEMA
NATIVE_FAMILIES
module-attribute
NATIVE_FAMILIES: frozenset[str] = _NATIVE_SOLVERS
SolverData
dataclass
The complete input of one arm's native solver, ssik::Solver.
The same record as ssik::SolverData (ssik_cpp/solver_data.hpp): a
family name, a schema number and a flat map of named arrays. Every array
is read-only, C-contiguous and either float64 or int32; a scalar has shape
(). The names, shapes, dtypes and meaning of each family's arrays are
specified in docs/specs/solver_data.md; semver covers them through
schema.
family: the solver family, :attr:ssik.Manipulator.solver_name.schema: :data:SOLVER_DATA_SCHEMA of the ssik that produced it.dof: the number of joints, 6 or 7.arrays: {name: ndarray}, read-only.
A pybind11 extension converts it with
ssik::pybind11_interop::solver_data_from_python
(ssik_cpp/pybind11/solver_data.hpp). It maps one-to-one onto an
.npz: np.savez(path, schema=d.schema, family=d.family, **d.arrays).
Source code in src/ssik/cpp/__init__.py
| @dataclass(frozen=True)
class SolverData:
"""The complete input of one arm's native solver, ``ssik::Solver``.
The same record as ``ssik::SolverData`` (``ssik_cpp/solver_data.hpp``): a
family name, a schema number and a flat map of named arrays. Every array
is read-only, C-contiguous and either float64 or int32; a scalar has shape
``()``. The names, shapes, dtypes and meaning of each family's arrays are
specified in ``docs/specs/solver_data.md``; semver covers them through
``schema``.
- ``family``: the solver family, :attr:`ssik.Manipulator.solver_name`.
- ``schema``: :data:`SOLVER_DATA_SCHEMA` of the ssik that produced it.
- ``dof``: the number of joints, 6 or 7.
- ``arrays``: ``{name: ndarray}``, read-only.
A pybind11 extension converts it with
``ssik::pybind11_interop::solver_data_from_python``
(``ssik_cpp/pybind11/solver_data.hpp``). It maps one-to-one onto an
``.npz``: ``np.savez(path, schema=d.schema, family=d.family, **d.arrays)``.
"""
family: str
schema: int
dof: int
arrays: Mapping[str, NDArray[Any]]
|
UnsupportedFamilyError
Bases: ValueError
:func:solver_data cannot describe this arm for ssik::Solver.
Raised when the arm's solver family has no native solver (it is not in
:data:NATIVE_FAMILIES), for a jointlock.seven_r arm built at runtime
(only the prebuilt jointlock arms ship the data its native solver needs),
and when a family's native geometry check rejects an arm the dispatcher put
in that family. The message says which. family is the arm's solver
family.
Source code in src/ssik/cpp/__init__.py
| class UnsupportedFamilyError(ValueError):
""":func:`solver_data` cannot describe this arm for ``ssik::Solver``.
Raised when the arm's solver family has no native solver (it is not in
:data:`NATIVE_FAMILIES`), for a ``jointlock.seven_r`` arm built at runtime
(only the prebuilt jointlock arms ship the data its native solver needs),
and when a family's native geometry check rejects an arm the dispatcher put
in that family. The message says which. ``family`` is the arm's solver
family.
"""
def __init__(self, family: str, message: str) -> None:
super().__init__(message)
self.family = family
|
JointData
dataclass
The arrays ssik::JointConsts<N> and ssik::JointLimits<N> hold.
Row i describes joint i from the base. Forward kinematics is
T = prod_i t_left[i] @ Joint(axis[i], q[i]) @ t_right[i], where
Joint rotates about axis (revolute) or translates along it
(prismatic); ssik::fk in ssik_cpp/fk.hpp evaluates exactly that.
axis (N, 3), t_left (N, 4, 4), t_right (N, 4, 4): the
JointConsts fields of the same names.joint_type (N,): JointConsts::type, as the ssik::JointType
value: 0 Revolute, 1 Prismatic.lo, hi (N,), present (N,) bool: the JointLimits fields.
A joint without limits has present false and lo = hi = 0.solver: the solver family these frames are prepared for
(:attr:ssik.Manipulator.solver_name). ikgeo.three_parallel data
goes to three_parallel_artifact_solve.
Source code in src/ssik/cpp/__init__.py
| @dataclass(frozen=True)
class JointData:
"""The arrays ``ssik::JointConsts<N>`` and ``ssik::JointLimits<N>`` hold.
Row ``i`` describes joint ``i`` from the base. Forward kinematics is
``T = prod_i t_left[i] @ Joint(axis[i], q[i]) @ t_right[i]``, where
``Joint`` rotates about ``axis`` (revolute) or translates along it
(prismatic); ``ssik::fk`` in ``ssik_cpp/fk.hpp`` evaluates exactly that.
- ``axis`` (N, 3), ``t_left`` (N, 4, 4), ``t_right`` (N, 4, 4): the
``JointConsts`` fields of the same names.
- ``joint_type`` (N,): ``JointConsts::type``, as the ``ssik::JointType``
value: 0 ``Revolute``, 1 ``Prismatic``.
- ``lo``, ``hi`` (N,), ``present`` (N,) bool: the ``JointLimits`` fields.
A joint without limits has ``present`` false and ``lo = hi = 0``.
- ``solver``: the solver family these frames are prepared for
(:attr:`ssik.Manipulator.solver_name`). ``ikgeo.three_parallel`` data
goes to ``three_parallel_artifact_solve``.
"""
solver: str
axis: NDArray[np.float64]
t_left: NDArray[np.float64]
t_right: NDArray[np.float64]
joint_type: NDArray[np.int32]
lo: NDArray[np.float64]
hi: NDArray[np.float64]
present: NDArray[np.bool_]
@property
def dof(self) -> int:
"""Number of joints, the ``N`` of ``JointConsts<N>``."""
return len(self.joint_type)
|
dof
property
Number of joints, the N of JointConsts<N>.
get_include
Absolute path of the directory that contains ssik_cpp/.
Add it to the include path to #include "ssik_cpp/solvers/three_parallel.hpp"
and the other headers. In an installed wheel this is ssik/cpp/include;
in an editable or development install it is the checkout's cpp/include.
Raises:
| Type |
Description |
FileNotFoundError
|
neither location holds the headers.
|
Source code in src/ssik/cpp/__init__.py
| def get_include() -> str:
"""Absolute path of the directory that contains ``ssik_cpp/``.
Add it to the include path to ``#include "ssik_cpp/solvers/three_parallel.hpp"``
and the other headers. In an installed wheel this is ``ssik/cpp/include``;
in an editable or development install it is the checkout's ``cpp/include``.
:raises FileNotFoundError: neither location holds the headers.
"""
for cand in _include_candidates():
if (cand / _PROBE).is_file():
return str(cand)
looked = ", ".join(str(c) for c in _include_candidates())
raise FileNotFoundError(
f"the ssik_cpp headers are not in this ssik install (looked in {looked}). "
f"They ship in every ssik wheel; reinstall ssik from a wheel or its sdist."
)
|
get_cmake_dir
Absolute path of the directory that contains ssik_cppConfig.cmake.
Pass it as CMAKE_PREFIX_PATH (or ssik_cpp_DIR); then
find_package(ssik_cpp CONFIG REQUIRED) defines the ssik::ssik_cpp
INTERFACE target, the same target cmake --install of cpp/ exports.
Its package version is this ssik release, compatible within one major
version.
Raises:
| Type |
Description |
FileNotFoundError
|
this install has no CMake package. Only wheels
carry it; in an editable or development install, use
:func:get_include or install cpp/ with cmake --install.
|
Source code in src/ssik/cpp/__init__.py
| def get_cmake_dir() -> str:
"""Absolute path of the directory that contains ``ssik_cppConfig.cmake``.
Pass it as ``CMAKE_PREFIX_PATH`` (or ``ssik_cpp_DIR``); then
``find_package(ssik_cpp CONFIG REQUIRED)`` defines the ``ssik::ssik_cpp``
INTERFACE target, the same target ``cmake --install`` of ``cpp/`` exports.
Its package version is this ssik release, compatible within one major
version.
:raises FileNotFoundError: this install has no CMake package. Only wheels
carry it; in an editable or development install, use
:func:`get_include` or install ``cpp/`` with ``cmake --install``.
"""
cmake_dir = _HERE / "cmake"
if (cmake_dir / "ssik_cppConfig.cmake").is_file():
get_include() # the config is useless without the headers beside it
return str(cmake_dir)
raise FileNotFoundError(
f"no ssik_cpp CMake package in this ssik install ({cmake_dir}). It ships in "
f"ssik wheels only. From a source checkout, use get_include() or "
f"`cmake --install` of cpp/ (see cpp/README.md)."
)
|
solver_data
solver_data(arm: Manipulator) -> SolverData
The input ssik::Solver (ssik_cpp/solve.hpp) needs for arm.
It is built by the same code that feeds ssik's own native backend, so for
the options ssik::ArtifactParams can express, ssik::Solver(d).solve<N>
returns the list, in the same order, that the native backend returns for
this data (docs/api.md, "The native backend"). For an arm from
:meth:ssik.Manipulator.from_prebuilt that is arm.solve(T, native=True).
Works for prebuilt arms and for arms built at runtime (from_urdf,
from_mjcf, from_axes), and needs no native extension. A runtime arm
on ikgeo.general_6r needs its numeric coefficient tensor: about 28 s of
symbolic work the first time on a machine (the JACO 2), then milliseconds
from the per-arm preparation cache (docs/api.md, "Per-arm preparation
cache"); :meth:ssik.Manipulator.prepare pays it ahead of time. Prebuilt
arms ship it.
import ssik
d = ssik.cpp.solver_data(ssik.Manipulator.from_prebuilt("ur5"))
d.family, d.schema, d.dof, d.arrays["t_left"].shape
('ikgeo.three_parallel', 1, 6, (6, 4, 4))
Raises:
| Type |
Description |
TypeError
|
arm is not a :class:ssik.Manipulator.
|
UnsupportedFamilyError
|
the arm's family has no native solver, the
arm is a jointlock.seven_r arm built at runtime, or the family's
native geometry check rejects the arm.
|
Source code in src/ssik/cpp/__init__.py
| def solver_data(arm: Manipulator) -> SolverData:
"""The input ``ssik::Solver`` (``ssik_cpp/solve.hpp``) needs for ``arm``.
It is built by the same code that feeds ssik's own native backend, so for
the options ``ssik::ArtifactParams`` can express, ``ssik::Solver(d).solve<N>``
returns the list, in the same order, that the native backend returns for
this data (``docs/api.md``, "The native backend"). For an arm from
:meth:`ssik.Manipulator.from_prebuilt` that is ``arm.solve(T, native=True)``.
Works for prebuilt arms and for arms built at runtime (``from_urdf``,
``from_mjcf``, ``from_axes``), and needs no native extension. A runtime arm
on ``ikgeo.general_6r`` needs its numeric coefficient tensor: about 28 s of
symbolic work the first time on a machine (the JACO 2), then milliseconds
from the per-arm preparation cache (``docs/api.md``, "Per-arm preparation
cache"); :meth:`ssik.Manipulator.prepare` pays it ahead of time. Prebuilt
arms ship it.
>>> import ssik
>>> d = ssik.cpp.solver_data(ssik.Manipulator.from_prebuilt("ur5"))
>>> d.family, d.schema, d.dof, d.arrays["t_left"].shape
('ikgeo.three_parallel', 1, 6, (6, 4, 4))
:raises TypeError: ``arm`` is not a :class:`ssik.Manipulator`.
:raises UnsupportedFamilyError: the arm's family has no native solver, the
arm is a ``jointlock.seven_r`` arm built at runtime, or the family's
native geometry check rejects the arm.
"""
from ssik import _native
from ssik.manipulator import Manipulator
if not isinstance(arm, Manipulator):
raise TypeError(f"solver_data() takes an ssik.Manipulator, got {type(arm).__name__}")
family = arm.solver_name
if family not in NATIVE_FAMILIES:
raise UnsupportedFamilyError(
family,
f"solver family {family!r} has no native solver; ssik::Solver serves "
f"{', '.join(sorted(NATIVE_FAMILIES))}",
)
kb = arm.kinbody
prebuilt = arm._prebuilt
geometry: Any = None
if family == "jointlock.seven_r":
# The artifact's own baked sweep, the data its native=True path uses.
geometry = None if prebuilt is None else prebuilt._jointlock_native_geometry()
if geometry is None:
raise UnsupportedFamilyError(
family,
"a jointlock.seven_r arm built at runtime has no native solver data: its "
"inner kernel (Raghavan-Roth or Husty-Pfurner) and its 16 lock samples are "
"fixed when an arm is onboarded, so only the prebuilt jointlock arms "
"(Manipulator.from_prebuilt) ship them",
)
elif family == "ikgeo.general_6r":
# A prebuilt arm's shipped tensor, the one its native=True path uses.
if prebuilt is not None:
geometry = prebuilt._rr_native_geometry()
if geometry is None:
geometry = _native.rr_native_geometry(kb)
arrays = _native._solver_arrays(kb, family, geometry)
if arrays is None:
raise UnsupportedFamilyError(
family,
f"the dispatcher put this arm in {family!r}, but that family's native "
f"geometry check rejects it, so there is no native solver data for it",
)
frozen: dict[str, NDArray[Any]] = {}
for name, value in arrays.items():
a = np.array(value, copy=True, order="C")
a.flags.writeable = False
frozen[name] = a
return SolverData(
family=family,
schema=SOLVER_DATA_SCHEMA,
dof=len(kb.joints),
arrays=MappingProxyType(frozen),
)
|
joint_data
joint_data(arm: Manipulator) -> JointData
The joint data to build JointConsts<N> / JointLimits<N> from, for arm.
These are the values the native backend passes to the C++ solvers and the
values scripts/cpp_emit.py bakes into cpp/gen/<arm>_ik.hpp. For
ikgeo.spherical_two_parallel the frames are in the canonical wrist
gauge that solver requires (forward kinematics is unchanged); other
families use the arm's own frames. Limits are always the arm's own.
import ssik
d = ssik.cpp.joint_data(ssik.Manipulator.from_prebuilt("ur5"))
d.solver, d.dof, d.t_left.shape
('ikgeo.three_parallel', 6, (6, 4, 4))
Source code in src/ssik/cpp/__init__.py
| def joint_data(arm: Manipulator) -> JointData:
"""The joint data to build ``JointConsts<N>`` / ``JointLimits<N>`` from, for ``arm``.
These are the values the native backend passes to the C++ solvers and the
values ``scripts/cpp_emit.py`` bakes into ``cpp/gen/<arm>_ik.hpp``. For
``ikgeo.spherical_two_parallel`` the frames are in the canonical wrist
gauge that solver requires (forward kinematics is unchanged); other
families use the arm's own frames. Limits are always the arm's own.
>>> import ssik
>>> d = ssik.cpp.joint_data(ssik.Manipulator.from_prebuilt("ur5"))
>>> d.solver, d.dof, d.t_left.shape
('ikgeo.three_parallel', 6, (6, 4, 4))
"""
return _joint_data(arm.kinbody, arm.solver_name)
|
CLI: ssik build
ssik build <urdf> --base <link> --ee <link> [--out <path>]
ssik classify <urdf> --base <link> --ee <link>
ssik add-arm <urdf> --base <link> --ee <link> --name <arm>
Full help: ssik <command> --help. See Setting up your robot for the full URDF-to-artifact workflow.