From 81c18e2de58b3e00275207578cac945c29bf486f Mon Sep 17 00:00:00 2001 From: DVIDIA Date: Mon, 9 Nov 2026 12:35:29 -0500 Subject: [PATCH] Add isolated encoder-only PiPER kinematics and static validation --- .gitattributes | 2 + .github/workflows/ci.yml | 2 +- .../piper-encoder-kinematics-v01/README.md | 24 ++ .../portable-timing.json | 42 ++++ .../prototype-timing.json | 41 ++++ docs/piper-encoder-kinematics-v01.md | 108 +++++++++ scripts/benchmark-piper-encoder-kinematics.py | 63 ++++++ .../piper_encoder_kinematics.py | 171 ++++++++++++++ tests/test_piper_encoder_kinematics.py | 210 ++++++++++++++++++ 9 files changed, 662 insertions(+), 1 deletion(-) create mode 100644 benchmarks/piper-encoder-kinematics-v01/README.md create mode 100644 benchmarks/piper-encoder-kinematics-v01/portable-timing.json create mode 100644 benchmarks/piper-encoder-kinematics-v01/prototype-timing.json create mode 100644 docs/piper-encoder-kinematics-v01.md create mode 100644 scripts/benchmark-piper-encoder-kinematics.py create mode 100644 src/dvidia_training/piper_encoder_kinematics.py create mode 100644 tests/test_piper_encoder_kinematics.py diff --git a/.gitattributes b/.gitattributes index aae0277..622b09c 100644 --- a/.gitattributes +++ b/.gitattributes @@ -17,6 +17,8 @@ src/dvidia_training/robot_profiles.lock.json text eol=lf src/dvidia_training/camera_action_cache.py text eol=lf src/dvidia_training/camera_action_reference_export.py text eol=lf src/dvidia_training/camera_action_reference_model.py text eol=lf +src/dvidia_training/piper_encoder_kinematics.py text eol=lf +scripts/benchmark-piper-encoder-kinematics.py text eol=lf src/dvidia_training/camera_action_study.py text eol=lf src/dvidia_training/piper_rgbd_tracker.py text eol=lf src/dvidia_training/action_demonstration.py text eol=lf diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 356fccb..43c96c7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -152,7 +152,7 @@ jobs: - name: Test profile contracts against pinned native assets env: DVIDIA_ROBOT_PROFILE_ASSETS: runs/robot-profile-assets-ci - run: python -m unittest tests.test_robot_profiles tests.test_piper_task tests.test_piper_task_benchmark tests.test_piper_grip_guard tests.test_piper_grip_benchmark tests.test_piper_contact_memory tests.test_piper_contact_benchmark tests.test_piper_contact_recovery tests.test_piper_recovery_benchmark -v + run: python -m unittest tests.test_robot_profiles tests.test_piper_task tests.test_piper_task_benchmark tests.test_piper_grip_guard tests.test_piper_grip_benchmark tests.test_piper_contact_memory tests.test_piper_contact_benchmark tests.test_piper_contact_recovery tests.test_piper_recovery_benchmark tests.test_piper_encoder_kinematics -v - name: Test reusable shoe assets and scene previews env: DVIDIA_ASSET_PREVIEW_TEST_ASSETS: runs/robot-profile-assets-ci diff --git a/benchmarks/piper-encoder-kinematics-v01/README.md b/benchmarks/piper-encoder-kinematics-v01/README.md new file mode 100644 index 0000000..c7c38f7 --- /dev/null +++ b/benchmarks/piper-encoder-kinematics-v01/README.md @@ -0,0 +1,24 @@ +# PiPER encoder geometry: static CPU timing + +This component translates measured joint angles and a desired tool pose into an authored bounded geometric update. It does not learn, simulate a task or execute an actuator. See [the API and frame contract](../../docs/piper-encoder-kinematics-v01.md). + +Two executions on November 9, 2026 are retained: the initial internal prototype probe and the subsequent portable-script verification. The module stayed unchanged; no parameters were tuned from timings. Both used 64 seeded configurations, 64 warmups per API and 1,024 timed calls per API. Targets were prepared outside the timer. + +| Probe | FK median / p95 (ms) | Joint-target calculation median / p95 (ms) | Initialization (ms) | +|---|---:|---:|---:| +| Internal prototype | 0.007875 / 0.008292 | 0.030750 / 0.032410 | 220.360 | +| Portable script | 0.007750 / 0.008208 | 0.030500 / 0.032410 | 221.497 | + +[Prototype receipt](prototype-timing.json) and [portable receipt](portable-timing.json) bind source and probe hashes. Only the portable probe is distributed here. Its whole-process peak RSS was 564,772,864 bytes, including Python, NumPy, asset loading and native compilation; this is not a minimum hardware requirement. The shared host was macOS 26.5.2 arm64, Python 3.12.9, NumPy 2.5.3, MuJoCo 3.15.0, with BLAS thread settings of 1. + +These are static wall-clock measurements, not an observation-to-actuator loop, real-time guarantee, collision-safety result or placement benchmark. Native time remained zero; dynamics entry points were patched to refuse calls. All learned placement results and qualification statuses remain unchanged. + +Run from the source checkout, using an existing pinned asset directory and a fresh receipt path: + +```sh +OPENBLAS_NUM_THREADS=1 OMP_NUM_THREADS=1 MKL_NUM_THREADS=1 VECLIB_MAXIMUM_THREADS=1 \ + PYTHONPATH=src python scripts/benchmark-piper-encoder-kinematics.py \ + --assets ./robot-assets --output ./encoder-kinematics-timing.json +``` + +The script accepts no model or episode input and needs no renderer or network. It pins the measured source module and refuses an existing output. Platform peak RSS is reported only where its units are known (Darwin/Linux); other platforms report null. Windows and Linux execution have not been checked in this local measurement. diff --git a/benchmarks/piper-encoder-kinematics-v01/portable-timing.json b/benchmarks/piper-encoder-kinematics-v01/portable-timing.json new file mode 100644 index 0000000..4826450 --- /dev/null +++ b/benchmarks/piper-encoder-kinematics-v01/portable-timing.json @@ -0,0 +1,42 @@ +{ + "api_wall_ms": { + "fk": { + "max": 0.0275, + "median": 0.00775, + "p95": 0.008208 + }, + "pose_action": { + "max": 0.071583, + "median": 0.0305, + "p95": 0.03240985 + } + }, + "configurations": 64, + "constructor_wall_ms_including_asset_validation_and_compile": 221.496958, + "host": { + "declared_blas_threads": { + "MKL_NUM_THREADS": "1", + "OMP_NUM_THREADS": "1", + "OPENBLAS_NUM_THREADS": "1", + "VECLIB_MAXIMUM_THREADS": "1" + }, + "logical_cpus": 10, + "machine": "arm64", + "macos": "26.5.2", + "mujoco": "3.15.0", + "numpy": "2.5.3", + "platform": "darwin", + "python": "3.12.9", + "whole_process_peak_rss_bytes": 564772864 + }, + "native_time_seconds": 0.0, + "physical_robot_ready": false, + "probe_sha256": "ffe49ba49802521f32e303b91bb268b5c4a8303dbdbf247308f45284e9de9b17", + "schema": "dvidia.piper-encoder-kinematics-static-timing/v1", + "scope": "Shared-host static CPU API timing only. Targets were prepared before timing. No dynamics, rendering, learned inference, training, IO loop or hardware commands; no real-time deadline claim.", + "seed": 20261109, + "source_sha256": "6606bb8aad04e5abb1e9b6d1a8ec628a4114acd167862167184fd4eef1d21c34", + "task_qualified": false, + "timed_calls_per_api": 1024, + "warmup_calls_per_api": 64 +} diff --git a/benchmarks/piper-encoder-kinematics-v01/prototype-timing.json b/benchmarks/piper-encoder-kinematics-v01/prototype-timing.json new file mode 100644 index 0000000..0f52548 --- /dev/null +++ b/benchmarks/piper-encoder-kinematics-v01/prototype-timing.json @@ -0,0 +1,41 @@ +{ + "api_wall_ms": { + "fk": { + "max": 0.028, + "median": 0.007875, + "p95": 0.008292 + }, + "pose_action": { + "max": 0.091959, + "median": 0.03075, + "p95": 0.03240985 + } + }, + "configurations": 64, + "constructor_wall_ms_including_asset_validation_and_compile": 220.359625, + "host": { + "declared_blas_threads": { + "MKL_NUM_THREADS": "1", + "OMP_NUM_THREADS": "1", + "OPENBLAS_NUM_THREADS": "1", + "VECLIB_MAXIMUM_THREADS": "1" + }, + "logical_cpus": 10, + "machine": "arm64", + "macos": "26.5.2", + "mujoco": "3.15.0", + "numpy": "2.5.3", + "python": "3.12.9", + "whole_process_peak_rss_bytes": 561676288 + }, + "native_time_seconds": 0.0, + "physical_robot_ready": false, + "probe_sha256": "5646f69ce2b6fb0616a615c8232af196fdabfc070e062cce6ef0c105d8f64431", + "schema": "dvidia.piper-encoder-kinematics-static-timing/v1", + "scope": "Shared-host static CPU API timing only. Targets were prepared before timing. No dynamics, rendering, learned inference, training, IO loop or hardware commands; no real-time deadline claim.", + "seed": 20261109, + "source_sha256": "6606bb8aad04e5abb1e9b6d1a8ec628a4114acd167862167184fd4eef1d21c34", + "task_qualified": false, + "timed_calls_per_api": 1024, + "warmup_calls_per_api": 64 +} diff --git a/docs/piper-encoder-kinematics-v01.md b/docs/piper-encoder-kinematics-v01.md new file mode 100644 index 0000000..b49a0e0 --- /dev/null +++ b/docs/piper-encoder-kinematics-v01.md @@ -0,0 +1,108 @@ +# PiPER encoder kinematics v0.1 + +`PiperEncoderKinematics` converts six measured joint angles into a tool pose and +one authored differential-IK joint-target update. It owns a private robot-only +model and accepts no live task environment, object state or contact observations. +This is a geometry component for future learning integration, not a learned skill. +It is not connected to the learner, installer or physical robot control. + +## Local use + +Use Python 3.12 or later with the package's `arm` extra (NumPy 2.5.3 and MuJoCo +3.15.0). From a checkout containing this module, install with +`python -m pip install '.[arm]'`. Supply the existing pinned PiPER assets; if you +need to download them, `dvidia-robot-sim fetch piper --output ./robot-assets` +fetches the locked upstream allowlist into a fresh directory. The kinematics +constructor itself is offline and verifies those bytes. + +```python +from dvidia_training.piper_encoder_kinematics import PiperEncoderKinematics + +robot = PiperEncoderKinematics("./robot-assets") +measured_q = [0.0, 1.57, -1.3485, 0.0, 0.0, 0.0] # example encoders, radians +pose = robot.fk(measured_q) +desired_position = pose["tcp_position_world_m"].copy() +desired_position[0] += 0.005 +target = robot.pose_action( + measured_q, + desired_position, + pose["tcp_quaternion_world_wxyz"], + 0.035, +) +print(target) # six joint targets and one single-finger target; nothing executed +``` + +The last argument is **one finger's slide target in metres**, from 0 closed to +0.035 open. It is not the full jaw gap. `jacobian(measured_q)` returns a detached +6×6 spatial Jacobian in world coordinates: three linear rows followed by three +angular rows. No renderer or graphics context is needed. + +## Points, frames and limits + +The authored TCP is link6-local `[0, 0, 0.12003]` metres with identity local +orientation. `fk` names its world-frame position and wxyz quaternion explicitly. +It also returns `link6_position_base_m` and `link6_quaternion_base_wxyz`: the +link6 **origin** in base coordinates. These are different points. The existing +camera-action encoder feature describes that link6 origin; this module does not +change it. For the pinned fixed base (identity world transform), TCP position is +`link6_position_base_m + R(link6_quaternion_base_wxyz) @ [0, 0, 0.12003]`. +Changing a real mounting transform or fingertip calibration requires a separate +contract. Neither pose is a hardware-calibrated fingertip measurement. + +Measured angles must be finite six-vectors within the native joint ranges, +including their endpoints. Even a small measured overshoot is refused rather +than clamped. This is separate from the **issued-target** interior margin of +0.02 rad. The final joint-target change stays within 0.06 rad of accepted +measurements, including native-limit endpoints. Quaternions must already be +normalized wxyz; malformed shapes, booleans, nonfinite values and invalid jaw +targets are refused. + +The update matches the existing `PiperTaskEnv.pose_action`: translation error +limited to 0.015 m, rotation error to 0.05 rad, angular weight 0.25, damping +0.012, and a maximum 0.06 rad joint update before the native-limit margin. +Measured joint angles are **not previous applied servo references**. The existing +rate limiter must still run downstream; this API does not execute it. + +This local geometric update does not check reachability, collision avoidance, +contact, grip, joint velocity, actuator strength or task completion. Targets are +not authorization to command hardware. One instance per worker is required +because private scratch data is reused. + +## Static verification + +The focused test suite passed 10 tests using the existing local MuJoCo 3.15.0 +environment. It compiled static robot/full-task models, without settling, +time integration, rendering, training or task rollouts. Python dynamics entry +points were patched to fail if called. + +- Independent XML transform-chain FK and the 0.12003 m tool offset matched at + 17 fixed/seeded configurations, including native joint-limit endpoints. +- World-frame linear/angular Jacobians matched central finite differences at + 15 interior configurations, across all six joints. +- 34 static pose-target comparisons matched the unchanged original authored + controller, using full-task snapshots with varied object/finger state only + as an independent test oracle. No such state enters the new API. +- Quaternion sign equivalence, zero-error targets, bounds/refusal behavior, + copied results and A→B→A statelessness passed. + +Run these checks with existing pinned assets: + +```sh +DVIDIA_ROBOT_PROFILE_ASSETS=./robot-assets PYTHONPATH=src \ + python -m unittest tests.test_piper_encoder_kinematics -v +``` + +Without the asset variable, eight static geometry tests are explicitly skipped; +only the two interface tests run. Passing these checks establishes geometric +agreement, not improved placement success or physical qualification. The asset +lock is `4b2ee2ca675aceb2f826983cb3082a3e08e9aa0ff419b23078abcff08ebcc6fe`; +the pinned PiPER simulation assets retain their MIT license. + +## Static cost + +A bounded [CPU timing probe](../benchmarks/piper-encoder-kinematics-v01/README.md) +measured 0.0305 ms median and 0.0324 ms p95 for one joint-target calculation +on the recorded shared macOS host. Initialization, including asset verification +and compilation, took 221.5 ms. The receipts, earlier prototype measurement and +portable probe are retained. No dynamics, learned inference, rendering or full +control loop is included, so these numbers establish no real-time guarantee. diff --git a/scripts/benchmark-piper-encoder-kinematics.py b/scripts/benchmark-piper-encoder-kinematics.py new file mode 100644 index 0000000..66dcb60 --- /dev/null +++ b/scripts/benchmark-piper-encoder-kinematics.py @@ -0,0 +1,63 @@ +"""Bounded static geometry timing, with no learned model or dynamics execution.""" +import argparse, inspect, json, os, platform, sys, time +from contextlib import ExitStack +from hashlib import sha256 +from pathlib import Path +from unittest.mock import patch + +try: + import resource +except ImportError: + resource = None + +def peak_rss_bytes(): + if resource is None or sys.platform not in ('darwin', 'linux'): + return None + return resource.getrusage(resource.RUSAGE_SELF).ru_maxrss * (1 if sys.platform == 'darwin' else 1024) + +parser = argparse.ArgumentParser(description=__doc__) +parser.add_argument('--assets', required=True, help='Existing pinned PiPER asset root') +parser.add_argument('--output', required=True, help='Fresh JSON receipt path') +args = parser.parse_args() +out = Path(args.output) +if out.exists(): + raise FileExistsError('Timing output must be fresh.') +import numpy as np +from dvidia_training.piper_encoder_kinematics import PiperEncoderKinematics, ARM_LIMITS_RAD +from dvidia_training import reference_sim +source = Path(inspect.getfile(PiperEncoderKinematics)) +expected = "6606bb8aad04e5abb1e9b6d1a8ec628a4114acd167862167184fd4eef1d21c34" +if sha256(source.read_bytes()).hexdigest() != expected: + raise ValueError("This v0.1 probe requires the frozen encoder-kinematics source.") +mj, _ = reference_sim._native_imports() +with ExitStack() as guard: + for name in ('mj_step','mj_step1','mj_step2','mj_forward','mj_inverse','mj_collision','mj_fwdPosition'): + guard.enter_context(patch.object(mj,name,side_effect=AssertionError('No dynamics in static timing.'))) + start = time.perf_counter_ns() + robot = PiperEncoderKinematics(args.assets) + constructor_ms = (time.perf_counter_ns()-start)/1e6 + lower, upper = np.array(ARM_LIMITS_RAD).T + qs = np.random.default_rng(20261109).uniform(lower+.1, upper-.1, (64,6)) + targets = [] + for q in qs: + pose = robot.fk(q) + targets.append((np.array(pose['tcp_position_world_m'])+[.005,-.003,.004],pose['tcp_quaternion_world_wxyz'])) + for q,(position,quaternion) in zip(qs,targets): + robot.fk(q); robot.pose_action(q,position,quaternion,.02) + samples = {'fk': [],'pose_action': []} + for i in range(1024): + q=qs[i%64]; position,quaternion=targets[i%64] + start=time.perf_counter_ns();robot.fk(q);samples['fk'].append((time.perf_counter_ns()-start)/1e6) + start=time.perf_counter_ns();action=robot.pose_action(q,position,quaternion,.02);samples['pose_action'].append((time.perf_counter_ns()-start)/1e6) + assert len(action['joint_targets_rad'])==6 and np.isfinite(action['joint_targets_rad']).all() + assert robot._data.time == 0. +receipt = {'schema':'dvidia.piper-encoder-kinematics-static-timing/v1','source_sha256':expected, + 'probe_sha256':sha256(Path(__file__).read_bytes()).hexdigest(),'scope':'Shared-host static CPU API timing only. Targets were prepared before timing. No dynamics, rendering, learned inference, training, IO loop or hardware commands; no real-time deadline claim.', + 'configurations':64,'warmup_calls_per_api':64,'timed_calls_per_api':1024,'seed':20261109, + 'constructor_wall_ms_including_asset_validation_and_compile':constructor_ms, + 'api_wall_ms':{name:{'median':float(np.median(values)),'p95':float(np.quantile(values,.95)),'max':float(max(values))} for name,values in samples.items()}, + 'host':{'machine':platform.machine(),'platform':sys.platform,'macos':platform.mac_ver()[0],'python':platform.python_version(),'numpy':np.__version__,'mujoco':mj.__version__,'logical_cpus':os.cpu_count(),'declared_blas_threads':{key:os.getenv(key) for key in ('OPENBLAS_NUM_THREADS','OMP_NUM_THREADS','MKL_NUM_THREADS','VECLIB_MAXIMUM_THREADS')},'whole_process_peak_rss_bytes':peak_rss_bytes()}, + 'native_time_seconds':0.,'physical_robot_ready':False,'task_qualified':False} +assert sha256(source.read_bytes()).hexdigest()==expected +with out.open('x') as f:json.dump(receipt,f,indent=2,sort_keys=True);f.write('\n') +print(json.dumps(receipt,sort_keys=True)) diff --git a/src/dvidia_training/piper_encoder_kinematics.py b/src/dvidia_training/piper_encoder_kinematics.py new file mode 100644 index 0000000..7985808 --- /dev/null +++ b/src/dvidia_training/piper_encoder_kinematics.py @@ -0,0 +1,171 @@ +"""Encoder-only PiPER geometry and one authored differential-IK update. + +Each instance owns a private, pinned robot-only model. There is no live model, +MjData, object, contact, camera or teacher argument. Only kinematics, comPos and +site Jacobians are evaluated; no dynamics or actuator execution occurs. Use one +instance per worker: its private scratch data is reused, not thread-safe. + +The controlled point is the authored link6-local [0, 0, .12003] TCP in WORLD +coordinates. The older learning feature is the link6 ORIGIN in BASE coordinates; +fk names both separately. Neither point is a hardware-calibrated fingertip. + +pose_action matches PiperTaskEnv.pose_action's bounded DLS calculation. It is +one local geometric update, not a reachability test or collision-safe planner. +Its q input is measured encoders, not previous applied servo references. The +existing downstream rate limiter remains necessary and is not executed here. +""" +from __future__ import annotations + +from hashlib import sha256 +from pathlib import Path + +import numpy as np + +from . import reference_sim as sim +from . import robot_profiles + + +ASSET_LOCK_SHA256 = '4b2ee2ca675aceb2f826983cb3082a3e08e9aa0ff419b23078abcff08ebcc6fe' +ARM_JOINTS = tuple(f'joint{i}' for i in range(1, 7)) +TCP_OFFSET_LINK6_M = (0., 0., .12003) +ARM_LIMITS_RAD = ((-2.618, 2.618), (0., 3.14), (-2.697, 0.), + (-1.832, 1.832), (-1.22, 1.22), (-3.14, 3.14)) + + +def _vector(value, length, name): + """Reject malformed data rather than coerce strings, booleans or matrices.""" + if not isinstance(value, (list, tuple, np.ndarray)): + raise ValueError(f'{name} requires a numeric vector of length {length}') + if isinstance(value, np.ndarray): + if value.shape != (length,) or value.dtype.kind not in 'iuf': + raise ValueError(f'{name} requires a numeric vector of length {length}') + elif len(value) != length or any(isinstance(x, (bool, np.bool_)) or + not isinstance(x, (int, float, np.integer, np.floating)) for x in value): + raise ValueError(f'{name} requires a numeric vector of length {length}') + try: + result = np.array(value, dtype=np.float64, copy=True) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f'{name} requires finite numbers') from exc + if not np.isfinite(result).all(): + raise ValueError(f'{name} requires finite numbers') + return result + + +class PiperEncoderKinematics: + """Fixed PiPER geometry; constructor takes only a verified asset directory. + + Measured angles must be inside the exact native joint ranges, inclusively. + Even a small measured overshoot is refused, never silently clamped. Issued + targets use a separate .02 rad interior margin. With accepted measurements, + that margin cannot enlarge the final change beyond the .06 rad DLS bound. + Jaw commands are one finger's slide target in [0, .035] metres, not full gap. + """ + + def __init__(self, assets): + if sha256(robot_profiles.LOCK_PATH.read_bytes()).hexdigest() != ASSET_LOCK_SHA256: + raise ValueError('PiPER asset lock differs from the kinematics contract') + root = Path(assets) + root = root.parent if root.name == 'agilex_piper' else root + item, blobs = robot_profiles._payload('piper', root) + self._mj, _ = sim._native_imports() + spec = sim._spec(blobs, item['folder'], item['model']) + spec.body('link6').add_site(name='encoder_tcp', pos=TCP_OFFSET_LINK6_M, + size=[.004, 0, 0]) + self._model = spec.compile() + mj, model = self._mj, self._model + if (model.nq, model.nv, model.nu, model.njnt, model.neq, model.nmocap, + model.nbody) != (8, 8, 7, 8, 1, 0, 10): + raise ValueError('Expected the pinned robot only, without a task scene') + names = [mj.mj_id2name(model, mj.mjtObj.mjOBJ_JOINT, i) for i in range(8)] + actuators = [mj.mj_id2name(model, mj.mjtObj.mjOBJ_ACTUATOR, i) for i in range(7)] + if names != [*ARM_JOINTS, 'joint7', 'joint8'] or actuators != [*ARM_JOINTS, 'gripper']: + raise ValueError('PiPER joint or actuator order changed') + if (not np.array_equal(model.jnt_qposadr[:6], np.arange(6)) + or not np.array_equal(model.jnt_dofadr[:6], np.arange(6)) + or not np.all(model.jnt_type[:6] == mj.mjtJoint.mjJNT_HINGE) + or not np.array_equal(model.actuator_trnid[:6, 0], np.arange(6))): + raise ValueError('PiPER encoder axes changed') + self._limits = np.array(ARM_LIMITS_RAD, dtype=np.float64) + if (not np.array_equal(model.jnt_range[:6], self._limits) + or not np.array_equal(model.actuator_ctrlrange[:6], self._limits) + or not np.array_equal(model.actuator_ctrlrange[6], [0., .035])): + raise ValueError('PiPER joint or servo bounds changed') + self._tcp = mj.mj_name2id(model, mj.mjtObj.mjOBJ_SITE, 'encoder_tcp') + self._link = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, 'link6') + self._base = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, 'base_link') + if (self._tcp < 0 or self._link < 0 or self._base != 1 + or model.site_bodyid[self._tcp] != self._link + or not np.array_equal(model.site_pos[self._tcp], TCP_OFFSET_LINK6_M) + or not np.array_equal(model.site_quat[self._tcp], [1., 0., 0., 0.]) + or model.body_parentid[self._base] != 0 or model.body_jntnum[self._base] != 0 + or not np.array_equal(model.body_pos[self._base], [0., 0., 0.]) + or not np.array_equal(model.body_quat[self._base], [1., 0., 0., 0.])): + raise ValueError('PiPER fixed base or authored TCP changed') + self._data = mj.MjData(model) + # Fixed fingers cannot change the upstream link6/TCP pose or Jacobian. + self._data.qpos[6:8] = [.035, -.035] + + def _update(self, measured_joint_position_rad): + q = _vector(measured_joint_position_rad, 6, 'measured arm encoders') + if np.any(q < self._limits[:, 0]) or np.any(q > self._limits[:, 1]): + raise ValueError('Measured encoders exceed native joint limits') + self._data.qpos[:6] = q + self._mj.mj_kinematics(self._model, self._data) + self._mj.mj_comPos(self._model, self._data) + return q + + def fk(self, measured_joint_position_rad): + """Return detached poses with explicit point, frame and wxyz convention.""" + self._update(measured_joint_position_rad) + d = self._data + tcp_quat, link_quat = np.empty(4), np.empty(4) + self._mj.mju_mat2Quat(tcp_quat, d.site_xmat[self._tcp]) + base_rotation = d.xmat[self._base].reshape(3, 3) + relative_rotation = base_rotation.T @ d.xmat[self._link].reshape(3, 3) + self._mj.mju_mat2Quat(link_quat, relative_rotation.ravel()) + return {'tcp_position_world_m': d.site_xpos[self._tcp].tolist(), + 'tcp_quaternion_world_wxyz': tcp_quat.tolist(), + 'link6_position_base_m': (base_rotation.T @ (d.xpos[self._link] - d.xpos[self._base])).tolist(), + 'link6_quaternion_base_wxyz': link_quat.tolist()} + + def _jacobian(self): + position, rotation = np.zeros((3, self._model.nv)), np.zeros((3, self._model.nv)) + self._mj.mj_jacSite(self._model, self._data, position, rotation, self._tcp) + return np.vstack((position[:, :6], rotation[:, :6])) + + def jacobian(self, measured_joint_position_rad): + """Return a detached 6x6 TCP WORLD spatial Jacobian: linear then angular.""" + self._update(measured_joint_position_rad) + return self._jacobian() + + def pose_action(self, measured_joint_position_rad, position_world_m, + quaternion_world_wxyz, gripper_position_m): + """Return one authored DLS target update; do not apply it to an actuator.""" + position = _vector(position_world_m, 3, 'TCP world position') + if np.any(np.abs(position) > 10.): + raise ValueError('TCP world position exceeds the numeric input envelope') + quaternion = _vector(quaternion_world_wxyz, 4, 'TCP world quaternion') + if np.any(np.abs(quaternion) > 1.) or not np.isclose(quaternion @ quaternion, 1., atol=1e-8, rtol=0): + raise ValueError('TCP quaternion must be normalized wxyz') + jaw = _vector([gripper_position_m], 1, 'single-finger target')[0] + if not 0 <= jaw <= .035: + raise ValueError('Single-finger target must lie in [0, .035] metres') + measured = self._update(measured_joint_position_rad) + current, rotation = np.empty(4), np.empty(3) + self._mj.mju_mat2Quat(current, self._data.site_xmat[self._tcp]) + self._mj.mju_subQuat(rotation, quaternion, current) + rotation = self._data.site_xmat[self._tcp].reshape(3, 3) @ rotation + translation = position - self._data.site_xpos[self._tcp] + for error, maximum in ((translation, .015), (rotation, .05)): + norm = float(np.linalg.norm(error)) + if norm > maximum: + error *= maximum / norm + jacobian = self._jacobian() + jacobian[3:] *= .25 + error = np.concatenate((translation, .25 * rotation)) + delta = jacobian.T @ np.linalg.solve(jacobian @ jacobian.T + .012**2 * np.eye(6), error) + targets = np.clip(measured + np.clip(delta, -.06, .06), + self._limits[:, 0] + .02, self._limits[:, 1] - .02) + if not np.isfinite(targets).all() or np.any(np.abs(targets - measured) > .06 + 1e-12): + raise ValueError('DLS output violates the finite joint-update contract') + return {'joint_targets_rad': targets.tolist(), 'gripper_position_m': float(jaw)} diff --git a/tests/test_piper_encoder_kinematics.py b/tests/test_piper_encoder_kinematics.py new file mode 100644 index 0000000..5d705eb --- /dev/null +++ b/tests/test_piper_encoder_kinematics.py @@ -0,0 +1,210 @@ +"""Static geometry checks only: no settling, integration, rendering or policy fit. + +Set DVIDIA_ROBOT_PROFILE_ASSETS to the existing pinned asset directory. Tests +compile robot/full-task snapshots but forbid Python dynamics entry points. +""" +from contextlib import ExitStack +from hashlib import sha256 +import inspect +import os +from pathlib import Path +import tempfile +import unittest +from unittest.mock import patch +import xml.etree.ElementTree as ET + +import numpy as np + +from dvidia_training import piper_encoder_kinematics as kin + + +def quaternion_matrix(q): + # Independent closed-form rotation, without MuJoCo quaternion utilities. + w, x, y, z = np.asarray(q) / np.linalg.norm(q) + return np.array([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)], + [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)], + [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]]) + + +def manual_xml_fk(xml, q): + """Independent homogeneous-transform chain read from the locked XML.""" + body = ET.fromstring(xml).find('worldbody/body') + rotation, position = np.eye(3), np.zeros(3) + for index in range(7): + local_position = np.fromstring(body.get('pos', '0 0 0'), sep=' ') + local_rotation = quaternion_matrix(np.fromstring(body.get('quat', '1 0 0 0'), sep=' ')) + position += rotation @ local_position + rotation = rotation @ local_rotation + if index: + joint = body.find('joint') + axis = np.fromstring(joint.get('axis', '0 0 1'), sep=' ') + axis /= np.linalg.norm(axis) + anchor = np.fromstring(joint.get('pos', '0 0 0'), sep=' ') + x, y, z = axis + cross = np.array([[0., -z, y], [z, 0., -x], [-y, x, 0.]]) + angle = q[index-1] + turn = np.eye(3) + np.sin(angle)*cross + (1-np.cos(angle))*(cross @ cross) + position += rotation @ (anchor - turn @ anchor) + rotation = rotation @ turn + if index < 6: + body = body.find('body') + return position, rotation + + +class EncoderInterfaceTests(unittest.TestCase): + def test_no_live_model_or_data_constructor_and_no_dynamics_calls(self): + self.assertEqual(list(inspect.signature(kin.PiperEncoderKinematics).parameters), ['assets']) + source = inspect.getsource(kin) + for forbidden in ('.mj_step(', '.mj_forward(', '.mj_collision(', '.ncon', '.contact', '.ctrl'): + self.assertNotIn(forbidden, source) + self.assertIn('measured_joint_position_rad', inspect.signature(kin.PiperEncoderKinematics.pose_action).parameters) + + def test_wrong_lock_refused_before_assets_or_native_import(self): + with tempfile.TemporaryDirectory() as directory: + path = Path(directory)/'changed-lock.json' + path.write_text('{}') + with patch.object(kin.robot_profiles, 'LOCK_PATH', path), \ + patch.object(kin.robot_profiles, '_payload') as payload, \ + patch.object(kin.sim, '_native_imports') as native: + with self.assertRaisesRegex(ValueError, 'asset lock'): + kin.PiperEncoderKinematics(directory) + payload.assert_not_called() + native.assert_not_called() + + +@unittest.skipUnless(os.environ.get('DVIDIA_ROBOT_PROFILE_ASSETS'), 'Set DVIDIA_ROBOT_PROFILE_ASSETS to pinned assets') +class StaticEncoderGeometryTests(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.assets = Path(os.environ['DVIDIA_ROBOT_PROFILE_ASSETS']) + cls.mj, _ = kin.sim._native_imports() + cls.guards = ExitStack() + cls.addClassCleanup(cls.guards.close) + for name in ('mj_step', 'mj_step1', 'mj_step2', 'mj_forward', 'mj_inverse', 'mj_collision', 'mj_fwdPosition'): + cls.guards.enter_context(patch.object(cls.mj, name, side_effect=AssertionError('Dynamics forbidden in static geometry test'))) + cls.robot = kin.PiperEncoderKinematics(cls.assets) + cls.xml = (cls.assets/'agilex_piper/piper.xml').read_text() + cls.lower, cls.upper = np.array(kin.ARM_LIMITS_RAD).T + cls.reset = np.array([0., 1.57, -1.3485, 0., 0., 0.]) + cls.configurations = [cls.reset, np.array([.4, .9, -1.9, .3, -.4, .8]), + np.array([-.7, 2.1, -.8, -.6, .5, -1.1]), cls.lower, cls.upper] + cls.configurations += list(np.random.default_rng(20261109).uniform(cls.lower+.1, cls.upper-.1, (12, 6))) + from dvidia_training import piper_task + cls.task = piper_task + # Original, unchanged method is the parity oracle; no copied DLS test oracle. + if sha256(Path(piper_task.__file__).read_bytes()).hexdigest() != 'e66a44a8df4e7a8ee42e4f3866f889f8797cdfb8e2688a146e09b75d2c76664f': + raise AssertionError('Original pose_action oracle source changed') + cls.task_model = piper_task.build_task_model(cls.assets) + + def test_fk_matches_independent_xml_chain_across_joint_space(self): + for q in self.configurations: + with self.subTest(q=q.tolist()): + link_position, rotation = manual_xml_fk(self.xml, q) + pose = self.robot.fk(q) + np.testing.assert_allclose(pose['link6_position_base_m'], link_position, atol=2e-12, rtol=0) + np.testing.assert_allclose(quaternion_matrix(pose['link6_quaternion_base_wxyz']), rotation, atol=2e-12, rtol=0) + expected = link_position + rotation @ np.array([0., 0., .12003]) + np.testing.assert_allclose(pose['tcp_position_world_m'], expected, atol=2e-12, rtol=0) + np.testing.assert_allclose(quaternion_matrix(pose['tcp_quaternion_world_wxyz']), rotation, atol=2e-12, rtol=0) + self.assertAlmostEqual(np.linalg.norm(expected-link_position), .12003, places=12) + + def test_world_spatial_jacobian_matches_central_finite_differences(self): + epsilon = 1e-6 + for q in self.configurations[:3] + self.configurations[5:]: + analytic = self.robot.jacobian(q) + expected = np.empty((6, 6)) + for joint in range(6): + step = np.eye(6)[joint]*epsilon + plus, minus = self.robot.fk(q+step), self.robot.fk(q-step) + expected[:3, joint] = (np.array(plus['tcp_position_world_m'])-minus['tcp_position_world_m'])/(2*epsilon) + rp = quaternion_matrix(plus['tcp_quaternion_world_wxyz']) + rm = quaternion_matrix(minus['tcp_quaternion_world_wxyz']) + world_relative = rp @ rm.T + expected[3:, joint] = np.array([world_relative[2, 1]-world_relative[1, 2], + world_relative[0, 2]-world_relative[2, 0], world_relative[1, 0]-world_relative[0, 1]])/(4*epsilon) + np.testing.assert_allclose(analytic, expected, atol=7e-10, rtol=0) + + def test_original_full_task_static_pose_action_parity(self): + for index, q in enumerate(self.configurations): + env = self.task.PiperTaskEnv.__new__(self.task.PiperTaskEnv) + env.ready, env.done = True, False + env.model, env.data = self.task_model, self.mj.MjData(self.task_model) + env.tcp = self.mj.mj_name2id(env.model, self.mj.mjtObj.mjOBJ_SITE, 'piper_task_tcp') + env.data.qpos[:6] = q + # Different fingers and object pose cannot enter the private encoder model. + env.data.qpos[6:8] = [.005*index % .035, -(.005*index % .035)] + env.data.qpos[8:11] = [.2+index*.01, -.2, .3] + self.mj.mj_kinematics(env.model, env.data) + self.mj.mj_comPos(env.model, env.data) + for desired, quat in (([.31, -.07, .12], [0., 0., 1., 0.]), + ([-.1, .2, .3], [.5, -.5, .5, -.5])): + expected = env.pose_action(desired, quat, .017) + actual = self.robot.pose_action(q, desired, quat, .017) + np.testing.assert_allclose(actual['joint_targets_rad'], expected['joint_targets_rad'], atol=2e-13, rtol=0) + self.assertEqual(actual['gripper_position_m'], expected['gripper_position_m']) + self.assertEqual(env.data.time, 0.) + + def test_quaternion_sign_equivalence_and_zero_error(self): + for q in self.configurations[:3]: + positive = self.robot.pose_action(q, [.3, .1, .2], [.5, -.5, .5, -.5], 0.) + negative = self.robot.pose_action(q, [.3, .1, .2], [-.5, .5, -.5, .5], 0.) + np.testing.assert_allclose(positive['joint_targets_rad'], negative['joint_targets_rad'], atol=2e-15, rtol=0) + pose = self.robot.fk(q) + action = self.robot.pose_action(q, pose['tcp_position_world_m'], pose['tcp_quaternion_world_wxyz'], .035) + np.testing.assert_allclose(action['joint_targets_rad'], q, atol=2e-15, rtol=0) + + def test_clipped_targets_honor_delta_and_margin_including_boundaries(self): + for q in self.configurations: + action = self.robot.pose_action(q, [9., -9., 9.], [0., 1., 0., 0.], .035) + target = np.array(action['joint_targets_rad']) + self.assertTrue(np.all(np.abs(target-q) <= .06+1e-12)) + self.assertTrue(np.all(target >= self.lower+.02)) + self.assertTrue(np.all(target <= self.upper-.02)) + + def test_stateless_copied_results_and_inputs(self): + a, b = self.configurations[:2] + original = a.copy() + first = self.robot.fk(a) + jacobian = self.robot.jacobian(a) + action = self.robot.pose_action(a, [.3, 0., .2], [0., 0., 1., 0.], .02) + self.robot.fk(b) + self.robot.pose_action(b, [.4, .1, .15], [1., 0., 0., 0.], 0.) + self.assertEqual(self.robot.fk(a), first) + np.testing.assert_array_equal(self.robot.jacobian(a), jacobian) + self.assertEqual(self.robot.pose_action(a, [.3, 0., .2], [0., 0., 1., 0.], .02), action) + first['tcp_position_world_m'][0] = 100. + jacobian[:] = 100. + action['joint_targets_rad'][0] = 100. + self.assertNotEqual(self.robot.fk(a)['tcp_position_world_m'][0], 100.) + self.assertFalse(np.any(self.robot.jacobian(a) == 100.)) + np.testing.assert_array_equal(a, original) + self.assertEqual(self.robot._data.time, 0.) + np.testing.assert_array_equal(self.robot._data.qvel, np.zeros(8)) + + def test_bad_measured_values_and_overshoot_are_refused(self): + bad = [None, '000000', [0.]*5, [[0.]*6], np.zeros((6, 1)), np.zeros(6, dtype=bool), + [False, 1., -1., 0., 0., 0.], ['0', 1., -1., 0., 0., 0.], + [np.nan, 1., -1., 0., 0., 0.], [np.inf, 1., -1., 0., 0., 0.], + self.lower-np.array([1e-12, 0., 0., 0., 0., 0.]), self.upper+1e-12] + for values in bad: + for method in (self.robot.fk, self.robot.jacobian): + with self.subTest(value=repr(values), method=method.__name__), self.assertRaises(ValueError): + method(values) + with self.assertRaises(ValueError): + self.robot.pose_action(values, [.3, 0., .2], [1., 0., 0., 0.], .02) + + def test_bad_targets_and_jaw_units_are_refused(self): + for position in ([0., 0.], [[.3], [0.], [.2]], [True, 0., .2], [np.nan, 0., .2], [11., 0., .2]): + with self.subTest(position=position), self.assertRaises(ValueError): + self.robot.pose_action(self.reset, position, [1., 0., 0., 0.], .02) + for quaternion in ([0., 0., 0., 0.], [1., 1., 0., 0.], [1.000000001, 0., 0., 0.], + [1., 0., 0.], [True, 0., 0., 0.], [np.inf, 0., 0., 0.]): + with self.subTest(quaternion=quaternion), self.assertRaises(ValueError): + self.robot.pose_action(self.reset, [.3, 0., .2], quaternion, .02) + for jaw in (-1e-12, .035000000001, .07, True, '0.02', np.nan, np.inf, [.02]): + with self.subTest(jaw=jaw), self.assertRaises(ValueError): + self.robot.pose_action(self.reset, [.3, 0., .2], [1., 0., 0., 0.], jaw) + + +if __name__ == '__main__': + unittest.main()