Skip to content

Commit 5db5c73

Browse files
author
Luca Toniolo
committed
Add comp_kins_uspace.h glue for .comp kinematics planner type 2 support
Provides macros (KINS_READ, COMP_KINS_BEGIN/END, COMP_KINS_NONRT_ATTACH) that let halcompile-based .comp kinematics modules work with the userspace trajectory planner without requiring conversion to hand-written .c files. Converts xyzab_tdr_kins.comp as reference example and adds documentation section to kinematics.adoc with the 5-step conversion recipe.
1 parent c8ffcb9 commit 5db5c73

File tree

3 files changed

+445
-13
lines changed

3 files changed

+445
-13
lines changed

docs/src/motion/kinematics.adoc

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)