First commit

.gitignore

@@ -0,0 +1,5 @@
+build/
+bin/
+.venv/
+__pycache__/
+*.pyc

MTEST04-MTN-ARM_parameters.yaml

@@ -0,0 +1,7 @@
+ioc_name: MTEST04-MTN-ARM
+ioc_host: c6025a-08
+ioc_port: 50001
+ioc_user: root
+epics_version: 7.0.10
+os: deb12
+cpu_architecture: x86_64

MTEST04-MTN-ARM_startup.script

@@ -0,0 +1,8 @@
+# Simulation only: no EtherCAT master. Run from this project directory.
+# Build with make logic ECMC_ROOT=<source matching the IOC>.
+# PSI launcher provides iocInit and ecmccfg finalization.
+epicsEnvSet("IOC", "${IOC=MTEST04-MTN-ARM}")
+require ecmccfg v11.2.0_RC1 "ECMC_VER=v11.2.0_RC1,MASTER_ID=-1,EC_RATE=1000"
+${SCRIPTEXEC} ./cfg/load.cmd
+# caqtdm -macro "IOC=MTEST04-MTN-ARM" ecmcCppLogicOverview.ui
+# caqtdm -macro "IOC=MTEST04-MTN-ARM,CPP_ID=0" ecmcCppLogic.ui

Makefile

@@ -0,0 +1,36 @@
+CXX ?= c++
+CXXFLAGS ?= -O2 -std=c++17 -Wall -Wextra -pedantic
+PYTHON ?= python3
+ECMC_ROOT ?= ../ecmc
+ifeq ($(shell uname -s),Darwin)
+EXT := dylib
+SHARED := -dynamiclib
+else
+EXT := so
+SHARED := -shared
+endif
+.DEFAULT_GOAL := all
+.PHONY: all viewer-lib test test-logic logic clean
+all: viewer-lib build/test_kinematics
+viewer-lib: build/libarm.$(EXT)
+build/libarm.$(EXT): src/c_api.cpp src/kinematics.hpp
+	mkdir -p $(@D)
+	$(CXX) $(CXXFLAGS) -fPIC $(SHARED) -o $@ src/c_api.cpp
+build/test_kinematics: tests/test_kinematics.cpp src/kinematics.hpp
+	mkdir -p $(@D)
+	$(CXX) $(CXXFLAGS) -Isrc -o $@ $<
+test: all
+	./build/test_kinematics
+	$(PYTHON) python/viewer.py --self-test
+	$(PYTHON) tests/test_epics_source.py
+logic: bin/libmain.$(EXT)
+bin/libmain.$(EXT): src/logic.cpp src/kinematics.hpp
+	mkdir -p $(@D)
+	$(CXX) $(CXXFLAGS) -fPIC $(SHARED) -I$(ECMC_ROOT)/devEcmcSup/logic -o $@ $<
+build/test_logic: tests/test_logic.cpp src/logic.cpp src/kinematics.hpp
+	mkdir -p $(@D)
+	$(CXX) $(CXXFLAGS) -I$(ECMC_ROOT)/devEcmcSup/logic -o $@ $<
+test-logic: build/test_logic
+	./build/test_logic
+clean:
+	rm -rf build bin

README.md

