Axes	Meaning	Trajectory source	Encoder source
1–6	X, Y, Z, Rx, Ry, Rz	Internal ecmc trajectories	External, from forward kinematics
7–12	Six leg strokes relative to home	External, from inverse kinematics	External, ideal simulated legs

The two axis ranges are configurable through COORD_FIRST and LEG_FIRST. cfg/axis_coordinate.yaml defines the coordinate axes and cfg/axis_leg.yaml defines the simulated legs. cfg/axes.cmd loads each shared template six times using loadYamlAxis.cmd with AX_ID, AX_NAME and AX_UNIT macros, following the PSI examples. IDs and names are assigned in cfg/axes.cmd; units are mm except Rx/Ry/Rz, which use deg. AX_UNIT defaults to mm. No motor hardware is linked.

Quick start: offline 3D view

cd /Users/sandst_a/sources/ecmc_hexapod
make test
python3 -m venv .venv
.venv/bin/pip install -r python/requirements.txt
.venv/bin/python python/viewer.py

Set the six sliders and click Move (3 seconds). The orange outline shows commanded pose; the blue platform is reconstructed by forward kinematics from simulated leg feedback. Offline movement uses one quintic interpolation parameter for all six coordinates, with zero endpoint velocity and acceleration. Retargeting mid-move starts a new profile and does not preserve velocity continuity. The offline leg model has simple first-order lag per display frame, not calibrated dynamics. Rotate the view with the mouse.

Both Python and the IOC adapter use src/kinematics.hpp; Python calls the compiled C interface, so there is no second implementation of the kinematic equations. The command-line tests need only C++17 and Python, not matplotlib or EPICS.

make test
make test-logic ECMC_ROOT=../ecmc
# Headless visualization check:
.venv/bin/python python/viewer.py --snapshot build/preview.png

Defining the geometry

The current model represents two flat plates with three pairs of joints on each plate. It is parameterized by radii, pair angles and nominal height. Geometry is measured between joint pivot centres, not plate edges, actuator housings or mounting bolt positions.

Use a right-handed base frame: origin at the centre of the base joint circle, base joints in the XY plane, +Z toward the platform. The platform frame has its origin at the centre of its joint circle and its own joints in its local XY plane. At home, the frames have parallel axes and the platform centre is at (0,0,H). The 60-degree offset between joint patterns is built into the attachment layout; it is not a 60-degree home yaw.

Parameters and where to set them

cfg/geometry.json is the viewer's seven-element array, in this exact order:

[200, 120, 250, 10, 10, 180, 400]

Index	C++ parameter	IOC load macro	Meaning	Default
0	`baseRadius`	`BASE_RADIUS`	Base centre to base joint centre	200 mm
1	`platformRadius`	`PLATFORM_RADIUS`	Platform centre to platform joint centre	120 mm
2	`height`	`HEIGHT`	Separation of joint planes at home	250 mm
3	`baseHalfAngle`	`BASE_HALF_ANGLE`	Half the angular separation of a base pair	10 deg
4	`platformHalfAngle`	`PLATFORM_HALF_ANGLE`	Half the angular separation of a platform pair	10 deg
5	`minLength`	`MIN_LENGTH`	Minimum total joint-centre-to-joint-centre leg length	180 mm
6	`maxLength`	`MAX_LENGTH`	Maximum total joint-centre-to-joint-centre leg length	400 mm

The last two values are total lengths, not axis displacement limits or actuator stroke. A joint assembly contributes to this total even if it does not telescope. All six legs currently share these limits.

For a measured distance d between the centres of two joints in a pair on a circle of radius r, the half-angle is asin(d/(2*r)), converted to degrees. It is half the included angle, not the full angle or half the chord length.

Set the IOC geometry in the MACROS argument of the existing C++ load line in cfg/load.cmd, for example:

${SCRIPTEXEC} ${ecmccfg_DIR}loadCppLogic.cmd, "DIR=./build/,FILE=libhexapod_logic.so,ASYN_PORT=HEX,REPORT=1,MACROS='COORD_FIRST=1,LEG_FIRST=7,SIMULATION=1,BASE_RADIUS=200,PLATFORM_RADIUS=120,HEIGHT=250,BASE_HALF_ANGLE=10,PLATFORM_HALF_ANGLE=10,MIN_LENGTH=180,MAX_LENGTH=400'"

Keep this macro configuration and cfg/geometry.json identical. The IOC does not read the JSON, and the viewer does not fetch geometry from the IOC. Restart the viewer and reload/restart the IOC after geometry changes. The axis YAML files configure the axes; they do not define the mechanical geometry.

Attachment numbering and leg connections

For zero-based leg index i, let sector = 120 * floor(i/2) degrees, base half-angle a and platform half-angle b. Attachment angles are:

                     even i                  odd i
base angle           sector - a              sector + a
platform angle       sector - 60 + b         sector + 60 - b

Using the default half-angles gives this explicit wiring table:

Leg	ecmc leg axis	Base angle	Platform-local angle
1	7	-10 deg	-50 deg
2	8	10 deg	50 deg
3	9	110 deg	70 deg
4	10	130 deg	170 deg
5	11	230 deg	190 deg
6	12	250 deg	290 deg

Angles increase counterclockwise when viewed from +Z toward the base. Leg i joins base point i to platform point i; preserve that association. Each base pair fans out to two neighbouring platform pairs. Upper close pairs are legs (2,3), (4,5), and (6,1), not (1,2), (3,4), (5,6). This is what avoids the original crossed-pair layout. Do not independently sort or swap the upper attachment points.

For base angle beta_i and platform angle gamma_i:

b_i = [Rb*cos(beta_i), Rb*sin(beta_i), 0]     # base frame
p_i = [Rp*cos(gamma_i), Rp*sin(gamma_i), 0]   # platform frame

Trigonometric functions use radians internally. Model::base and Model::top in src/kinematics.hpp contain these six point pairs.

Choosing and checking a geometry

Measure both radii and pair spacings at the pivot centres; calculate the half-angles.
Choose a home height that leaves useful actuator travel in both directions.
Enter actual minimum/maximum assembled leg lengths, including joints.
Verify all-zero pose gives the expected six total lengths and zero leg-axis strokes.
Exercise the intended poses and paths in the viewer, checking stroke, joint articulation and clearance. Endpoint feasibility alone does not validate a path.

The default symmetric home length is approximately 283.0721935 mm for every leg. With the example 180–400 mm total-length limits, each leg's home-relative stroke range is approximately -103.0722 to +116.9278 mm. These are derived individual leg bounds, not a rectangular Cartesian workspace.

The parameter validation checks finite positive radii/height, half-angles between 0 and 60 degrees and ordered positive length limits. It does not establish that the mechanism is well-conditioned. For example, a sum of half-angles equal to 60 degrees aligns each leg's base/platform azimuth at home and loses first-order yaw sensitivity. Avoid degenerate layouts and check conditioning over the intended workspace before using hardware.

Arbitrary measured XYZ attachment coordinates, unequal joint heights, per-leg length limits and calibrated encoder zero offsets are not currently loadable through JSON or macros. The mathematical equations below support arbitrary b_i and p_i, but using them requires extending the shared geometry loader and IOC/viewer configuration together. Do not approximate a substantially asymmetric mechanism by changing radii alone.

Kinematics mathematics

Pose and rotation convention

The six coordinate axes represent

q = [X, Y, Z, Rx, Ry, Rz]
t = [X, Y, H + Z]

Translations are in mm, exported angles in degrees. Z=0 is the nominal home height, not the base plane. A positive Z raises the platform. Define alpha = Rx*pi/180, beta = Ry*pi/180, gamma = Rz*pi/180 and use active, right-handed rotations acting on column vectors:

Rx(alpha) = [ 1     0          0     ]
            [ 0  cos(alpha) -sin(alpha)]
            [ 0  sin(alpha)  cos(alpha)]

Ry(beta)  = [ cos(beta)  0  sin(beta)]
            [    0      1     0    ]
            [-sin(beta) 0  cos(beta)]

