# 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 libraries or Python GUI dependencies are needed by `make test`;
the Makefiles use the PSI `/ioc/tools` build infrastructure.

## 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
make pvs CPP_LOGIC_ROOT=/path/to/ecmccfg/examples/PSI/plugins/cpp_logic
make test-logic ECMC_ROOT=/path/to/ecmc
```

The Makefiles follow the hexapod project's PSI pattern: the top-level
Makefile includes `/ioc/tools/sls_ioc.makefile` and delegates `make build` to
`src/Makefile`, which includes `/ioc/tools/driver.makefile`. The module is `main`,
uses C++17, targets `deb10%` / `deb12%`, and requires `ecmc v11.2.0_RC1`.
Use the normal PSI build/install workflow to stage `bin/libmain.so`, together
with `cfg/` and the startup script. `make logic` is an alias for the IOC build.
`build` is phony so the local `build/` directory cannot suppress IOC compilation.

Custom PV substitutions are checked in as `cfg/libmain.so_cpp_logic.subs`.
`make pvs` regenerates them from `src/logic.cpp`; set `CPP_LOGIC_ROOT` to the
source tree's `examples/PSI/plugins/cpp_logic` directory. `make ui` uses the same
path to generate `qt/MTEST04-MTN-ARM_cpp_logic.ui`. The viewer's local C API
library remains a separate `make viewer-lib` target.

`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 inhibits new IK trajectory commands, but valid simulated joint encoder
and FK feedback continue following the selected joint trajectories. This 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

![Running ecmc arm IOC with caQtDM axis panels and the read-only 3D viewer](ecmc_arm_ioc_running.png)

```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-fault-axis` / `arm-fault-axis-error` | First faulting axis ID and its error code, captured for fault 4 |
| `arm-joint-trajectory` | Selected trajectory of each joint axis, home-relative degrees |
| `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.

### Tracing simulated feedback

During a small move, compare `arm-joint-set` (IK command),
`arm-joint-trajectory` (each joint's selected trajectory), and `arm-joint-actual`
(encoder feedback). In this ideal simulation the latter two should agree after
cyclic pipeline latency. A changing IK command with stationary selected
trajectories points to the axis/source/enable/state-machine path. A changing
selected trajectory with stationary encoder feedback points to the simulation
plant, encoder-source selection, or a stopped C++ logic instance. The joint
`Jn-PosSet` and `Jn-PosAct` records expose those same axis values.

`arm-fault-axis` and `arm-fault-axis-error` capture the first axis error seen by
the adapter, even if that axis error later clears. Feedback continues through
latched IK/axis faults when joint samples are finite and within model limits;
new IK commands remain inhibited until reload/restart. This allows actual
feedback to follow an axis stop trajectory instead of freezing on the fault.