@@ -0,0 +1,199 @@
+# ecmc six-DOF robot arm — simulation prototype
+
+![Six-joint arm simulation](docs/arm.png)
+
+A generic six-revolute-joint industrial-style arm with C++ forward and inverse
+kinematics, twelve ecmc axes, and an offline/read-only EPICS 3D viewer.
+This is an example geometry, not a calibrated model of a commercial robot.
+
+**All twelve axes are virtual in this simulation.** `virt` and `phys` are group
+names: `virt` contains the six Cartesian tool coordinates; `phys` contains the
+six simulated rotary joints. No physical drives or EtherCAT hardware are used.
+
+| Axis IDs | Group | Meaning | Trajectory | Encoder feedback |
+|---|---|---|---|---|
+| 1–6 | `virt` | X, Y, Z, Rx, Ry, Rz offsets from home tool pose | Internal | FK from joint encoders |
+| 7–12 | `phys` | J1–J6 angular offsets from home joints | External IK, or internal for direct joint moves | Ideal copy of selected joint trajectory |
+
+## Run the offline demo
+
+```sh
+make test
+python3 -m venv .venv
+.venv/bin/pip install -r python/requirements.txt
+.venv/bin/python python/viewer.py
+# Direct joint controls instead of Cartesian controls:
+.venv/bin/python python/viewer.py --joints
+# Headless screenshot:
+.venv/bin/python python/viewer.py --snapshot build/arm.png
+```
+
+The six sliders set Cartesian home offsets, then **Move (3 seconds)** starts a
+shared quintic interpolation. Blue shows joint feedback and orange the IK
+command. An unsuccessful IK solve retains the last valid joints and displays an
+error. Retargeting starts a new profile without velocity continuity. Joint mode
+interpolates joint offsets instead. The offline plant is ideal; it has no lag,
+load, drive dynamics, collision checks or physical safety system.
+
+Both Python and the IOC share `src/kinematics.hpp`. The viewer calls a small
+C ABI in `src/c_api.cpp`; `make viewer-lib` rebuilds `build/libarm.so` (`.dylib`
+on macOS). No EPICS or Python GUI dependencies are needed by `make test`.
+
+## Arm geometry and conventions
+
+Lengths are in mm; joint and Cartesian Euler angles are in degrees. The base
+frame is right handed, with +Z upward. Revolute axes act in each successive
+local frame. With transforms multiplied in the order written:
+
+```text
+T = Rz(q1) Tz(H) Ry(q2) Tx(A) Ry(q3) Tx(B)
+    Rx(q4) Ry(q5) Rx(q6) Tx(Tool)
+```
+
+The last three axes intersect at the wrist centre. The eight plotted points
+include coincident wrist joints; the drawing is a link skeleton, not solid CAD.
+
+`cfg/geometry.json` contains `[H, A, B, Tool]`, default `[300, 300, 250, 80]`.
+The IOC uses macros `BASE_HEIGHT`, `UPPER_ARM`, `FOREARM`, `TOOL` in the
+`MACROS` argument in `cfg/load.cmd`. They have the same defaults. Keep JSON and
+IOC macros identical; neither side automatically reads the other's geometry.
+
+| Joint | Local rotation | Home angle | Absolute model limits | Axis offset limits |
+|---|---|---:|---|---|
+| J1 | Z | 0° | −170…170° | −170…170° |
+| J2 | Y | −35° | −120…120° | −85…155° |
+| J3 | Y | 70° | −140…140° | −210…70° |
+| J4 | X | 0° | −180…180° | −180…180° |
+| J5 | Y | 35° | −120…120° | −155…85° |
+| J6 | X | 0° | −180…180° | −180…180° |
+
+The chosen home avoids a straight elbow/wrist and gives a useful local workspace.
+All IOC axes start at **zero offsets**, representing these nominal joint angles.
+This is mathematical initialization, not a homing or calibration procedure.
+
+Cartesian XYZ are base-frame displacements from the home tool position. For
+orientation, `R = Rz(Rz) Ry(Ry) Rx(Rx) R_home`. Thus zero Cartesian angles mean
+the home tool orientation, not alignment of the tool with the base. The Euler
+representation has a singularity at Ry = ±90°. FK computes absolute tool
+transforms directly, then converts them into these Cartesian offsets for EPICS.
+
+To model a particular robot, replace the transform chain, joint limits and home
+configuration consistently in C++, YAML and viewer. Arbitrary URDF loading,
+manufacturer-specific wrist offsets and calibration are not implemented.
+
+## Inverse kinematics
+
+IK is a bounded, local damped least-squares solve, seeded from the previous
+accepted joint command (nominal home initially). It minimizes a six-component
+residual: three translation errors and the rotation logarithm of
+`R_target R_actual^T`, weighted by 200 mm/radian. Orientation residuals use
+rotation matrices rather than subtracting Euler angles.
+
+A central-difference Jacobian uses 1e-5-radian perturbations. Each solve allows
+at most 100 iterations, clamps trial angles to joint limits, limits steps to
+0.2 radians, and adapts damping according to whether the error decreases.
+Convergence requires weighted residual norm below 1e-5. All cyclic arrays have
+fixed size; the solver performs no file I/O or heap allocation.
+
+The seed encourages a nearby continuous solution. It does not enumerate all
+elbow/wrist configurations or guarantee the nearest branch. Unreachable targets,
+joint limits, singularities and poor seeds can prevent convergence. No explicit
+Jacobian-condition monitor or global path planner is provided. A reachable
+endpoint alone does not validate the path to it.
+
+## ecmc IOC
+
+Build on the Linux IOC development host with the source headers matching the
+installed ecmc version:
+
+```sh
+make logic ECMC_ROOT=/path/to/ecmc
+make test-logic ECMC_ROOT=/path/to/ecmc
+```
+
+`make logic` produces `bin/libmain.so`; deploy it with `cfg/` and the startup
+script. Custom PV substitutions are checked in as
+`cfg/libmain.so_cpp_logic.subs`, matching `loadCppLogic.cmd` defaults.
+`src/Makefile` also provides the PSI `driver.makefile` build option for Debian
+IOC architectures; it builds only `logic.cpp`. The top-level Makefile is a
+portable local build and does not install or launch an IOC.
+
+`MTEST04-MTN-ARM_startup.script` follows the ecmccfg `cpp_logic` examples and
+requires `ecmccfg v11.2.0_RC1` / `ecmc v11.2.0_RC1`, no EtherCAT master,
+and `EC_RATE=1000`. Start with your normal PSI IOC launcher from this project
+directory; the launcher supplies `iocInit`, and ecmccfg finalizes at that point.
+The parameters YAML is a deployment example: adjust host, port and version for
+your installation. Axis IDs 1–12 and the default state-machine ID 0 must be free.
+
+The startup loads the shared Cartesian YAML six times and the six numbered
+joint YAML files (each with its own numeric limits), then C++ logic, then
+`Arm_SM` (`virt` master group, `phys` slave group). Each axis has motor records,
+source changes allowed while enabled, 1 s auto-enable timeout, 2 s auto-disable
+delay, and at-target monitoring with a 0.01-unit band for 100 cycles (100 ms
+at 1 kHz). Default velocity is 1 mm/s for XYZ and 1°/s for rotation/joints.
+Joint soft limits are configured in home-relative degrees. Cartesian soft limits
+are disabled; IK checks joint limits on each accepted command.
+
+C++ binds to asyn `axN.setpos` and `axN.actpos`, not PLC object paths.
+IK feeds external joint setpoints. The ideal joint plant copies the selected
+joint trajectory into the external encoder, so direct joint trajectories also
+produce FK feedback. Axis/C++ ordering introduces cyclic pipeline latency.
+A disabled simulated joint still follows this mathematical ideal plant.
+
+**Use Cartesian `virt` axes for normal tool motion.** Independent `phys` joint
+moves can cross singularities, change assembly branches or cause collisions.
+Direct joint moves are for controlled simulation/commissioning checks, with the
+state machine selecting internal joint trajectories. Before returning to
+Cartesian motion, synchronize Cartesian commands to actual pose and verify the
+IK seed/branch; automatic bumpless mode transfer is not implemented.
+
+Fault codes are latched: 0 OK; 1 invalid input or joint bounds; 2 IK failure;
+3 host output write failure; 4 axis error. Fix the cause and restart/reload.
+A fault retains outputs; it is not a coherent group stop, and a failed write can
+leave partial updates. C++ faults are not wired into a safety-rated stop system.
+The built-in master/slave state machine does not replace that integration.
+
+Independent Cartesian motor moves are not a coordinated six-axis Cartesian path.
+No joint-space speed/acceleration limiting of IK output, collision avoidance,
+singularity avoidance, torque control, real drives or physical safety integration
+is implemented. This is simulation software, not a hardware motion controller
+ready for commissioning. Hexapod timing measurements do not apply to this arm.
+Measure the complete IOC cycle and overruns before assuming 1 kHz operation.
+
+## Read-only EPICS viewer and caQtDM
+
+```sh
+.venv/bin/pip install -r python/requirements-ioc.txt
+make viewer-lib
+.venv/bin/python python/viewer.py --prefix MTEST04-MTN-ARM:
+caqtdm -macro "IOC=MTEST04-MTN-ARM" ecmcCppLogicOverview.ui
+caqtdm -macro "IOC=MTEST04-MTN-ARM,CPP_ID=0" ecmcCppLogic.ui
+```
+
+The standard caQtDM panels come from ecmccfg. There is no custom arm panel yet.
+Live Python mode disables motion controls and subscribes to:
+
+| PV suffix | Value |
+|---|---|
+| `arm-pose-set` / `arm-pose-actual` | Six home-relative tool coordinates, mm/degrees |
+| `arm-joint-set` / `arm-joint-actual` | Six home-relative joint offsets, degrees |
+| `arm-fault` | Latched adapter fault |
+| `arm-ik-residual` | Weighted IK residual norm |
+| `arm-ik-iterations` | IK correction attempts |
+| `arm-simulation` | Always 1 |
+
+The viewer accepts a prefix with or without the trailing colon. Its geometry
+must match the IOC. pyepics requires libca and CA connectivity. Callbacks update
+a locked cache; GUI refresh does not issue blocking PV reads. Disconnected,
+invalid and pending samples are reported. Separate PV records are not an atomic
+IOC snapshot, and unchanged values do not constitute a heartbeat check.
+
+## Validation
+
+`make test` covers independent straight-arm/base-rotation checks, rotation error
+at 180°, 500 near-home IK/FK round trips, 1,000 continuous joint-path samples,
+invalid/unreachable inputs, singular pose holding, Python/C ABI round trips,
+and mocked CA cache connection/value checks. `make test-logic` checks bindings,
+IK/plant/FK timing, direct joint feedback, joint limits and latched faults.
+A headless viewer snapshot checks rendering separately. A live robot-arm IOC and
+physical robot have not been tested; the full workspace is not validated.

