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

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:

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:

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

.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.