Rz(gamma) = [cos(gamma) -sin(gamma) 0]
            [sin(gamma)  cos(gamma) 0]
            [    0           0     1]

R = Rz(gamma) * Ry(beta) * Rx(alpha)

Thus Rx acts first on a local attachment vector, then Ry, then Rz. Changing the order changes the resulting orientation. The platform rotates about its frame origin; translation then puts that origin at t.

Inverse kinematics: pose to six leg strokes

Transform each upper attachment into the base frame, subtract its lower attachment and take the Euclidean length:

u_i(q) = t + R*p_i                  # upper joint in base coordinates
v_i(q) = u_i(q) - b_i               # leg vector
L_i(q) = sqrt(v_ix^2 + v_iy^2 + v_iz^2)

This is a direct calculation, with no iterative solve. The six axis commands are strokes measured from the individual nominal home lengths:

L0_i = L_i([0,0,0,0,0,0])
s_i_command = L_i(q_command) - L0_i

At the all-zero pose all six stroke commands are zero. For this symmetric geometry, with delta = (60-a-b)*pi/180, home lengths can also be checked independently using the cosine rule:

L0 = sqrt(H^2 + Rb^2 + Rp^2 - 2*Rb*Rp*cos(delta))

The adapter checks every commanded total length against minLength and maxLength before publishing leg setpoints. A valid length does not by itself prove joint-angle or collision feasibility.

Forward kinematics: six measured strokes to pose

First convert the six encoder strokes back into measured total lengths:

L_i_measured = s_i_encoder + L0_i

We need a pose satisfying all six equations L_i(q) = L_i_measured at once. The implementation minimizes the sum of squared residuals:

r_i(q) = L_i_measured - L_i(q)
C(q) = sum_i r_i(q)^2

It starts from the previous valid pose (home at startup). To give angular and linear steps comparable magnitudes, define the diagonal scale matrix:

D = diag(1, 1, 1, 1/(Rp*pi/180), 1/(Rp*pi/180), 1/(Rp*pi/180))
q_trial = q + D*delta_u

A unit angular component of delta_u corresponds approximately to 1 mm of platform-edge movement. The solver estimates the Jacobian with central differences using h = 0.001 in these scaled coordinates:

J_ij ≈ [L_i(q + h*D*e_j) - L_i(q - h*D*e_j)] / (2*h)

Here e_j is a unit vector along coordinate j. Each iteration solves the damped normal equations for a six-component correction:

(J^T*J + lambda*I)*delta_u = J^T*r

The code uses Gaussian elimination with partial pivoting, rather than explicitly inverting a matrix. It limits the largest scaled step component to 20, evaluates the trial pose and accepts it only when C decreases. Damping starts at 1e-5; it is multiplied by 0.3 after an accepted step or by 10 after a rejected step, within bounds 1e-12 to 1e12.

Convergence means max(abs(r_i)) < 1e-7 mm. At most 30 correction attempts are made; the final residual is also checked after the last attempt. A rejected step still consumes an iteration. This tolerance is a numerical residual threshold, not a claim of achievable mechanical accuracy. Near a singularity, a small length residual can correspond to a large pose uncertainty.

On success, the six recovered coordinates feed the coordinate axes' external encoders. On failure, the current adapter latches fault 2 and retains the last valid feedback. It does not silently use the requested pose as measured feedback.

FK is a local solve: multiple assembly configurations can have the same six lengths. The seed selects the nearby branch, not necessarily the intended physical branch after a large jump or unknown startup position. The Euler representation also becomes singular near Ry=±90 degrees; this is separate from mechanical singularities. There is no Jacobian-conditioning, joint-angle or collision monitor in the current prototype.

ecmc integration

Build on the Linux IOC host against headers matching that IOC's ecmc build:

make logic ECMC_ROOT=../ecmc ECMCCFG_ROOT=../ecmccfg
make test-logic ECMC_ROOT=../ecmc

This produces build/libhexapod_logic.so and the generated EPICS substitutions. On macOS the build produces .dylib files for development tests; these cannot be loaded in a Linux IOC.