cfg/axes.cmd

@@ -0,0 +1,13 @@
+# Coordinates: base-frame offsets from nominal home. Joint axes: home-relative degrees.
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=1,AX_NAME=X,AX_UNIT=mm"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=2,AX_NAME=Y,AX_UNIT=mm"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=3,AX_NAME=Z,AX_UNIT=mm"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=4,AX_NAME=Rx,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=5,AX_NAME=Ry,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_coordinate.yaml,AX_ID=6,AX_NAME=Rz,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint1.yaml,AX_ID=7,AX_NAME=J1,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint2.yaml,AX_ID=8,AX_NAME=J2,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint3.yaml,AX_ID=9,AX_NAME=J3,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint4.yaml,AX_ID=10,AX_NAME=J4,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint5.yaml,AX_ID=11,AX_NAME=J5,AX_UNIT=deg"
+${SCRIPTEXEC} ${ecmccfg_DIR}loadYamlAxis.cmd, "FILE=./cfg/axis_joint6.yaml,AX_ID=12,AX_NAME=J6,AX_UNIT=deg"

cfg/axis_coordinate.yaml

@@ -0,0 +1,62 @@
+# Macros: AX_ID, AX_NAME, AX_UNIT (default mm).
+# Coordinate axis: external encoder from forward kinematics.
+axis:
+  id: ${AX_ID}
+  group: virt
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=mm}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by C++ arm FK; no EtherCAT position link.
+
+trajectory:
+  source: 0
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: false
+
+monitoring:
+  target:
+    enable: true
+    # Axis units: 0.01 mm for X/Y/Z, 0.01 deg for Rx/Ry/Rz.
+    tolerance: 0.01
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint1.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -170
+  forward: 170
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint2.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -85
+  forward: 155
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint3.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -210
+  forward: 70
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint4.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -180
+  forward: 180
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint5.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -155
+  forward: 85
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/axis_joint6.yaml

@@ -0,0 +1,66 @@
+# Simulated rotary joints, home-relative degrees.
+# WARNING: independent joint moves can leave the Cartesian workspace or cross
+# singularities; use virt axes for normal tool moves. No collision checking.
+axis:
+  id: ${AX_ID}
+  group: phys
+  type: virtual
+  features:
+    allowSrcChangeWhenEnabled: true
+  autoEnable:
+    atStartup: false
+    enableTimeout: 1.0
+    disableTimeout: 2.0
+
+epics:
+  name: ${AX_NAME}
+  precision: 4
+  unit: ${AX_UNIT=deg}
+  motorRecord:
+    enable: true
+    description: "Arm ${AX_NAME}"
+
+encoder:
+  type: 0
+  source: 1
+  numerator: 1
+  denominator: 1
+  bits: 32
+  # External feedback is supplied by the C++ ideal plant; no EtherCAT position link.
+
+trajectory:
+  source: 1
+  axis:
+    velocity: 1
+    acceleration: 10
+    deceleration: 10
+
+input:
+  limit:
+    forward: ec0.s-1.ONE.0
+    backward: ec0.s-1.ONE.0
+  home: ec0.s-1.ZERO.0
+
+softlimits:
+  enable: true
+  forwardEnable: true
+  backwardEnable: true
+  backward: -180
+  forward: 180
+
+monitoring:
+  target:
+    enable: true
+    tolerance: 0.01  # deg
+    # Consecutive cycles within tolerance: 100 ms at 1 kHz.
+    time: 100
+  lag:
+    enable: false
+    tolerance: 0.1
+    time: 100
+  velocity:
+    enable: false
+    max: 0
+    time:
+      trajectory: 100
+      drive: 100

cfg/geometry.json

@@ -0,0 +1 @@
+[300, 300, 250, 80]

cfg/libmain.so_cpp_logic.subs

@@ -0,0 +1,22 @@
+# Auto-generated C++ logic substitutions
+# Logic: arm
+
+file ecmcCppLogicWaveformIn.template {
+  pattern { REC, PARAM, DESC, NELM, FTVL, DTYP }
+  { "arm-pose-set", "arm.pose_set", "arm.pose_set", "6", "DOUBLE", "asynFloat64ArrayIn" }
+  { "arm-pose-actual", "arm.pose_actual", "arm.pose_actual", "6", "DOUBLE", "asynFloat64ArrayIn" }
+  { "arm-joint-set", "arm.joint_set", "arm.joint_set", "6", "DOUBLE", "asynFloat64ArrayIn" }
+  { "arm-joint-actual", "arm.joint_actual", "arm.joint_actual", "6", "DOUBLE", "asynFloat64ArrayIn" }
+}
+
+file ecmcCppLogicLongIn.template {
+  pattern { REC, PARAM, DESC }
+  { "arm-fault", "arm.fault", "arm.fault" }
+  { "arm-ik-iterations", "arm.ik_iterations", "arm.ik_iterations" }
+  { "arm-simulation", "arm.simulation", "arm.simulation" }
+}
+
+file ecmcCppLogicAi.template {
+  pattern { REC, PARAM, DESC, PREC }
+  { "arm-ik-residual", "arm.ik_residual", "arm.ik_residual", "3" }
+}

cfg/load.cmd

@@ -0,0 +1,5 @@
+# Include after ecmccfg initialization, before runtime startup.
+# Axis IDs 1..12 and state-machine ID 0 must be available.
+${SCRIPTEXEC} ./cfg/axes.cmd
+${SCRIPTEXEC} ${ecmccfg_DIR}loadCppLogic.cmd, "DIR=./bin/,FILE=libmain.so,ASYN_PORT=CPP.ARM,REPORT=1,MACROS='COORD_FIRST=1,JOINT_FIRST=7,SIMULATION=1'"
+${SCRIPTEXEC} ${ecmccfg_DIR}addMasterSlaveSM.cmd, "NAME=Arm_SM,MST_GRP_NAME=virt,SLV_GRP_NAME=phys"

python/epics_source.py

