231 lines
12 KiB
Markdown
231 lines
12 KiB
Markdown
# ecmc six-DOF robot arm — simulation prototype
|
|
|
|
|
|
|
|
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
|
|
|
|
|
|
|
|
```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.
|