HEXAPD_startup.script is the simulation IOC startup, following the ecmccfg cpp_logic_motion example. It requires ecmccfg strucpp with ECMC_VER=strucpp, selects a 1 kHz cycle without an EtherCAT master, and loads all twelve virtual axes and the C++ logic through cfg/load.cmd. Build the library first using headers matching that ecmc installation, then run HEXAPD_startup.script with your normal PSI IOC launcher and this project as its working directory. The launcher supplies iocInit; ecmccfg finalizes the configuration and enters runtime during initialization. The default IOC name is HEXAPD unless the launcher sets IOC. Axes remain disabled at startup. Motor records are loaded for all twelve axes. The coordinate axes 1–6 belong to group virt; the simulated leg axes 7–12 belong to group phys. After loading the axes and C++ logic, cfg/load.cmd creates the built-in Hexapod_SM master/slave state machine with virt as masters and phys as slaves, using the default disable and timeout settings. The phys name does not select physical hardware.

To integrate into an existing ecmccfg IOC startup, with this project as its working directory, include the following after ecmccfg initialization and before runtime startup:

${SCRIPTEXEC} ./cfg/load.cmd

The include instantiates twelve axes from the two YAML templates through cfg/axes.cmd, then loads the C++ logic and custom status PVs. It is an integration fragment, not a complete site-specific IOC launcher. Each YAML file explicitly sets epics.motorRecord.enable: true and axis.autoEnable.atStartup: false, preserving the previous simulation setup. The YAML loader also creates the standard axis diagnostic/control PVs using the names X, Y, Z, Rx, Ry, Rz and L1–L6 (mm for translations/legs, deg for rotations). The YAML loader configures motor-record support through ecmccfg. Use the coordinate motor records for commanding axes 1–6. Direct leg moves on axes 7–12 require the master/slave state machine to select the legs' internal trajectories.

Warning: Use the virt coordinate axes for normal hexapod motion. Moving phys legs independently can produce an infeasible set of leg lengths, approach a singularity, or move into a different assembly configuration. Forward kinematics can fail or recover an unintended pose; the current simulation does not check joint clearance or collisions. Reserve direct leg moves for controlled simulation/commissioning checks. On real hardware, such moves could damage the mechanism. Axis IDs 1–12 must be free. No axis PLC may overwrite these external setpoints or encoder feeds.

In the inspected ecmc source, axes execute before C++ logic. The adapter reads coordinate trajectory positions and leg encoder positions from that cycle, then writes values consumed by the next axis cycle. This introduces pipeline latency; coordinate feedback also includes the inverse/plant/forward round trip.

The ideal leg plant copies each leg axis's selected trajectory position (axN.setpos) to its external encoder. This follows IK during coordinate moves and the internal leg trajectory during direct leg moves. The feedback is consumed on a subsequent axis cycle. The forward solver reads the axis encoder values, not the commanded pose, and writes its resulting six coordinates to the external coordinate encoders. A disabled virtual leg does not model a stopped physical actuator: the ideal plant is just a mathematical simulation, independent of drive-enable state.

The adapter does not yet provide a coordinated six-coordinate move command. Internal coordinate trajectories can be used for individual-axis experiments. For a specified Cartesian path, a later common-time trajectory generator must feed all six coordinate axes; independent point-to-point moves are not equivalent. The quintic profile in the offline viewer is not an IOC trajectory generator.

EPICS viewer

Start the IOC first, then run the viewer on a machine with a graphical display and network access to the IOC. From the project root, build the viewer's local C API library separately from the IOC's libmain.so and install the dependencies:

make viewer-lib
python3 -m venv .venv  # First-time setup; reuse an existing .venv if present.
.venv/bin/pip install -r python/requirements-ioc.txt

This creates build/libhexapod.so (.dylib on macOS) from src/c_api.cpp without EPICS dependencies. Rebuild it after changing the kinematics code.

For the IOC configured in MTEST04-MTN-HEXAPOD_parameters.yaml, start the live viewer with the IOC record prefix, including the trailing colon:

.venv/bin/python python/viewer.py --prefix MTEST04-MTN-HEXAPOD:

Use the prefix configured by loadCppLogic.cmd (DB_PREFIX, default ${IOC}:). If the IOC was started with the startup script's fallback name HEXAPD, use --prefix HEXAPD: instead. The prefix is the PV name prefix, not the IOC hostname. Pyepics needs an EPICS Channel Access library (libca) and network access to the IOC. If broadcast discovery does not reach the IOC, explicitly address the host configured in the parameters file (replace it for a different IOC):

EPICS_CA_AUTO_ADDR_LIST=NO EPICS_CA_ADDR_LIST=c6025a-08 \
  .venv/bin/python python/viewer.py --prefix MTEST04-MTN-HEXAPOD:

To check connectivity independently, run caget MTEST04-MTN-HEXAPOD:hex-pose-actual if EPICS command-line tools are available. It should return six coordinate values. If the viewer reports missing connections, check the prefix, IOC status and Channel Access network access.

IOC mode is read-only, with the offline motion controls disabled. It subscribes to these six PVs through pyepics monitors:

PV suffix	Display use
`hex-pose-set`	Orange commanded platform and coordinate setpoints
`hex-pose-actual`	Blue actual platform using the IOC's forward-kinematic result
`hex-leg-set`	Six leg stroke setpoints
`hex-leg-actual`	Six leg stroke readbacks
`hex-fault`	IOC fault status
`hex-fk-residual`	IOC forward-solver residual in mm

Both coordinate and leg values are shown as setpoint/readback pairs. Pose order is X, Y, Z, Rx, Ry, Rz (mm/degrees). Leg arrays are strokes relative to individual home lengths, in mm. The IOC computes the actual pose; live mode does not replace it with a separate Python FK solution or simulate leg feedback. C++ geometry is still used to draw the attachment positions, so the geometry JSON must match the IOC configuration.

CA callbacks update a thread-safe cache. Display refreshes never issue blocking PV reads. Missing connections, invalid values and pending initial samples are reported; the last drawing is retained with red status until valid data returns. Unchanging monitored values remain valid: this is not an IOC heartbeat monitor. The separate records are not an atomic snapshot of one real-time cycle.

Without --prefix, the standalone offline demo remains available. --snapshot is for offline rendering only. The monitor-cache tests run without pyepics or an IOC: python3 tests/test_epics_source.py.

Fault handling and current boundary

hex-fault is latched: 0 OK; 1 invalid pose or commanded stroke outside limits; 2 forward solution failed; 3 host write failed; 4 one of twelve axes reports error. On fault the adapter stops publishing new output values and retains the last accepted outputs. Fix the cause and restart/reload the simulation. This is not a controlled group stop: coordinate trajectories can continue, and a failed host write can leave some channels updated. Motor operation needs a separate coherent stop/interlock implementation and physical calibration.

The current adapter explicitly requires SIMULATION=1. Replacing leg axes with physical axes requires removing the ideal encoder feed, selecting real encoder sources, adding enable/group-stop logic and validating trajectory limits, calibration, branch handling and timing. The kinematic core can be reused unchanged. Velocity/acceleration limits in joint space are not implemented by the transform.

Can IK and FK run at 1 kHz?

Observed execution time on a Beckhoff C6025 with an Intel Core i7: approximately 50 µs while idle and 200 µs while moving. These are reported measurements, not worst-case execution-time bounds.

Yes, the measured computational cost makes 1 kHz plausible for this C++ implementation. A 1 kHz cycle is 1,000 microseconds, shared with all other ecmc, EtherCAT, motion and user-logic work. The actual real-time IOC must still be measured under its intended load; a standalone benchmark is not a deadline proof.

IK is a fixed-size calculation for six lengths. FK is more expensive because it iterates. Previous-pose seeding usually keeps consecutive tracking solves close to the solution. The solver uses fixed-size stack arrays, no heap allocation, no I/O and a fixed iteration cap. Those properties suit a cyclic task, although bounded iteration count alone does not establish worst-case execution time.