@@ -0,0 +1,79 @@
+"""Read-only CA monitor cache; GUI reads never issue blocking network requests."""
+import math
+from threading import Lock
+
+
+class IocSource:
+    SUFFIXES = {
+        'pose_set': 'arm-pose-set',
+        'pose_actual': 'arm-pose-actual',
+        'joint_set': 'arm-joint-set',
+        'joint_actual': 'arm-joint-actual',
+        'fault': 'arm-fault',
+        'residual': 'arm-ik-residual',
+    }
+    ARRAYS = {'pose_set', 'pose_actual', 'joint_set', 'joint_actual'}
+
+    def __init__(self, prefix, pv_factory=None):
+        prefix = prefix.rstrip(':') + ':'
+        if pv_factory is None:
+            from epics import PV
+            pv_factory = PV
+        self._lock = Lock()
+        self._values = {}
+        self._connected = {}
+        self._errors = {}
+        self.pvs = {}
+        for key, suffix in self.SUFFIXES.items():
+            self.pvs[key] = pv_factory(
+                prefix + suffix, auto_monitor=True,
+                callback=lambda value=None, _key=key, **kw: self._update(_key, value),
+                connection_callback=lambda conn=False, _key=key, **kw: self._connection(_key, conn))
+
+    def _connection(self, key, connected):
+        with self._lock:
+            self._connected[key] = connected
+            if not connected:
+                self._values.pop(key, None)
+                self._errors.pop(key, None)
+
+    def _update(self, key, value):
+        try:
+            if key in self.ARRAYS:
+                value = tuple(float(v) for v in value)
+                if len(value) != 6 or not all(math.isfinite(v) for v in value):
+                    raise ValueError('expected six finite values')
+            else:
+                value = float(value)
+                if not math.isfinite(value):
+                    raise ValueError('expected finite scalar')
+                if key == 'fault':
+                    if not value.is_integer():
+                        raise ValueError('expected integer fault code')
+                    value = int(value)
+        except (TypeError, ValueError, OverflowError) as exc:
+            with self._lock:
+                self._values.pop(key, None)
+                self._errors[key] = str(exc)
+            return
+        with self._lock:
+            self._values[key] = value
+            self._errors.pop(key, None)
+
+    def snapshot(self):
+        # Consistent local cache copy; separate CA records are not a single IOC cycle.
+        with self._lock:
+            missing = [self.SUFFIXES[k] for k in self.SUFFIXES if not self._connected.get(k)]
+            if missing:
+                raise RuntimeError('EPICS disconnected:\n' + '\n'.join(missing))
+            if self._errors:
+                key = next(iter(self._errors))
+                raise RuntimeError(f'{self.SUFFIXES[key]}:\n{self._errors[key]}')
+            pending = [self.SUFFIXES[k] for k in self.SUFFIXES if k not in self._values]
+            if pending:
+                raise RuntimeError('Waiting for PV data:\n' + '\n'.join(pending))
+            return dict(self._values)
+
+    def close(self):
+        for pv in self.pvs.values():
+            pv.disconnect()

python/requirements-ioc.txt

@@ -0,0 +1,2 @@
+-r requirements.txt
+pyepics>=3.5

python/requirements.txt

@@ -0,0 +1 @@
+matplotlib>=3.7

python/viewer.py

