@@ -815,4 +815,158 @@ RT `.so` to load and resolves `nonrt_attach` from it at startup.
815815 `maxkins.c` (dynamic params, not switchkins), `5axiskins.c`
816816 (switchkins with dynamic params).
817817
818+ [[sec:comp-kins-planner2]]
819+ === Adding planner type 2 support to .comp kinematics
820+
821+ Kinematics modules written as `.comp` files (compiled by `halcompile`)
822+ can be made compatible with the userspace planner (planner type 2) by
823+ using the `comp_kins_uspace.h` glue header. This avoids converting the
824+ module to a hand-written `.c` file while still providing the
825+ `nonrt_attach()` entry point and shared-memory parameter bridge that
826+ the planner requires.
827+
828+ ==== How it works
829+
830+ In the RT module, `kinematicsForward()` and `kinematicsInverse()` read
831+ parameters from HAL pins via `*(haldata->pin)`. When the userspace
832+ planner loads the same `.so` via `dlopen()`, the `haldata` pointer is
833+ `NULL` because the HAL pin infrastructure only exists in the RT instance.
834+
835+ The glue header solves this with a dual-path approach:
836+
837+ - *RT path* (`haldata` is set): reads HAL pins as normal, and mirrors
838+ each value into a `params.raw[]` slot in HAL shared memory so the
839+ userspace side can see it.
840+ - *Userspace path* (`haldata` is `NULL`): reads a consistent snapshot
841+ from the shared-memory `params.raw[]` array instead of touching HAL pins.
842+
843+ The `KINS_READ()` macro handles the switching automatically -- the
844+ ternary short-circuits so that the HAL pin expression is never evaluated
845+ when `haldata` is `NULL`.
846+
847+ ==== Conversion recipe
848+
849+ Five mechanical steps turn any `.comp` kinematics into a planner-2-aware
850+ module:
851+
852+ ===== Step 1: Include the glue header
853+
854+ Add one include after the `;;` line, alongside the existing kinematics
855+ headers:
856+
857+ [source,c]
858+ ----
859+ #include <rtapi_math.h>
860+ #include <kinematics.h>
861+ #include "comp_kins_uspace.h" // planner 2 glue
862+ ----
863+
864+ ===== Step 2: Register shared memory in setup
865+
866+ After `hal_ready()` in your setup function, call:
867+
868+ [source,c]
869+ ----
870+ comp_kins_uspace_setup(comp_id, "mykins", num_joints, "XYZAB");
871+ ----
872+
873+ Arguments: the HAL component ID, the module name (must match
874+ `[KINS]KINEMATICS` in the INI), the number of joints, and the
875+ coordinate letters.
876+
877+ ===== Step 3: Assign parameter indices
878+
879+ Create a parameter index map. Each HAL pin that is read inside
880+ `kinematicsForward()` or `kinematicsInverse()` gets a unique integer
881+ index into `params.raw[]` (up to 127 slots). For switchable kinematics,
882+ index 0 is reserved for the switch type.
883+
884+ [source,c]
885+ ----
886+ /*
887+ * Parameter index map for params.raw[]:
888+ * 0 = switchkins_type (reserved for switchable modules)
889+ * 1 = tool_offset_z
890+ * 2 = x_offset
891+ * 3 = z_offset
892+ * 4 = x_rot_point
893+ * 5 = y_rot_point
894+ * 6 = z_rot_point
895+ */
896+ ----
897+
898+ ===== Step 4: Modify kinematicsForward / kinematicsInverse
899+
900+ Replace each `*(haldata->pin)` read with `KINS_READ(haldata->pin, IDX)`
901+ and bracket the function body with `COMP_KINS_BEGIN` / `COMP_KINS_END`:
902+
903+ [source,c]
904+ ----
905+ int kinematicsForward(const double *j,
906+ EmcPose *pos,
907+ const KINEMATICS_FORWARD_FLAGS *fflags,
908+ KINEMATICS_INVERSE_FLAGS *iflags)
909+ {
910+ (void)fflags;
911+ (void)iflags;
912+ COMP_KINS_BEGIN(haldata); // <1>
913+
914+ double x_rot = KINS_READ(haldata->x_rot_point, 4); // <2>
915+ double y_rot = KINS_READ(haldata->y_rot_point, 5);
916+ double z_rot = KINS_READ(haldata->z_rot_point, 6);
917+ double dz = KINS_READ(haldata->z_offset, 3);
918+ double dt = KINS_READ(haldata->tool_offset_z, 1);
919+
920+ // ... kinematics math (unchanged) ...
921+
922+ COMP_KINS_END(); // <3>
923+ return 0;
924+ }
925+ ----
926+ <1> Snapshots shared memory in userspace; opens a write batch in RT.
927+ <2> Reads the HAL pin in RT (and pushes to shmem); reads from shmem in
928+ userspace. The index (second argument) must match the parameter map.
929+ <3> Closes the shmem write batch (sets `tail = head` in RT).
930+
931+ For switchable kinematics, replace `switch (switchkins_type)` with:
932+
933+ [source,c]
934+ ----
935+ int sw = _comp_uspace_loaded ? COMP_KINS_GET_SWITCH_TYPE()
936+ : (int)switchkins_type;
937+ switch (sw) {
938+ ----
939+
940+ And in `kinematicsSwitch()`, add:
941+
942+ [source,c]
943+ ----
944+ COMP_KINS_SET_SWITCH_TYPE(switchkins_type);
945+ ----
946+
947+ ===== Step 5: Add the nonrt_attach entry point
948+
949+ At the very end of the file, add one line:
950+
951+ [source,c]
952+ ----
953+ COMP_KINS_NONRT_ATTACH("mykins")
954+ ----
955+
956+ This generates the `nonrt_attach()` function and its `EXPORT_SYMBOL`.
957+ The module name must match the string used in `comp_kins_uspace_setup()`.
958+
959+ ==== Additional macros
960+
961+ For non-float HAL pin types, use `KINS_READ_S32()` and `KINS_READ_BIT()`
962+ instead of `KINS_READ()`. They store integer values as doubles in
963+ `params.raw[]` and cast back on read.
964+
965+ ==== Reference example
966+
967+ See `src/hal/components/xyzab_tdr_kins.comp` for a complete working
968+ example of a switchable `.comp` kinematics module converted using this
969+ approach. The lines marked with `+++` comments show all the additions
970+ relative to the original module.
971+
818972// vim: set syntax=asciidoc:
0 commit comments