Reproducible numerical-core benchmark

Run on the target IOC CPU, with the same optimization flags as the logic module:

make bench
# To rebuild explicitly with selected compiler flags:
make -B bench CXXFLAGS='-O2 -std=c++17 -Wall -Wextra -pedantic'

bench/kinematics_bench.cpp measures one IK plus one FK per sample. Each case has 1,000 warm-up calls and 20,000 measured calls. Input poses are sampled along a smooth near-home trajectory with a 1 ms parameter increment; the benchmark executes as fast as possible, without sleeping or scheduling actual 1 ms periods. It consumes results to prevent removal of unused calculations and includes the per-sample clock overhead. Allocation and sorting of statistics occur outside the timed section.

Local results on 2026-10-22, macOS/Darwin arm64, Apple Clang 21.0.0, -O2:

IK + FK case	Mean	Median	p99	Observed maximum	FK iterations	Failed solves
Tracking, previous pose seed	1.406 us	1.416 us	1.500 us	17.959 us	2	0 / 20,000
Same trajectory, fresh home seed each time	1.936 us	1.917 us	2.042 us	48.833 us	3	0 / 20,000
Impossible length input, full iteration cap	21.132 us	21.000 us	25.333 us	106.959 us	30	20,000 / 20,000

The last case deliberately replaces leg 1's total length with 1,000 mm. It tests the nonconvergent path; it is not an acceptable working pose. All calls reached the iteration cap in this run. The benchmark repeatedly invokes the solver; the real adapter would latch a fault on its first failed solve.

These measurements exclude host-service calls, axis execution, binding copies, EPICS publication, EtherCAT and the rest of the IOC. They were taken on the development Mac, without real-time scheduling or CPU isolation; the exact CPU model was not recorded. The observed maximum is a finite-sample result, not a certified upper bound, and cannot be transferred directly to a different CPU. At the measured mean, normal tracking used about 0.14% of a 1 ms period and the 30-iteration case about 2.1%; the largest observed duration used about 10.7%.

Validating on the IOC

Run the core benchmark on the actual IOC host, then measure the complete C++ callback and the complete ecmc cycle at 1 kHz with the intended twelve axes.
Exercise startup, normal tracking, maximum intended velocity, boundary poses, noisy encoder input and deliberately failing FK. Count overruns and record iteration counts and high-percentile/maximum durations over a sustained run.
Include the real EtherCAT and other PLC/C++ logic load. Leave margin for the remaining cycle work and scheduling variation; do not budget the whole 1 ms for kinematics. Keep file I/O, printing and visualization outside the RT path.
Keep FK iteration count bounded and define a coherent group response to nonconvergence before commissioning physical motion. The current latched-output simulation fault handling is not yet that hardware response.

The current finite-difference Jacobian evaluates all six lengths twice per coordinate (12 evaluations), in addition to the current and trial pose. A full 30-attempt solve can therefore perform 421 six-length evaluations, including the last residual check. If target measurements require more margin, an analytic Jacobian and computing rotation/trigonometry once per pose are natural optimization steps. Changing iteration limits or tolerances needs convergence testing over the actual workspace; the current 1e-7 mm tolerance is much tighter than many encoder measurement resolutions.

The Python viewer is independent of the 1 kHz loop and refreshes at approximately 20 Hz. EPICS monitor publication can be decimated separately. Neither GUI refresh nor Channel Access delivery needs to run at 1 kHz for the kinematics to do so.

Verification

1,000 deterministic random pose → IK → FK round trips near home.
1,000 samples of a continuous trajectory using the previous FK result as seed.
Independent analytic home-length and rotation-convention checks.
Invalid geometry, non-finite input, negative length, iteration-limit and stroke checks.
C++ adapter tests with mocked ecmc host services: IK to leg axes, FK to coordinate encoders, and latched geometry/FK/axis/host faults.
Python/C ABI round trip and headless 3D rendering.

Live IOC execution times reported from the Beckhoff C6025 are recorded above; sustained real-time deadline and overrun testing remains outstanding.