@@ -0,0 +1,176 @@
+#!/usr/bin/env python3
+"""Generic six-joint arm: offline Cartesian/joint controls or read-only IOC view."""
+import argparse
+import ctypes as ct
+import json
+import math
+from pathlib import Path
+import time
+
+ROOT = Path(__file__).resolve().parents[1]
+HOME = [0., -35., 70., 0., 35., 0.]
+LOWER = [-170., -120., -140., -180., -120., -180.]
+UPPER = [170., 120., 140., 180., 120., 180.]
+
+class Model:
+    def __init__(self, geometry):
+        if len(geometry) != 4 or not all(math.isfinite(x) and x > 0 for x in geometry):
+            raise ValueError('Geometry requires four positive finite lengths in mm')
+        path = next((ROOT/'build'/('libarm.'+ext) for ext in ('so','dylib')
+                     if (ROOT/'build'/('libarm.'+ext)).exists()), None)
+        if path is None:
+            raise RuntimeError('Build the viewer library first: make viewer-lib')
+        self.lib = ct.CDLL(str(path))
+        ptr = ct.POINTER(ct.c_double)
+        self.lib.arm_forward.argtypes = [ptr]*4
+        self.lib.arm_forward.restype = ct.c_int
+        self.lib.arm_inverse.argtypes = [ptr]*5
+        self.lib.arm_inverse.restype = ct.c_int
+        self.geometry = (ct.c_double*4)(*geometry)
+
+    @staticmethod
+    def six(values):
+        if len(values) != 6 or not all(math.isfinite(x) for x in values):
+            raise ValueError('Expected six finite numbers')
+        return (ct.c_double*6)(*values)
+
+    def forward(self, joints):
+        pose, points = (ct.c_double*6)(), (ct.c_double*24)()
+        if self.lib.arm_forward(self.geometry, self.six(joints), pose, points):
+            raise ValueError('Invalid geometry or joint limits exceeded')
+        return list(pose), [list(points[i:i+3]) for i in range(0,24,3)]
+
+    def inverse(self, pose, seed):
+        joints, residual = (ct.c_double*6)(), ct.c_double()
+        status = self.lib.arm_inverse(self.geometry,self.six(pose),self.six(seed),joints,ct.byref(residual))
+        if status < 0:
+            raise ValueError('Invalid geometry, pose or seed')
+        return list(joints), residual.value, status == 0
+
+
+def self_test(model):
+    joints = [a+b for a,b in zip(HOME,[5,-4,3,2,-3,4])]
+    pose, points = model.forward(joints)
+    recovered, residual, ok = model.inverse(pose,HOME)
+    pose2, _ = model.forward(recovered)
+    if not ok or len(points) != 8 or max(abs(a-b) for a,b in zip(pose,pose2)) > 1e-4:
+        raise RuntimeError('Python/C++ round trip failed')
+    if model.inverse([10000,0,0,0,0,0],HOME)[2]:
+        raise RuntimeError('Unreachable pose accepted')
+    print(f'PASS: Python/C++ arm IK/FK; residual={residual:.3g}')
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument('--prefix',help='Read-only IOC view, e.g. MTEST04-MTN-ARM:')
+    parser.add_argument('--joints',action='store_true',help='Offline joint-space controls, home-relative degrees')
+    parser.add_argument('--geometry',type=Path,default=ROOT/'cfg'/'geometry.json')
+    parser.add_argument('--self-test',action='store_true')
+    parser.add_argument('--snapshot',type=Path,help='Save one offline view as PNG')
+    args = parser.parse_args()
+    if args.prefix and (args.joints or args.snapshot):
+        parser.error('--prefix cannot be combined with --joints or --snapshot')
+    model = Model(json.loads(args.geometry.read_text()))
+    if args.self_test:
+        self_test(model)
+        return
+    import matplotlib
+    if args.snapshot:
+        matplotlib.use('Agg')
+    import matplotlib.pyplot as plt
+    from matplotlib.animation import FuncAnimation
+    from matplotlib.widgets import Slider, Button
+    source = None
+    if args.prefix:
+        from epics_source import IocSource
+        source = IocSource(args.prefix)
+    fig = plt.figure(figsize=(12,9))
+    ax = fig.add_axes([.04,.32,.68,.61],projection='3d')
+    status = fig.text(.73,.85,'',family='monospace',fontsize=9,va='top')
+    fig.suptitle('ecmc 6-DOF arm — generic simulation (no collision checking)')
+    sliders = []
+    labels = [f'J{i+1} offset [deg]' for i in range(6)] if args.joints else ['X [mm]','Y [mm]','Z [mm]','Rx [deg]','Ry [deg]','Rz [deg]']
+    for i,label in enumerate(labels):
+        limit = 45 if args.joints else (50 if i<3 else 20)
+        slider = Slider(fig.add_axes([.16,.25-i*.035,.46,.02]),label,-limit,limit,valinit=0)
+        if source:
+            slider.set_active(False)
+        sliders.append(slider)
+    state = dict(current=[0.]*6,start=[0.]*6,target=[0.]*6,started=time.monotonic(),
+                 command=list(HOME),actual=list(HOME),pose=[0.]*6)
+    def move(_):
+        state.update(start=list(state['current']),target=[s.val for s in sliders],started=time.monotonic())
+    button = Button(fig.add_axes([.75,.12,.2,.05]),'Move (3 seconds)')
+    button.on_clicked(move)
+    if source:
+        button.set_active(False)
+        button.label.set_text('IOC monitor')
+    if args.snapshot:
+        state.update(current=[10,-5,5,3,-3,4],start=[10,-5,5,3,-3,4],target=[10,-5,5,3,-3,4])
+        for slider,value in zip(sliders,state['current']):
+            slider.set_val(value)
+    def update(_):
+        try:
+            fault, residual, ok = 0, 0., True
+            if source:
+                live = source.snapshot()
+                state['command'] = [h+v for h,v in zip(HOME,live['joint_set'])]
+                state['actual'] = [h+v for h,v in zip(HOME,live['joint_actual'])]
+                state['pose'] = list(live['pose_actual'])
+                desired_pose = live['pose_set']
+                residual, fault = live['residual'], live['fault']
+            else:
+                t = min(1.,(time.monotonic()-state['started'])/3.)
+                blend = t*t*t*(10+t*(-15+6*t))
+                state['current'] = [a+blend*(b-a) for a,b in zip(state['start'],state['target'])]
+                if args.joints:
+                    state['command'] = [h+v for h,v in zip(HOME,state['current'])]
+                    desired_pose, _ = model.forward(state['command'])
+                else:
+                    desired_pose = state['current']
+                    command,residual,ok = model.inverse(desired_pose,state['command'])
+                    if ok:
+                        state['command'] = command
+                # Offline ideal plant. There is no dynamics or drive model.
+                state['actual'] = list(state['command'])
+                state['pose'], _ = model.forward(state['actual'])
+            _, requested = model.forward(state['command'])
+            _, actual = model.forward(state['actual'])
+            ax.clear()
+            ax.plot(*zip(*requested),'o--',color='tab:orange',label='IK command',alpha=.7)
+            ax.plot(*zip(*actual),'o-',color='tab:blue',linewidth=3,label='Joint feedback')
+            for index,label in ((0,'J1'),(1,'J2'),(2,'J3'),(3,'J4/5/6')):
+                ax.text(*actual[index],label)
+            reach = sum(model.geometry)
+            ax.set(xlim=(-reach,reach),ylim=(-reach,reach),zlim=(0,reach),xlabel='X [mm]',ylabel='Y [mm]',zlabel='Z [mm]')
+            ax.set_box_aspect((2,2,1))
+            ax.legend(loc='upper left')
+            text = ('IOC read-only' if source else ('Offline joint mode' if args.joints else 'Offline Cartesian mode'))
+            text += f'\nIK: {"OK" if ok else "NO SOLUTION"}\nWeighted residual: {residual:.3g}\nIOC fault: {fault}\n'
+            text += '\nJoint offsets [deg]\n      IK set   Actual\n'
+            text += '\n'.join(f'J{i+1} {a-h:8.3f} {b-h:8.3f}' for i,(h,a,b) in enumerate(zip(HOME,state['command'],state['actual'])))
+            text += '\n\nTool offsets [mm / deg]\n       Set    Actual\n'
+            text += '\n'.join(f'{n:2} {a:8.3f} {b:8.3f}' for n,a,b in zip(('X','Y','Z','Rx','Ry','Rz'),desired_pose,state['pose']))
+            if not ok:
+                text += '\n\nHolding last valid joints.'
+            text += '\n\nNo collision checking.'
+            status.set_text(text)
+            status.set_color('black' if ok and fault==0 else 'red')
+        except (RuntimeError,ValueError,TypeError) as exc:
+            status.set_text(str(exc))
+            status.set_color('red')
+        return ()
+    if args.snapshot:
+        update(0)
+        args.snapshot.parent.mkdir(parents=True,exist_ok=True)
+        fig.savefig(args.snapshot,dpi=130)
+    else:
+        animation = FuncAnimation(fig,update,interval=50,cache_frame_data=False)
+        try:
+            plt.show()
+        finally:
+            if source:
+                source.close()
+
+if __name__ == '__main__':
+    main()

src/Makefile

@@ -0,0 +1,11 @@
+# Optional PSI driver build: make -C src build (on the IOC development host).
+include /ioc/tools/driver.makefile
+MODULE = main
+BUILDCLASSES = Linux
+ARCH_FILTER = deb10% deb12%
+EXCLUDE_VERSIONS += 3 7.0.5 7.0.6 7.0.7
+USR_CXXFLAGS += -std=c++17
+ecmc_VERSION = v11.2.0_RC1
+SOURCES = logic.cpp
+HEADERS = kinematics.hpp
+# The portable C API for the viewer is built separately by make viewer-lib.

src/c_api.cpp

@@ -0,0 +1,20 @@
+#include "kinematics.hpp"
+extern "C" {
+// Geometry: base height, upper-arm length, forearm length, tool length, all mm.
+int arm_forward(const double* geometry,const double* joints,double* pose,double* points) {
+  arm::Model m({geometry[0],geometry[1],geometry[2],geometry[3]}); arm::Six q{};
+  std::copy(joints,joints+6,q.begin());
+  if(!m.g.valid()||!m.withinLimits(q)) return -1;
+  std::array<arm::Vec,8> p{}; m.forward(q,&p); auto result=m.pose(q);
+  std::copy(result.begin(),result.end(),pose);
+  for(int i=0;i<8;++i) for(int j=0;j<3;++j) points[i*3+j]=p[i][j];
+  return 0;
+}
+int arm_inverse(const double* geometry,const double* pose,const double* seed,double* joints,double* residual) {
+  arm::Model m({geometry[0],geometry[1],geometry[2],geometry[3]}); arm::Six p{},q{};
+  std::copy(pose,pose+6,p.begin()); std::copy(seed,seed+6,q.begin());
+  if(!m.g.valid()||!arm::finite(p)||!m.withinLimits(q)) return -1;
+  auto result=m.inverse(p,q); std::copy(result.joints.begin(),result.joints.end(),joints); *residual=result.residual;
+  return result.converged?0:1;
+}
+}

src/kinematics.hpp

@@ -0,0 +1,120 @@
+#pragma once
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+namespace arm {
+constexpr double pi=3.14159265358979323846, rad=pi/180.;
+using Six=std::array<double,6>;
+using Vec=std::array<double,3>;
+using Mat=std::array<Vec,3>;
+inline bool finite(const Six& v) { for(double x:v) if(!std::isfinite(x)) return false; return true; }
+inline Mat identity() { return {{{1,0,0},{0,1,0},{0,0,1}}}; }
+inline Mat transpose(const Mat& a) { Mat b{}; for(int i=0;i<3;++i) for(int j=0;j<3;++j) b[i][j]=a[j][i]; return b; }
+inline Mat mul(const Mat& a,const Mat& b) { Mat c{}; for(int i=0;i<3;++i) for(int j=0;j<3;++j) for(int k=0;k<3;++k) c[i][j]+=a[i][k]*b[k][j]; return c; }
+inline Vec mul(const Mat& a,const Vec& b) { Vec c{}; for(int i=0;i<3;++i) for(int k=0;k<3;++k) c[i]+=a[i][k]*b[k]; return c; }
+inline Mat rotation(int axis,double angle) {
+  Mat m=identity(); const int i=(axis+1)%3,j=(axis+2)%3;
+  m[i][i]=m[j][j]=std::cos(angle); m[i][j]=-std::sin(angle); m[j][i]=std::sin(angle); return m;
+}
+struct Transform { Vec p{}; Mat r=identity(); };
+struct Geometry {
+  double baseHeight=300,upperArm=300,forearm=250,tool=80;
+  bool valid() const { for(double x:{baseHeight,upperArm,forearm,tool}) if(!std::isfinite(x)||x<=0) return false; return true; }
+};
+struct Result { Six joints{}; double residual=0; int iterations=0; bool converged=false; };
+inline bool solve(double a[6][6],double b[6],Six& x) {
+  for(int k=0;k<6;++k) {
+    int p=k; for(int i=k+1;i<6;++i) if(std::abs(a[i][k])>std::abs(a[p][k])) p=i;
+    if(std::abs(a[p][k])<1e-14) return false;
+    for(int j=k;j<6;++j) std::swap(a[k][j],a[p][j]);
+    std::swap(b[k],b[p]);
+    for(int i=k+1;i<6;++i) { const double f=a[i][k]/a[k][k]; for(int j=k;j<6;++j) a[i][j]-=f*a[k][j]; b[i]-=f*b[k]; }
+  }
+  for(int i=5;i>=0;--i) { double s=b[i]; for(int j=i+1;j<6;++j) s-=a[i][j]*x[j]; x[i]=s/a[i][i]; } return true;
+}
+// Rotation logarithm, including a stable branch near 180 degrees.
+inline Vec logRotation(const Mat& m) {
+  const double angle=std::acos(std::clamp((m[0][0]+m[1][1]+m[2][2]-1)*.5,-1.,1.));
+  Vec v{m[2][1]-m[1][2],m[0][2]-m[2][0],m[1][0]-m[0][1]};
+  if(angle<1e-7) { for(double& x:v) x*=.5; return v; }
+  if(pi-angle<1e-5) {
+    int k=0; for(int i=1;i<3;++i) if(m[i][i]>m[k][k]) k=i;
+    Vec axis{}; axis[k]=std::sqrt(std::max(0.,(m[k][k]+1)*.5));
+    for(int i=0;i<3;++i) if(i!=k) axis[i]=(m[i][k]+m[k][i])/(4*axis[k]);
+    double dot=0; for(int i=0;i<3;++i) dot+=axis[i]*v[i];
+    for(int i=0;i<3;++i) v[i]=axis[i]*angle*(dot<0?-1:1);
+    return v;
+  }
+  for(double& x:v) x*=angle/(2*std::sin(angle));
+  return v;
+}
+struct Model {
+  Geometry g;
+  // A generic elbow arm with a spherical X-Y-X wrist; all axes are revolute.
+  const Six home{0,-35,70,0,35,0};
+  const Six lower{-170,-120,-140,-180,-120,-180};
+  const Six upper{170,120,140,180,120,180};
+  explicit Model(Geometry geometry={}):g(geometry) {}
+  bool withinLimits(const Six& q) const { if(!finite(q)) return false; for(int i=0;i<6;++i) if(q[i]<lower[i]||q[i]>upper[i]) return false; return true; }
+  Transform forward(const Six& q,std::array<Vec,8>* points=nullptr) const {
+    Transform t; int n=0;
+    auto point=[&]() { if(points) (*points)[n]=t.p; ++n; };
+    auto move=[&](Vec local) { const Vec d=mul(t.r,local); for(int i=0;i<3;++i) t.p[i]+=d[i]; };
+    point(); t.r=mul(t.r,rotation(2,q[0]*rad)); move({0,0,g.baseHeight}); point();
+    t.r=mul(t.r,rotation(1,q[1]*rad)); move({g.upperArm,0,0}); point();
+    t.r=mul(t.r,rotation(1,q[2]*rad)); move({g.forearm,0,0}); point();
+    t.r=mul(t.r,rotation(0,q[3]*rad)); point();
+    t.r=mul(t.r,rotation(1,q[4]*rad)); point();
+    t.r=mul(t.r,rotation(0,q[5]*rad)); point();
+    move({g.tool,0,0}); point(); return t;
+  }
+  // Cartesian axes are base-frame offsets from the nominal home tool pose.
+  // R = Rz(Rz)*Ry(Ry)*Rx(Rx)*R_home; XYZ are added in the base frame.
+  Transform target(const Six& pose) const {
+    Transform t=forward(home);
+    for(int i=0;i<3;++i) t.p[i]+=pose[i];
+    t.r=mul(mul(mul(rotation(2,pose[5]*rad),rotation(1,pose[4]*rad)),rotation(0,pose[3]*rad)),t.r); return t;
+  }
+  Six pose(const Six& q) const {
+    const auto t=forward(q),h=forward(home); Six p{};
+    for(int i=0;i<3;++i) p[i]=t.p[i]-h.p[i];
+    const Mat r=mul(t.r,transpose(h.r));
+    p[4]=std::asin(std::clamp(-r[2][0],-1.,1.))/rad;
+    if(std::abs(std::cos(p[4]*rad))>1e-8) { p[3]=std::atan2(r[2][1],r[2][2])/rad; p[5]=std::atan2(r[1][0],r[0][0])/rad; }
+    else { p[3]=0; p[5]=std::atan2(-r[0][1],r[1][1])/rad; }
+    return p;
+  }
+  Six error(const Transform& desired,const Six& q) const {
+    auto t=forward(q); Six e{}; for(int i=0;i<3;++i) e[i]=desired.p[i]-t.p[i];
+    const Vec v=logRotation(mul(desired.r,transpose(t.r)));
+    for(int i=0;i<3;++i) e[i+3]=v[i]*200.; // orientation weight: 200 mm/radian
+    return e;
+  }
+  Result inverse(const Six& pose,const Six& seed,int maxIterations=100) const {
+    Result out; out.joints=seed;
+    if(!g.valid()||!finite(pose)||!withinLimits(seed)) return out;
+    const Transform desired=target(pose); double damping=.1;
+    auto cost=[](const Six& e) { double c=0; for(double v:e)c+=v*v; return c; };
+    for(int step=0;step<=maxIterations;++step) {
+      const Six e=error(desired,out.joints); out.residual=std::sqrt(cost(e)); out.iterations=step;
+      if(out.residual<1e-5) { out.converged=true; return out; }
+      if(step==maxIterations) break;
+      double j[6][6]{},a[6][6]{},b[6]{};
+      for(int k=0;k<6;++k) {
+        Six plus=out.joints,minus=out.joints; plus[k]+=1e-5/rad; minus[k]-=1e-5/rad;
+        const Six ep=error(desired,plus),em=error(desired,minus);
+        for(int i=0;i<6;++i) j[i][k]=-(ep[i]-em[i])/(2e-5);
+      }
+      for(int k=0;k<6;++k) { for(int i=0;i<6;++i) b[k]+=j[i][k]*e[i]; for(int l=0;l<6;++l) for(int i=0;i<6;++i) a[k][l]+=j[i][k]*j[i][l]; a[k][k]+=damping; }
+      Six delta{}; if(!solve(a,b,delta)) return out;
+      double largest=0; for(double v:delta) largest=std::max(largest,std::abs(v));
+      const double scale=largest>.2?.2/largest:1.; Six trial=out.joints;
+      for(int k=0;k<6;++k) trial[k]=std::clamp(trial[k]+scale*delta[k]/rad,lower[k],upper[k]);
+      if(cost(error(desired,trial))<cost(e)) { out.joints=trial; damping=std::max(1e-8,damping*.3); }
+      else damping=std::min(1e12,damping*10);
+    }
+    return out;
+  }
+};
+} // namespace arm

src/logic.cpp

@@ -0,0 +1,60 @@
+#include "ecmcCppLogic.hpp"
+#include "kinematics.hpp"
+#include <stdexcept>
+
+struct ArmLogic : ecmcCpp::LogicBase {
+  arm::Model model;
+  arm::Six pose_set{},pose_actual{},joint_set{},joint_actual{},joint_trajectory{};
+  arm::Six seed=model.home;
+  int32_t fault=0,ik_iterations=0,simulation=1;
+  double ik_residual=0;
+  int coordFirst=1,jointFirst=7;
+  ArmLogic() {
+    const auto macros=ecmcCpp::getMacrosString();
+    coordFirst=ecmcCpp::getMacroValueInt(macros,"COORD_FIRST",1);
+    jointFirst=ecmcCpp::getMacroValueInt(macros,"JOINT_FIRST",7);
+    simulation=ecmcCpp::getMacroValueInt(macros,"SIMULATION",1);
+    if(simulation!=1) throw std::runtime_error("Only SIMULATION=1 is supported");
+    if(coordFirst<1||jointFirst<1||std::abs(coordFirst-jointFirst)<6) throw std::runtime_error("Axis ranges must be positive and disjoint");
+    auto& g=model.g;
+    g.baseHeight=ecmcCpp::getMacroValueDouble(macros,"BASE_HEIGHT",g.baseHeight);
+    g.upperArm=ecmcCpp::getMacroValueDouble(macros,"UPPER_ARM",g.upperArm);
+    g.forearm=ecmcCpp::getMacroValueDouble(macros,"FOREARM",g.forearm);
+    g.tool=ecmcCpp::getMacroValueDouble(macros,"TOOL",g.tool);
+    if(!g.valid()) throw std::runtime_error("Invalid geometry");
+    for(int i=0;i<6;++i) {
+      ecmc.input("ax"+std::to_string(coordFirst+i)+".setpos",pose_set[i]);
+      ecmc.input("ax"+std::to_string(jointFirst+i)+".actpos",joint_actual[i]);
+      ecmc.input("ax"+std::to_string(jointFirst+i)+".setpos",joint_trajectory[i]);
+    }
+    epics.readOnlyArray("arm.pose_set",pose_set)
+      .readOnlyArray("arm.pose_actual",pose_actual)
+      .readOnlyArray("arm.joint_set",joint_set)
+      .readOnlyArray("arm.joint_actual",joint_actual)
+      .readOnly("arm.fault",fault)
+      .readOnly("arm.ik_iterations",ik_iterations)
+      .readOnly("arm.ik_residual",ik_residual)
+      .readOnly("arm.simulation",simulation);
+  }
+  void run() override {
+    if(fault) return;
+    for(int i=0;i<6;++i) if(ecmcCpp::axisHasError(coordFirst+i)||ecmcCpp::axisHasError(jointFirst+i)) { fault=4; return; }
+    if(!arm::finite(pose_set)||!arm::finite(joint_actual)||!arm::finite(joint_trajectory)) { fault=1; return; }
+    arm::Six actual{},trajectory{};
+    for(int i=0;i<6;++i) { actual[i]=joint_actual[i]+model.home[i]; trajectory[i]=joint_trajectory[i]+model.home[i]; }
+    if(!model.withinLimits(actual)||!model.withinLimits(trajectory)) { fault=1; return; }
+    // Previous accepted command selects a continuous local IK branch.
+    const auto result=model.inverse(pose_set,seed);
+    ik_iterations=result.iterations; ik_residual=result.residual;
+    if(!result.converged) { fault=2; return; }
+    seed=result.joints; pose_actual=model.pose(actual);
+    for(int i=0;i<6;++i) joint_set[i]=result.joints[i]-model.home[i];
+    for(int i=0;i<6;++i) {
+      int rc=ecmcCpp::axisSetExternalSetpointPos(jointFirst+i,joint_set[i]);
+      rc|=ecmcCpp::axisSetExternalEncoderPos(jointFirst+i,joint_trajectory[i]);
+      rc|=ecmcCpp::axisSetExternalEncoderPos(coordFirst+i,pose_actual[i]);
+      if(rc) { fault=3; return; }
+    }
+  }
+};
+ECMC_CPP_LOGIC_REGISTER_DEFAULT(ArmLogic)

tests/test_epics_source.py

@@ -0,0 +1,53 @@
+import sys
+import unittest
+from pathlib import Path
+sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'python'))
+from epics_source import IocSource
+
+
+class FakePV:
+    def __init__(self, name, auto_monitor, callback, connection_callback):
+        self.name, self.callback, self.connection = name, callback, connection_callback
+        assert auto_monitor
+    def send(self, value): self.callback(value=value)
+    def connect(self): self.connection(conn=True)
+    def disconnect(self): self.connection(conn=False)
+
+
+class SourceTest(unittest.TestCase):
+    def setUp(self):
+        self.source = IocSource('ARM:', FakePV)
+    def ready(self):
+        for key, pv in self.source.pvs.items():
+            pv.connect()
+            pv.send([1, 2, 3, 4, 5, 6] if key in IocSource.ARRAYS else 0)
+    def test_prefix_normalization(self):
+        source = IocSource("ARM", FakePV)
+        self.assertEqual(source.pvs["fault"].name, "ARM:arm-fault")
+    def test_mapping_and_updates(self):
+        self.ready()
+        self.assertEqual(self.source.pvs['pose_actual'].name, 'ARM:arm-pose-actual')
+        self.assertEqual(self.source.pvs['joint_set'].name, 'ARM:arm-joint-set')
+        old = self.source.snapshot()
+        self.source.pvs['pose_actual'].send([6]*6)
+        self.assertEqual(self.source.snapshot()['pose_actual'], (6,)*6)
+        self.assertEqual(old['pose_actual'], (1,2,3,4,5,6))
+    def test_disconnect_and_reconnect_waits_for_data(self):
+        self.ready()
+        pv = self.source.pvs['joint_actual']
+        pv.disconnect()
+        with self.assertRaisesRegex(RuntimeError, 'disconnected'): self.source.snapshot()
+        pv.connect()
+        with self.assertRaisesRegex(RuntimeError, 'Waiting'): self.source.snapshot()
+        pv.send([0]*6)
+        self.assertEqual(self.source.snapshot()['joint_actual'], (0,)*6)
+    def test_bad_array_and_recovery(self):
+        self.ready()
+        for bad in ([1]*5, [float('nan')]*6, None):
+            self.source.pvs['pose_set'].send(bad)
+            with self.assertRaisesRegex(RuntimeError, 'arm-pose-set'): self.source.snapshot()
+        self.source.pvs['pose_set'].send([0]*6)
+        self.source.pvs['fault'].send(2)
+        self.assertEqual(self.source.snapshot()['fault'], 2)
+
+if __name__ == '__main__': unittest.main()

tests/test_kinematics.cpp

@@ -0,0 +1,33 @@
+#include "kinematics.hpp"
+#include <iostream>
+#include <random>
+#include <stdexcept>
+void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
+int main() {
+  arm::Model m;
+  arm::Six zero{}; const auto home=m.pose(m.home);
+  for(double x:home) check(std::abs(x)<1e-10,"home pose offsets");
+  // Independent straight-arm and base-yaw geometry checks.
+  auto t=m.forward(zero); check(std::abs(t.p[0]-630)<1e-10 && t.p[2]==300,"straight arm dimensions");
+  zero[0]=90; t=m.forward(zero); check(std::abs(t.p[0])<1e-9 && std::abs(t.p[1]-630)<1e-9,"base rotation");
+  auto half=arm::logRotation(arm::rotation(0,arm::pi)); check(std::abs(std::abs(half[0])-arm::pi)<1e-8,"180 degree orientation error");
+  std::mt19937 rng(42); std::uniform_real_distribution<double> jitter(-8,8);
+  for(int sample=0;sample<500;++sample) {
+    auto q=m.home; for(double& v:q) v+=jitter(rng);
+    const auto pose=m.pose(q); auto r=m.inverse(pose,m.home);
+    check(r.converged && m.withinLimits(r.joints),"near-home IK round trip");
+    check(r.residual<1e-5,"round trip residual");
+  }
+  auto seed=m.home;
+  for(int i=0;i<1000;++i) {
+    auto q=m.home; for(int k=0;k<6;++k) q[k]+=10*std::sin(i*.005+k*.4);
+    auto r=m.inverse(m.pose(q),seed); check(r.converged,"continuous IK tracking"); seed=r.joints;
+  }
+  check(!m.inverse({10000,0,0,0,0,0},m.home).converged,"unreachable target");
+  auto bad=m.home; bad[0]=1000; check(!m.inverse({},bad).converged,"invalid seed");
+  arm::Geometry invalid; invalid.tool=-1; check(!arm::Model(invalid).inverse({},m.home).converged,"invalid geometry");
+  auto nan=arm::Six{}; nan[0]=NAN; check(!m.inverse(nan,m.home).converged,"nonfinite pose");
+  // Straight wrist is singular but holding its current pose must remain valid.
+  arm::Six singular{}; check(m.inverse(m.pose(singular),singular).converged,"singular pose hold");
+  std::cout<<"PASS: geometry, orientation, 500 IK round trips, 1000 tracking samples, invalid and unreachable inputs\n";
+}

tests/test_logic.cpp

@@ -0,0 +1,34 @@
+#include "../src/logic.cpp"
+#include <iostream>
+#include <stdexcept>
+std::array<double,13> setpoint{},encoder{};
+int badAxis=0,writeFailure=0;
+int32_t error(int32_t axis) { return axis==badAxis; }
+int32_t putSet(int32_t axis,double value) { setpoint.at(axis)=value; return writeFailure; }
+int32_t putEnc(int32_t axis,double value) { encoder.at(axis)=value; return writeFailure; }
+void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
+int main() {
+  ecmcCppLogicHostServices host{};
+  host.get_axis_error=error; host.set_axis_ext_set_pos=putSet; host.set_axis_ext_act_pos=putEnc;
+  ecmcCpp::setHostServices(&host);
+  ArmLogic logic; logic.run(); check(!logic.fault,"zero axes represent home");
+  // Verify real binding descriptors rather than relying on the mock to resolve names.
+  check(logic.ecmc.count()==18,"18 input bindings");
+  check(std::string(logic.ecmc.data()[0].itemName)=="ax1.setpos","coordinate asyn path");
+  check(std::string(logic.ecmc.data()[1].itemName)=="ax7.actpos","joint asyn path");
+  logic.pose_set={5,-4,8,2,-3,4}; logic.run(); check(!logic.fault,"IK cycle");
+  for(int i=0;i<6;++i) logic.joint_trajectory[i]=setpoint[i+7];
+  logic.run();
+  for(int i=0;i<6;++i) logic.joint_actual[i]=encoder[i+7];
+  logic.run(); check(!logic.fault,"FK cycle");
+  for(int i=0;i<6;++i) check(std::abs(encoder[i+1]-logic.pose_set[i])<1e-4,"coordinate FK feedback");
+  ArmLogic direct; direct.joint_trajectory={1,2,3,4,5,6}; direct.run();
+  for(int i=0;i<6;++i) { check(encoder[i+7]==i+1,"direct joint ideal plant"); direct.joint_actual[i]=encoder[i+7]; }
+  direct.run(); check(!direct.fault&&std::abs(direct.pose_actual[0])>.01,"direct joint FK");
+  const auto held=setpoint; logic.pose_set[0]=10000; logic.run(); check(logic.fault==2&&setpoint==held,"IK failure holds outputs");
+  logic.pose_set={}; logic.run(); check(logic.fault==2,"latched fault");
+  ArmLogic limits; limits.joint_trajectory[0]=200; limits.run(); check(limits.fault==1,"joint limit");
+  ArmLogic axis; badAxis=7; axis.run(); check(axis.fault==4,"axis error"); badAxis=0;
+  ArmLogic write; writeFailure=1; write.run(); check(write.fault==3,"host failure");
+  std::cout<<"PASS: input bindings, IK/FK pipeline, direct joint plant, latched faults\n";
+}