diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 5c5139059e4..b4d92205c25 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -860,8 +860,13 @@ Finally, no amount of tweaking will speed up a tool path with lots of small, tig * `MAX_LINEAR_VELOCITY = 5.0` - (((MAX VELOCITY))) The maximum velocity for any axis or coordinated move, in 'machine units' per second. The value shown equals 300 units per minute. * `MAX_LINEAR_ACCELERATION = 20.0` - (((MAX ACCELERATION))) The maximum acceleration for any axis or coordinated axis move, in 'machine units' per second per second. -* `PLANNER_TYPE = 0` - (((PLANNER TYPE))) Selects the trajectory planner type: 0 = trapezoidal (default), 1 = S-curve with jerk limiting. - S-curve planning is only active when `PLANNER_TYPE = 1` AND `MAX_LINEAR_JERK > 0`. +* `PLANNER_TYPE = 0` - (((PLANNER TYPE))) Selects the trajectory planner type: + ** 0 = trapezoidal velocity profile (default) + ** 1 = S-curve with jerk limiting + ** 2 = 9D jerk-limited planner with Ruckig library (see <>) ++ +S-curve planning (type 1) is only active when `PLANNER_TYPE = 1` AND `MAX_LINEAR_JERK > 0`. +The 9D planner (type 2) provides time-optimal jerk-limited trajectories with responsive feed override handling. * `MAX_LINEAR_JERK = 10000.0` - (((MAX JERK))) The maximum jerk (rate of change of acceleration) for coordinated moves, in 'machine units' per second cubed. Default is 1e9 (1 billion) if not specified, which effectively disables jerk limiting while avoiding numerical instability. Values are clamped to a maximum of 1e9 to prevent numerical issues in S-curve calculations. @@ -896,6 +901,101 @@ LinuxCNC will not know your joint travel limits when using `NO_FORCE_HOMING = 1` * `NO_PROBE_JOG_ERROR = 0` - Allow to bypass probe tripped check when you jog manually. * `NO_PROBE_HOME_ERROR = 0` - Allow to bypass probe tripped check when homing is in progress. +[[sub:ini:sec:traj:9d-planner]] +==== 9D Jerk-Limited Planner (PLANNER_TYPE = 2) + +The 9D planner is an advanced trajectory planning system that uses the Ruckig library +for time-optimal, jerk-limited motion profiles. It features a dual-layer architecture +with userspace planning and real-time execution, providing responsive feed override +handling without motion discontinuities. + +[NOTE] +==== +The 9D planner is an experimental feature. Use `PLANNER_TYPE = 2` to enable it. +This planner is recommended for machines that require smooth motion with jerk limiting, +and responsive feed override changes during motion. +==== + +===== Basic 9D Planner Parameters + +* `PLANNER_TYPE = 2` - Selects the 9D jerk-limited trajectory planner. + This enables the Ruckig-based motion planning with userspace optimization. +* `OPTIMIZATION_DEPTH = 8` - Number of segments to look ahead for velocity optimization (range: 4-200, default: 8). + Higher values allow the planner to achieve higher velocities on longer paths + by planning deceleration further in advance. +* `RAMP_FREQUENCY = 10.0` - Cutoff frequency in Hz for ramped velocity mode (range: 1.0-1000.0, default: 10.0). + Segments shorter than 1/RAMP_FREQUENCY seconds use constant acceleration instead + of trapezoidal profiles to reduce jerk on very short moves. +* `SMOOTHING_PASSES = 2` - Number of velocity smoothing passes (range: 1-10, default: 2). + Additional passes reduce velocity peaks but may slightly reduce maximum throughput. +* `TC_QUEUE_SIZE = 50` - Size of the trajectory queue (range: 32-400, default: 50). + Larger queues allow more look-ahead but use more memory. +===== Predictive Feed Override Handoff Parameters + +These parameters control the branch/merge architecture for handling feed override changes. +When the operator adjusts the feed override, the planner computes a new trajectory +branch in userspace and hands it off to the real-time layer at a predicted future time. +This allows smooth velocity transitions without motion discontinuities. + +* `HANDOFF_HORIZON_MS = 100` - How far ahead (in milliseconds) to predict the handoff point (range: 1-1000, default: 100). + This value must exceed the worst-case userspace latency to ensure the new trajectory + is ready before the real-time layer reaches the handoff point. + Typical values: 50-200ms depending on system load. +* `BRANCH_WINDOW_MS = 50` - Window size (in milliseconds) for RT to accept the branch (range: 10-500, default: 50). + The real-time layer must reach the handoff point within this window after HANDOFF_HORIZON_MS + to accept the new trajectory. If the window expires, the branch is discarded. +* `MIN_BUFFER_TIME_MS = 100` - Minimum buffered motion time before alarm (range: 10-1000, default: 100). + If the motion buffer drops below this value, a warning is generated. + Very short buffer times risk motion starvation. +* `TARGET_BUFFER_TIME_MS = 200` - Target buffered motion time (range: MIN_BUFFER_TIME_MS-2000, default: 200). + The optimizer aims to maintain at least this much motion in the buffer. +* `MAX_BUFFER_TIME_MS = 500` - Maximum buffered motion time (range: TARGET_BUFFER_TIME_MS-5000, default: 500). + The optimizer stops adding new segments when the buffer exceeds this value. +* `FEED_OVERRIDE_DEBOUNCE_MS = 50` - Minimum time between branch computations (range: 1-500, default: 50). + When the operator rapidly changes the feed override (e.g., with a rotary knob), + this debounce prevents excessive recomputation. After computing a branch, + the planner waits this long before computing another. + +===== Example 9D Planner Configuration + +[source,{ini}] +---- +[TRAJ] +# Basic trajectory settings +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +MAX_LINEAR_VELOCITY = 100.0 +MAX_LINEAR_ACCELERATION = 2000.0 +MAX_LINEAR_JERK = 50000.0 + +# Enable 9D planner +PLANNER_TYPE = 2 + +# 9D planner tuning +OPTIMIZATION_DEPTH = 16 +RAMP_FREQUENCY = 20.0 +SMOOTHING_PASSES = 2 +TC_QUEUE_SIZE = 100 + +# Predictive handoff tuning (optional - defaults work well for most systems) +# HANDOFF_HORIZON_MS = 100 +# BRANCH_WINDOW_MS = 50 +# MIN_BUFFER_TIME_MS = 100 +# TARGET_BUFFER_TIME_MS = 200 +# MAX_BUFFER_TIME_MS = 500 +# FEED_OVERRIDE_DEBOUNCE_MS = 50 +---- + +[NOTE] +==== +The 9D planner automatically uses the servo cycle time from `[EMCMOT]SERVO_PERIOD` +and jerk limits from `MAX_LINEAR_JERK`. If `MAX_LINEAR_JERK` is not specified, +a default of 50000 (units per second cubed) is used. + +Per-joint jerk limits can also be specified in `[JOINT_N]MAX_JERK` sections. +The planner uses the most restrictive limit for each move. +==== + [[sub:ini:sec:kins]] === [KINS] Section(((INI File,Sections,KINS Section))) diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..714d253decd 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -295,7 +295,7 @@ Implements the inverse kinematics function. KINEMATICS_TYPE kinematicsType(void) ---- -Returns the kinematics type identifier, típicamente 'KINEMATICS_BOTH': +Returns the kinematics type identifier, typically 'KINEMATICS_BOTH': . KINEMATICS_IDENTITY (each joint number corresponds to an axis letter) . KINEMATICS_BOTH (forward and inverse kinematics functions are provided) @@ -361,4 +361,396 @@ modifications are somewhat more complicated. See 'millturn.comp' as an example of a switchable kinematic module that was created using the 'userkins.comp' template. +[[sec:writing-custom-kinematics]] +== Writing a Custom Kinematics Module + +This section provides a complete guide to writing a kinematics module +that works with both the real-time motion controller and the userspace +trajectory planner (planner type 2). + +=== Architecture overview + +A kinematics module in LinuxCNC serves two consumers: + +. The *real-time motion controller* (motmod) -- runs the kinematics + every servo cycle to convert between joint and Cartesian space. + This code has access to HAL pins and runs in the RTAPI environment. +. The *userspace trajectory planner* (milltask, planner type 2) -- + calls kinematics during path planning to compute blending, velocity + limits, and look-ahead. This code runs in normal userspace and + cannot access HAL pins directly. + +Both sets of functions live in the *same `.c` source file* and are +compiled into the *same RT `.so` module*. The userspace planner loads +the RT `.so` via `dlopen()` and resolves the non-RT symbols via +`dlsym()`. + +The key insight is that the *math is shared*: write your forward and +inverse kinematics as static helper functions that take plain +parameters (doubles, structs), then call those helpers from both the +RT interface (which reads HAL pins) and the non-RT interface (which +reads from a `kinematics_params_t` structure). + +=== Required functions + +Every kinematics module must export *two sets* of functions: + +==== RT functions (for motmod) + +These are the standard LinuxCNC kinematics functions documented above: +`kinematicsForward()`, `kinematicsInverse()`, `kinematicsType()`, +and either `KINS_NOT_SWITCHABLE` or the switchkins functions. +Additionally, `rtapi_app_main()` and `rtapi_app_exit()` handle +module initialization and cleanup. + +==== Non-RT functions (for the userspace planner) + +The userspace planner resolves these four symbols from the RT `.so`: + +---- +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos); +---- + +Forward kinematics. The `params` argument is a pointer to a +`kinematics_params_t` structure (see below) that contains all the +parameters the math needs. Returns 0 on success, -1 on failure. + +---- +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints); +---- + +Inverse kinematics. Same parameter convention. + +---- +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)); +---- + +Called periodically by the userspace planner to update the +kinematics parameters from HAL pin values. The three callback +functions read HAL pins by name. Use them to populate the +module-specific fields in `kinematics_params_t`. Returns 0 on +success. + +For modules with no runtime parameters (e.g., trivkins, corexykins), +this function is a no-op that returns 0. + +---- +int nonrt_is_identity(void); +---- + +Returns 1 if this module implements identity kinematics (each +joint maps directly to one axis), 0 otherwise. The planner +uses this to enable optimized code paths. + +All four functions must be exported with `EXPORT_SYMBOL()`: + +---- +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); +---- + +=== The `kinematics_params_t` structure + +The non-RT functions receive their parameters through a +`kinematics_params_t` structure defined in +`src/emc/motion/kinematics_params.h`. This structure contains: + +- *`module_name`* -- the module name string (e.g., "maxkins") +- *`coordinates`* -- the coordinates string (e.g., "XYZBC") +- *`num_joints`* -- number of configured joints +- *`joint_to_axis[]`* -- maps joint number to axis index (0=X through 8=W) +- *`axis_to_joint[]`* -- maps axis index to its principal joint number +- *`params`* -- a union of module-specific parameter structs + +The `params` union has a member for each built-in kinematics module. +When writing a custom module, add a new struct typedef and a +corresponding member to this union. + +==== Adding parameters for a custom module + +. Define a parameter struct in `kinematics_params.h`: ++ +---- +/* Parameters for mykins */ +typedef struct { + double my_param_a; + double my_param_b; + int my_flag; +} kins_mykins_params_t; +---- + +. Add the struct to the `params` union inside `kinematics_params_t`: ++ +---- +union { + kins_5axis_params_t fiveaxis; + kins_trt_params_t trt; + /* ... existing members ... */ + kins_mykins_params_t mykins; /* <-- add this */ + double raw[128]; +} params; +---- + +=== Complete example: a parametric 2-axis kinematics module + +The following example implements a kinematics module for a machine +with an arm length parameter. It demonstrates all three layers: +the math, the RT interface, and the non-RT interface. + +[source,c] +---- +/******************************************************************** + * mykins.c -- Example custom kinematics module + * License: GPL Version 2 + ********************************************************************/ + +/* ---- RT headers ---- */ +#include "kinematics.h" +#include "hal.h" +#include "rtapi.h" +#include "rtapi_app.h" +#include "rtapi_math.h" + +/* ================================================================ + * Layer 1: Pure math (shared by RT and non-RT) + * + * These functions take plain parameters, no HAL, no globals. + * ================================================================ */ + +static int mykins_forward(double arm_length, + const double *joints, + EmcPose *pos) +{ + /* Example: polar-to-cartesian with joint[0]=angle, joint[1]=radius */ + double angle_rad = joints[0] * (PM_PI / 180.0); + double r = joints[1] + arm_length; + pos->tran.x = r * cos(angle_rad); + pos->tran.y = r * sin(angle_rad); + pos->tran.z = joints[2]; + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + return 0; +} + +static int mykins_inverse(double arm_length, + const EmcPose *pos, + double *joints) +{ + double r = sqrt(pos->tran.x * pos->tran.x + + pos->tran.y * pos->tran.y); + if (r < 1e-10) return -1; /* singularity at origin */ + joints[0] = atan2(pos->tran.y, pos->tran.x) * (180.0 / PM_PI); + joints[1] = r - arm_length; + joints[2] = pos->tran.z; + return 0; +} + +/* ================================================================ + * Layer 2: RT interface (reads HAL pins) + * ================================================================ */ + +static struct haldata { + hal_float_t *arm_length; +} *haldata; + +int kinematicsForward(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)fflags; (void)iflags; + return mykins_forward(*(haldata->arm_length), joints, pos); +} + +int kinematicsInverse(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)iflags; (void)fflags; + return mykins_inverse(*(haldata->arm_length), pos, joints); +} + +KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } + +const char *kinematicsGetName(void) { return "mykins"; } + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); +MODULE_LICENSE("GPL"); + +static int comp_id; + +int rtapi_app_main(void) +{ + comp_id = hal_init("mykins"); + if (comp_id < 0) return comp_id; + + haldata = hal_malloc(sizeof(struct haldata)); + if (hal_pin_float_new("mykins.arm-length", HAL_IO, + &(haldata->arm_length), comp_id) < 0) + goto error; + + *(haldata->arm_length) = 100.0; /* default value */ + hal_ready(comp_id); + return 0; +error: + hal_exit(comp_id); + return -1; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ================================================================ + * Layer 3: Non-RT interface (for userspace trajectory planner) + * + * These functions are resolved via dlsym() by the planner. + * They call the same math as the RT side but get parameters + * from kinematics_params_t instead of HAL pins. + * ================================================================ */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + return mykins_forward(kp->params.mykins.arm_length, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + return mykins_inverse(kp->params.mykins.arm_length, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_bit; (void)read_s32; + + /* Read the HAL pin value into our params struct. + * The pin name must match what rtapi_app_main() created. */ + if (read_float("mykins.arm-length", + &kp->params.mykins.arm_length) != 0) + return -1; + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); +---- + +=== Structure of the code + +The example above shows the three-layer pattern that all built-in +kinematics modules follow: + +. **Layer 1 -- Pure math**: Static functions that take plain C + parameters (doubles, arrays) and compute the kinematics. These + functions have no dependencies on HAL, RTAPI, or any global state. + Both the RT and non-RT interfaces call them. + +. **Layer 2 -- RT interface**: The standard `kinematicsForward()` / + `kinematicsInverse()` functions. They read HAL pin values and pass + them to the Layer 1 math. Also includes `rtapi_app_main()` which + creates the HAL component and pins. + +. **Layer 3 -- Non-RT interface**: The `nonrt_*` functions. They + read from the `kinematics_params_t` structure instead of HAL pins, + and call the same Layer 1 math. The `nonrt_refresh()` function + uses the provided callbacks to copy current HAL pin values into + the params structure. + +This three-layer pattern ensures: + +- The math is written once and tested once +- The RT module works identically whether or not planner 2 is in use +- The userspace planner gets the same kinematics results as the + real-time controller + +=== Adding the module to the build system + +For an in-tree kinematics module, add a build rule in +`src/Makefile`. For a standalone module named `mykins`: + +---- +obj-m += mykins.so +mykins-objs := emc/kinematics/mykins.o +---- + +The module will be compiled to `rtlib/mykins.so`. + +For an out-of-tree module, the standard `halcompile --install` +method works for the RT side. However, to support the non-RT +interface, the module must be compiled as a shared library (`.so`) +and placed in the `rtlib/` directory where the planner can find it. + +=== INI file configuration + +Configure your kinematics module in the INI file: + +---- +[KINS] +KINEMATICS = mykins +JOINTS = 3 +---- + +For planner type 2, also set: + +---- +[TRAJ] +PLANNER_TYPE = 2 +---- + +The userspace planner reads `[KINS]KINEMATICS` to determine which +RT `.so` to load and will automatically resolve the `nonrt_*` +symbols from it. + +=== Tips + +- Keep the math functions *pure*: no global state, no HAL access, + no RTAPI calls. This makes them testable and reusable. +- Use `rtapi_math.h` (not `math.h`) for math functions in the RT + sections. The non-RT sections (after `#include "kinematics_params.h"`) + can use standard C headers since they only run in userspace, but + it is simplest to use `rtapi_math.h` throughout. +- Use `rtapi_snprintf()` instead of `snprintf()` in RT code. + Standard C library functions are not available in the RT + environment. +- The `nonrt_refresh()` pin names must exactly match the HAL pin + names created in `rtapi_app_main()`. +- For modules with no runtime parameters (pure geometric transforms + with fixed joint mappings), `nonrt_refresh()` can be a no-op that + returns 0. +- For switchkins modules that link with `switchkins.o`, do *not* + include `rtapi_app.h` in your kinematics source -- `switchkins.c` + already provides `rtapi_app_main()` and `rtapi_app_exit()`. Put + the non-RT interface section after all RT code with its own + `#include "kinematics_params.h"`. +- Look at the existing modules in `src/emc/kinematics/` for + real-world examples: `trivkins.c` (identity, no params), + `maxkins.c` (simple with params), `xyzac-trt-kins.c` (switchkins + with params). + // vim: set syntax=asciidoc: diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index f92bf28a782..23e14cd614a 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -354,6 +354,14 @@ Preempt-rt make example: $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid ---- +[NOTE] +When using planner type 2, the parent switchkins module (e.g., +xyzac-trt-kins) must also export `nonrt_*` functions for the +userspace trajectory planner. All built-in switchkins modules +already include these exports. See the +<> chapter for details on the non-RT +interface required by the userspace planner. + == Warnings Unexpected behavior can result if a G-code program is inadvertently diff --git a/src/Makefile b/src/Makefile index 509b12248de..236365c5014 100644 --- a/src/Makefile +++ b/src/Makefile @@ -182,7 +182,7 @@ SUBDIRS := \ emc/usr_intf/gremlin emc/usr_intf/gscreen emc/usr_intf/pyui emc/usr_intf/qtvcp \ emc/usr_intf/gmoccapy emc/usr_intf/qtplasmac emc/usr_intf/mdro\ emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/tp emc/canterp \ - emc/motion emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ + emc/motion emc/motion_planning emc/kinematics_userspace emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ emc \ @@ -232,7 +232,9 @@ USERSRCS += $(PYSRCS) CUSERSRCS := $(filter %.c,$(USERSRCS)) CXXUSERSRCS := $(filter %.cc %.cpp,$(USERSRCS)) CUSEROBJS := $(call TOOBJS,$(CUSERSRCS)) -CXXUSEROBJS += $(call TOOBJS,$(CXXUSERSRCS)) +CXXUSEROBJS_CC := $(patsubst %.cc,objects/%.o,$(filter %.cc,$(CXXUSERSRCS))) +CXXUSEROBJS_CPP := $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(CXXUSERSRCS))) +CXXUSEROBJS += $(CXXUSEROBJS_CC) $(CXXUSEROBJS_CPP) ifeq ($(TRIVIAL_BUILD),no) # Find the dependency filenames, then include them all @@ -279,27 +281,7 @@ $(sort $(CUSEROBJS)) : objects/%.o: %.c -MP -MD -MF "${@:.o=.d}" -MT "$@" \ $< -o $@ -# Ruckig files need C++20 -RUCKIG_CXX_FILES := $(filter emc/tp/ruckig%,$(CXXUSERSRCS)) -NON_RUCKIG_CXX_FILES := $(filter-out emc/tp/ruckig%,$(CXXUSERSRCS)) - -$(sort $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(RUCKIG_CXX_FILES)))) : objects/%.o: %.cpp - $(ECHO) Compiling $< - @mkdir -p $(dir $@) - @rm -f $@ - $(Q)$(CXX) -c $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 \ - -MP -MD -MF "${@:.o=.d}" -MT "$@" \ - $< -o $@ - -$(sort $(patsubst %.cc,objects/%.o,$(filter %.cc,$(RUCKIG_CXX_FILES)))) : objects/%.o: %.cc - $(ECHO) Compiling $< - @mkdir -p $(dir $@) - @rm -f $@ - $(Q)$(CXX) -c $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 \ - -MP -MD -MF "${@:.o=.d}" -MT "$@" \ - $< -o $@ - -$(sort $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(NON_RUCKIG_CXX_FILES)))) : objects/%.o: %.cpp +$(sort $(CXXUSEROBJS_CC)) : objects/%.o: %.cc $(ECHO) Compiling $< @mkdir -p $(dir $@) @rm -f $@ @@ -307,7 +289,7 @@ $(sort $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(NON_RUCKIG_CXX_FILES)))) : -MP -MD -MF "${@:.o=.d}" -MT "$@" \ $< -o $@ -$(sort $(patsubst %.cc,objects/%.o,$(filter %.cc,$(NON_RUCKIG_CXX_FILES)))) : objects/%.o: %.cc +$(sort $(CXXUSEROBJS_CPP)) : objects/%.o: %.cpp $(ECHO) Compiling $< @mkdir -p $(dir $@) @rm -f $@ @@ -320,19 +302,11 @@ $(sort $(patsubst %.c,%.i,$(CUSERSRCS))): %.i: %.c $(ECHO) Preprocessing $< to $@ $(Q)$(CC) -dD $(CFLAGS) $(EXTRAFLAGS) -E $< -o $@ -$(sort $(patsubst %.cc,%.ii,$(filter %.cc,$(RUCKIG_CXX_FILES)))): %.ii: %.cc - $(ECHO) Preprocessing $< to $@ - $(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 -E $< -o $@ - -$(sort $(patsubst %.cpp,%.ii,$(filter %.cpp,$(RUCKIG_CXX_FILES)))): %.ii: %.cpp - $(ECHO) Preprocessing $< to $@ - $(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 -E $< -o $@ - -$(sort $(patsubst %.cc,%.ii,$(filter %.cc,$(NON_RUCKIG_CXX_FILES)))): %.ii: %.cc +$(sort $(patsubst %.cc,%.ii,$(filter %.cc,$(CXXUSERSRCS)))): %.ii: %.cc $(ECHO) Preprocessing $< to $@ $(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -E $< -o $@ -$(sort $(patsubst %.cpp,%.ii,$(filter %.cpp,$(NON_RUCKIG_CXX_FILES)))): %.ii: %.cpp +$(sort $(patsubst %.cpp,%.ii,$(filter %.cpp,$(CXXUSERSRCS)))): %.ii: %.cpp $(ECHO) Preprocessing $< to $@ $(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -E $< -o $@ @@ -1299,6 +1273,7 @@ tpmod-objs += emc/nml_intf/emcpose.o tpmod-objs += libnml/posemath/_posemath.o tpmod-objs += emc/tp/sp_scurve.o tpmod-objs += emc/tp/ruckig_wrapper.o +tpmod-objs += emc/tp/bezier9_rt.o tpmod-objs += emc/tp/ruckig/src/ruckig/brake.o tpmod-objs += emc/tp/ruckig/src/ruckig/position_first_step1.o tpmod-objs += emc/tp/ruckig/src/ruckig/position_first_step2.o @@ -1310,6 +1285,7 @@ tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step1.o tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step2.o tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step1.o tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step2.o +tpmod-objs += emc/tp/tc_9d.o tpmod-objs += libnml/posemath/sincos.o $(MATHSTUB) TORTOBJS = $(foreach file,$($(patsubst %.o,%,$(1))-objs), objects/rt$(file)) diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc index 0d25ad396fb..70280f1d366 100644 --- a/src/emc/ini/inihal.cc +++ b/src/emc/ini/inihal.cc @@ -309,10 +309,10 @@ int check_ini_hal_items(int numjoints) if (CHANGED(traj_planner_type)) { if (debug) SHOW_CHANGE_INT(traj_planner_type) UPDATE(traj_planner_type); - // Only 0 and 1 are supported, set to 0 if invalid - // Also force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) + // Planner types: 0=trapezoidal, 1=scurve, 2=9D (experimental) + // Force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) int planner_type = NEW(traj_planner_type); - if (planner_type != 0 && planner_type != 1) { + if (planner_type < 0 || planner_type > 2) { planner_type = 0; } if (planner_type == 1 && NEW(traj_max_jerk) < 1.0) { diff --git a/src/emc/ini/inijoint.cc b/src/emc/ini/inijoint.cc index c7647da40bd..72726dcef87 100644 --- a/src/emc/ini/inijoint.cc +++ b/src/emc/ini/inijoint.cc @@ -77,7 +77,6 @@ static int loadJoint(int joint, EmcIniFile *jointIniFile) double units; double backlash; double offset; - double limit; double home; double search_vel; double latch_vel; @@ -95,6 +94,7 @@ static int loadJoint(int joint, EmcIniFile *jointIniFile) double maxAcceleration; double maxJerk; double ferror; + double limit; // compose string to match, joint = 0 -> JOINT_0, etc. snprintf(jointString, sizeof(jointString), "JOINT_%d", joint); @@ -231,6 +231,10 @@ static int loadJoint(int joint, EmcIniFile *jointIniFile) } old_inihal_data.joint_jerk[joint] = maxJerk; + // NOTE: Userspace kinematics reads joint limits dynamically from shared memory + // (emcmotStruct->status.joint_status[j]) to support runtime changes + // via HAL pins (ini.N.max_limit, ini.N.max_velocity, etc.) + comp_file_type = 0; // default jointIniFile->Find(&comp_file_type, "COMP_FILE_TYPE", jointString); auto comp_file = jointIniFile->Find("COMP_FILE", jointString); diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index dade6236594..809bddcbd28 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -25,9 +25,19 @@ #include "emcglb.h" /*! \todo TRAVERSE_RATE (FIXME) */ #include "inihal.hh" #include +#include "motion.h" // For emcmotConfig, emcmot_config_t + +// Userspace kinematics for trajectory planning +#include "userspace_kinematics.hh" + +// Predictive handoff configuration for feed override handling +#include "motion_planning_9d.hh" extern value_inihal_data old_inihal_data; +/* Access to emcmotConfig from usrmotintf.cc (initialized by usrmotInit()) */ +extern emcmot_config_t *emcmotConfig; + /* loadKins() @@ -220,16 +230,168 @@ static int loadTraj(EmcIniFile *trajInifile) } return -1; } - planner_type = 0; // Default: 0 = trapezoidal, 1 = S-curve + planner_type = 0; // Default: 0 = trapezoidal, 1 = S-curve, 2 = 9D (EXPERIMENTAL) trajInifile->Find(&planner_type, "PLANNER_TYPE", "TRAJ"); - // Only 0 and 1 are supported, set to 0 if invalid - // Also force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) - if (planner_type != 0 && planner_type != 1) { + // Support types 0, 1, and 2 (9D experimental) + // Force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) + if (planner_type < 0 || planner_type > 2) { + rcs_print("Invalid PLANNER_TYPE %d, must be 0, 1, or 2. Defaulting to 0.\n", planner_type); planner_type = 0; } if (planner_type == 1 && jerk < 1.0) { planner_type = 0; } + if (planner_type == 2) { + rcs_print("PLANNER_TYPE 2 (9D) is EXPERIMENTAL. Use with caution.\n"); + + // Planner type 2 requires userspace kinematics - enable automatically + { + // Read kinematics module name from emcmotConfig (set by motion module) + const char *kins_module = emcmotConfig->kins_module_name; + + if (!kins_module || kins_module[0] == '\0') { + rcs_print_error("ERROR: No kinematics module name in emcmotConfig\n"); + rcs_print_error("PLANNER_TYPE 2 requires a kinematics module to be loaded.\n"); + rcs_print_error("Ensure your HAL file loads a kinematics module (e.g., loadrt trivkins)\n"); + return -1; + } + + rcs_print("Detected kinematics module: %s\n", kins_module); + + // Get configuration from INI file + auto coord = trajInifile->Find("COORDINATES", "TRAJ"); + int joints = 0; + trajInifile->Find(&joints, "JOINTS", "KINS"); + if (joints <= 0) joints = 3; + + if (motion_planning::userspace_kins_init(kins_module, + joints, + coord ? coord->c_str() : "XYZ") != 0) { + rcs_print_error("WARNING: Failed to initialize userspace kinematics for '%s'\n", + kins_module); + rcs_print_error(" Falling back to PLANNER_TYPE 0 (trapezoidal)\n"); + planner_type = 0; + } else { + rcs_print("Userspace kinematics initialized (module=%s, joints=%d, coords=%s)\n", + kins_module, joints, coord ? coord->c_str() : "XYZ"); + } + } + + // Parse 9D-specific parameters + int optimization_depth = 8; + double ramp_frequency = 10.0; + int smoothing_passes = 2; + int tc_queue_size = 50; + + trajInifile->Find(&optimization_depth, "OPTIMIZATION_DEPTH", "TRAJ"); + trajInifile->Find(&ramp_frequency, "RAMP_FREQUENCY", "TRAJ"); + trajInifile->Find(&smoothing_passes, "SMOOTHING_PASSES", "TRAJ"); + trajInifile->Find(&tc_queue_size, "TC_QUEUE_SIZE", "TRAJ"); + + // Validate 9D parameters + if (optimization_depth < 4 || optimization_depth > 200) { + rcs_print("Warning: OPTIMIZATION_DEPTH %d out of range [4,200], using default 8\n", optimization_depth); + optimization_depth = 8; + } + if (ramp_frequency < 1.0 || ramp_frequency > 1000.0) { + rcs_print("Warning: RAMP_FREQUENCY %.1f out of range [1.0,1000.0], using default 10.0\n", ramp_frequency); + ramp_frequency = 10.0; + } + if (smoothing_passes < 1 || smoothing_passes > 10) { + rcs_print("Warning: SMOOTHING_PASSES %d out of range [1,10], using default 2\n", smoothing_passes); + smoothing_passes = 2; + } + if (tc_queue_size < 32 || tc_queue_size > 400) { + rcs_print("Warning: TC_QUEUE_SIZE %d out of range [32,400], using default 50\n", tc_queue_size); + tc_queue_size = 50; + } + + rcs_print("9D Planner Configuration:\n"); + rcs_print(" OPTIMIZATION_DEPTH = %d\n", optimization_depth); + rcs_print(" RAMP_FREQUENCY = %.1f Hz\n", ramp_frequency); + rcs_print(" SMOOTHING_PASSES = %d\n", smoothing_passes); + rcs_print(" TC_QUEUE_SIZE = %d\n", tc_queue_size); + + // Predictive handoff configuration for feed override handling + // All timing values in milliseconds + double handoff_horizon_ms = 100.0; + double branch_window_ms = 50.0; + double min_buffer_time_ms = 100.0; + double target_buffer_time_ms = 200.0; + double max_buffer_time_ms = 500.0; + double feed_override_debounce_ms = 50.0; + + trajInifile->Find(&handoff_horizon_ms, "HANDOFF_HORIZON_MS", "TRAJ"); + trajInifile->Find(&branch_window_ms, "BRANCH_WINDOW_MS", "TRAJ"); + trajInifile->Find(&min_buffer_time_ms, "MIN_BUFFER_TIME_MS", "TRAJ"); + trajInifile->Find(&target_buffer_time_ms, "TARGET_BUFFER_TIME_MS", "TRAJ"); + trajInifile->Find(&max_buffer_time_ms, "MAX_BUFFER_TIME_MS", "TRAJ"); + trajInifile->Find(&feed_override_debounce_ms, "FEED_OVERRIDE_DEBOUNCE_MS", "TRAJ"); + + // Validate predictive handoff timing parameters + if (handoff_horizon_ms < 1.0 || handoff_horizon_ms > 1000.0) { + rcs_print("Warning: HANDOFF_HORIZON_MS %.1f out of range [1,1000], using default 100\n", handoff_horizon_ms); + handoff_horizon_ms = 100.0; + } + if (branch_window_ms < 10.0 || branch_window_ms > 500.0) { + rcs_print("Warning: BRANCH_WINDOW_MS %.1f out of range [10,500], using default 50\n", branch_window_ms); + branch_window_ms = 50.0; + } + if (min_buffer_time_ms < 10.0 || min_buffer_time_ms > 1000.0) { + rcs_print("Warning: MIN_BUFFER_TIME_MS %.1f out of range [10,1000], using default 100\n", min_buffer_time_ms); + min_buffer_time_ms = 100.0; + } + if (target_buffer_time_ms < min_buffer_time_ms || target_buffer_time_ms > 2000.0) { + rcs_print("Warning: TARGET_BUFFER_TIME_MS %.1f out of range [%.1f,2000], using default 200\n", + target_buffer_time_ms, min_buffer_time_ms); + target_buffer_time_ms = 200.0; + } + if (max_buffer_time_ms < target_buffer_time_ms || max_buffer_time_ms > 5000.0) { + rcs_print("Warning: MAX_BUFFER_TIME_MS %.1f out of range [%.1f,5000], using default 500\n", + max_buffer_time_ms, target_buffer_time_ms); + max_buffer_time_ms = 500.0; + } + if (feed_override_debounce_ms < 1.0 || feed_override_debounce_ms > 500.0) { + rcs_print("Warning: FEED_OVERRIDE_DEBOUNCE_MS %.1f out of range [1,500], using default 50\n", feed_override_debounce_ms); + feed_override_debounce_ms = 50.0; + } + + // Get servo cycle time from motion config (already initialized) + // Default to 1ms if not available + double servo_cycle_time_sec = 0.001; + if (emcmotConfig && emcmotConfig->trajCycleTime > 0) { + servo_cycle_time_sec = emcmotConfig->trajCycleTime; + } + + // Use max jerk from INI (already parsed above) + double default_max_jerk = jerk; + if (default_max_jerk < 1.0) { + default_max_jerk = 1e9; // Match initraj.cc default + } + + // Apply predictive handoff configuration + setHandoffConfig(handoff_horizon_ms, + branch_window_ms, + min_buffer_time_ms, + target_buffer_time_ms, + max_buffer_time_ms, + feed_override_debounce_ms, + servo_cycle_time_sec, + default_max_jerk); + + rcs_print("Predictive Handoff Configuration:\n"); + rcs_print(" HANDOFF_HORIZON_MS = %.1f\n", handoff_horizon_ms); + rcs_print(" BRANCH_WINDOW_MS = %.1f\n", branch_window_ms); + rcs_print(" MIN_BUFFER_TIME_MS = %.1f\n", min_buffer_time_ms); + rcs_print(" TARGET_BUFFER_TIME_MS = %.1f\n", target_buffer_time_ms); + rcs_print(" MAX_BUFFER_TIME_MS = %.1f\n", max_buffer_time_ms); + rcs_print(" FEED_OVERRIDE_DEBOUNCE_MS = %.1f\n", feed_override_debounce_ms); + rcs_print(" servo_cycle_time = %.3f ms\n", servo_cycle_time_sec * 1000.0); + rcs_print(" default_max_jerk = %.0f\n", default_max_jerk); + + // Initialize predictive handoff system + initPredictiveHandoff(); + } if (0 != emcTrajPlannerType(planner_type)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("bad return value from emcTrajPlannerType\n"); @@ -288,6 +450,20 @@ static int loadTraj(EmcIniFile *trajInifile) } return -1; } + + // Dual-layer architecture: 1 = send NML after userspace planning (backward compat) + // 0 = userspace only mode (new dual-layer architecture) + int emulate_legacy = 1; // Default: backward compatible + trajInifile->Find(&emulate_legacy, "EMULATE_LEGACY_MOVE_COMMANDS", "TRAJ"); + if (0 != emcSetEmulateLegacyMoveCommands(emulate_legacy)) { + if (emc_debug & EMC_DEBUG_CONFIG) { + rcs_print("bad return value from emcSetEmulateLegacyMoveCommands\n"); + } + return -1; + } + if (!emulate_legacy) { + rcs_print("Dual-layer architecture enabled: NML motion commands disabled\n"); + } } catch (EmcIniFile::Exception &e) { diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 8f29969c3c5..bc08e25a239 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -42,8 +42,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -63,26 +63,107 @@ #include "posemath.h" #include "switchkins.h" +/* ======================================================================== + * Internal math (was in 5axiskins_math.h) + * ======================================================================== */ + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#ifndef TO_RAD +#define TO_RAD (M_PI / 180.0) +#endif + +typedef struct { + double pivot_length; +} fiveaxis_params_t; + +typedef struct { + int jx, jy, jz; + int ja, jb, jc; + int ju, jv, jw; +} fiveaxis_joints_t; + +static void fiveaxis_s2r(double r, double t, double p, + double *x, double *y, double *z) { + double t_rad = TO_RAD * t; + double p_rad = TO_RAD * p; + *x = r * sin(p_rad) * cos(t_rad); + *y = r * sin(p_rad) * sin(t_rad); + *z = r * cos(p_rad); +} + +static int fiveaxis_forward_impl(const fiveaxis_params_t *params, + const fiveaxis_joints_t *jm, + const double *joints, + EmcPose *pos) { + double rx, ry, rz; + double b_val = (jm->jb >= 0) ? joints[jm->jb] : 0; + double c_val = (jm->jc >= 0) ? joints[jm->jc] : 0; + double w_val = (jm->jw >= 0) ? joints[jm->jw] : 0; + + fiveaxis_s2r(params->pivot_length + w_val, c_val, 180.0 - b_val, + &rx, &ry, &rz); + + pos->tran.x = (jm->jx >= 0 ? joints[jm->jx] : 0) + rx; + pos->tran.y = (jm->jy >= 0 ? joints[jm->jy] : 0) + ry; + pos->tran.z = (jm->jz >= 0 ? joints[jm->jz] : 0) + params->pivot_length + rz; + pos->a = (jm->ja >= 0) ? joints[jm->ja] : 0; + pos->b = b_val; + pos->c = c_val; + pos->u = (jm->ju >= 0) ? joints[jm->ju] : 0; + pos->v = (jm->jv >= 0) ? joints[jm->jv] : 0; + pos->w = w_val; + + return 0; +} + +static int fiveaxis_inverse_impl(const fiveaxis_params_t *params, + const EmcPose *world, + EmcPose *axis_values) { + double rx, ry, rz; + + fiveaxis_s2r(params->pivot_length + world->w, world->c, 180.0 - world->b, + &rx, &ry, &rz); + + axis_values->tran.x = world->tran.x - rx; + axis_values->tran.y = world->tran.y - ry; + axis_values->tran.z = world->tran.z - params->pivot_length - rz; + axis_values->a = world->a; + axis_values->b = world->b; + axis_values->c = world->c; + axis_values->u = world->u; + axis_values->v = world->v; + axis_values->w = world->w; + + return 0; +} + +static void fiveaxis_axis_to_joints(const fiveaxis_joints_t *jm, + const EmcPose *axis_values, + double *joints) { + if (jm->jx >= 0) joints[jm->jx] = axis_values->tran.x; + if (jm->jy >= 0) joints[jm->jy] = axis_values->tran.y; + if (jm->jz >= 0) joints[jm->jz] = axis_values->tran.z; + if (jm->ja >= 0) joints[jm->ja] = axis_values->a; + if (jm->jb >= 0) joints[jm->jb] = axis_values->b; + if (jm->jc >= 0) joints[jm->jc] = axis_values->c; + if (jm->ju >= 0) joints[jm->ju] = axis_values->u; + if (jm->jv >= 0) joints[jm->jv] = axis_values->v; + if (jm->jw >= 0) joints[jm->jw] = axis_values->w; +} + +/* ======================================================================== + * RT interface (reads HAL pins) + * ======================================================================== */ + struct haldata { hal_float_t *pivot_length; } *haldata; static int fiveaxis_max_joints; -static PmCartesian s2r(double r, double t, double p) { - // s2r: spherical coordinates to cartesian coordinates - // r = length of vector - // p=phi = angle of vector wrt z axis - // t=theta = angle of vector projected onto xy plane - // (projection length in xy plane is r*sin(p) - PmCartesian c; - t = TO_RAD*t; p = TO_RAD*p; // degrees to radians - - c.x = r * sin(p) * cos(t); - c.y = r * sin(p) * sin(t); - c.z = r * cos(p); - - return c; -} //s2r() +// Joint mapping struct +static fiveaxis_joints_t jmap; // assignments of principal joints to axis letters: // (-1 means not defined (yet)) @@ -103,24 +184,9 @@ static int fiveaxis_KinematicsForward(const double *joints, { (void)fflags; (void)iflags; - PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW], - joints[JC], - 180.0 - joints[JB]); - - // Note: 'principal' joints are used - pos->tran.x = joints[JX] + r.x; - pos->tran.y = joints[JY] + r.y; - pos->tran.z = joints[JZ] + *(haldata->pivot_length) + r.z; - pos->b = joints[JB]; - pos->c = joints[JC]; - pos->w = joints[JW]; - - // optional letters (specify with coordinates module parameter) - pos->a = (JA != -1)? joints[JA] : 0; - pos->u = (JU != -1)? joints[JU] : 0; - pos->v = (JV != -1)? joints[JV] : 0; - - return 0; + fiveaxis_params_t params; + params.pivot_length = *(haldata->pivot_length); + return fiveaxis_forward_impl(¶ms, &jmap, joints, pos); } //fiveaxis_KinematicsForward() static int fiveaxis_KinematicsInverse(const EmcPose * pos, @@ -130,23 +196,12 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, - pos->c, - 180.0 - pos->b); + fiveaxis_params_t params; + params.pivot_length = *(haldata->pivot_length); - EmcPose P; // computed position - P.tran.x = pos->tran.x - r.x; - P.tran.y = pos->tran.y - r.y; - P.tran.z = pos->tran.z - *(haldata->pivot_length) - r.z; - - P.b = pos->b; - P.c = pos->c; - P.w = pos->w; - - // optional letters (specify with coordinates module parameter) - P.a = (JA != -1)? pos->a : 0; - P.u = (JU != -1)? pos->u : 0; - P.v = (JV != -1)? pos->v : 0; + // Compute axis values using pure math function + EmcPose P; + fiveaxis_inverse_impl(¶ms, pos, &P); // update joints with support for // multiple-joints per-coordinate letter: @@ -210,6 +265,11 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + // Populate joint map struct for math functions + jmap.jx = JX; jmap.jy = JY; jmap.jz = JZ; + jmap.ja = JA; jmap.jb = JB; jmap.jc = JC; + jmap.ju = JU; jmap.jv = JV; jmap.jw = JW; + haldata = hal_malloc(sizeof(struct haldata)); result = hal_pin_float_newf(HAL_IN,&(haldata->pivot_length),comp_id, @@ -270,3 +330,69 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + fiveaxis_params_t p; + fiveaxis_joints_t jm; + + p.pivot_length = kp->params.fiveaxis.pivot_length; + jm.jx = kp->axis_to_joint[0]; jm.jy = kp->axis_to_joint[1]; + jm.jz = kp->axis_to_joint[2]; jm.ja = kp->axis_to_joint[3]; + jm.jb = kp->axis_to_joint[4]; jm.jc = kp->axis_to_joint[5]; + jm.ju = kp->axis_to_joint[6]; jm.jv = kp->axis_to_joint[7]; + jm.jw = kp->axis_to_joint[8]; + + return fiveaxis_forward_impl(&p, &jm, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + fiveaxis_params_t p; + fiveaxis_joints_t jm; + EmcPose axis_values; + + p.pivot_length = kp->params.fiveaxis.pivot_length; + jm.jx = kp->axis_to_joint[0]; jm.jy = kp->axis_to_joint[1]; + jm.jz = kp->axis_to_joint[2]; jm.ja = kp->axis_to_joint[3]; + jm.jb = kp->axis_to_joint[4]; jm.jc = kp->axis_to_joint[5]; + jm.ju = kp->axis_to_joint[6]; jm.jv = kp->axis_to_joint[7]; + jm.jw = kp->axis_to_joint[8]; + + fiveaxis_inverse_impl(&p, pos, &axis_values); + fiveaxis_axis_to_joints(&jm, &axis_values, joints); + return 0; +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_bit; (void)read_s32; + + if (read_float("5axiskins.pivot-length", + &kp->params.fiveaxis.pivot_length) != 0) + return -1; + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index acead1d061e..af32cb0b90c 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -7,12 +7,46 @@ #include "motion.h" #include "hal.h" #include "rtapi.h" -#include "rtapi.h" #include "rtapi_app.h" -#include "rtapi_math.h" #include "rtapi_string.h" #include "kinematics.h" +/* + * CoreXY kinematics - pure linear transformation, no parameters + * + * Forward: X = 0.5*(J0+J1), Y = 0.5*(J0-J1) + * Inverse: J0 = X+Y, J1 = X-Y + */ +static int corexy_forward_math(const double *joints, EmcPose *world) +{ + world->tran.x = 0.5 * (joints[0] + joints[1]); + world->tran.y = 0.5 * (joints[0] - joints[1]); + world->tran.z = joints[2]; + world->a = joints[3]; + world->b = joints[4]; + world->c = joints[5]; + world->u = joints[6]; + world->v = joints[7]; + world->w = joints[8]; + + return 0; +} + +static int corexy_inverse_math(const EmcPose *world, double *joints) +{ + joints[0] = world->tran.x + world->tran.y; + joints[1] = world->tran.x - world->tran.y; + joints[2] = world->tran.z; + joints[3] = world->a; + joints[4] = world->b; + joints[5] = world->c; + joints[6] = world->u; + joints[7] = world->v; + joints[8] = world->w; + + return 0; +} + static struct data { hal_s32_t joints[EMCMOT_MAX_JOINTS]; } *data; @@ -24,17 +58,7 @@ int kinematicsForward(const double *joints ) { (void)fflags; (void)iflags; - pos->tran.x = 0.5 * (joints[0] + joints[1]); - pos->tran.y = 0.5 * (joints[0] - joints[1]); - pos->tran.z = joints[2]; - pos->a = joints[3]; - pos->b = joints[4]; - pos->c = joints[5]; - pos->u = joints[6]; - pos->v = joints[7]; - pos->w = joints[8]; - - return 0; + return corexy_forward_math(joints, pos); } int kinematicsInverse(const EmcPose *pos @@ -44,17 +68,7 @@ int kinematicsInverse(const EmcPose *pos ) { (void)iflags; (void)fflags; - joints[0] = pos->tran.x + pos->tran.y; - joints[1] = pos->tran.x - pos->tran.y; - joints[2] = pos->tran.z; - joints[3] = pos->a; - joints[4] = pos->b; - joints[5] = pos->c; - joints[6] = pos->u; - joints[7] = pos->v; - joints[8] = pos->w; - - return 0; + return corexy_inverse_math(pos, joints); } int kinematicsHome(EmcPose *world @@ -69,10 +83,13 @@ int kinematicsHome(EmcPose *world KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +const char* kinematicsGetName(void) { return "corexykins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); static int comp_id; @@ -87,3 +104,40 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + (void)params; + return corexy_forward_math(joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + (void)params; + return corexy_inverse_math(pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + (void)params; (void)read_float; (void)read_bit; (void)read_s32; + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index b4f08116371..1d8ac00189e 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -115,6 +115,227 @@ #include "kinematics.h" /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "switchkins.h" +/* ======================================================================== + * Math types and functions (was in genhexkins_math.h) + * ======================================================================== */ + +#define GENHEX_NUM_STRUTS 6 + +typedef struct { + PmCartesian base[GENHEX_NUM_STRUTS]; + PmCartesian platform[GENHEX_NUM_STRUTS]; + PmCartesian base_n[GENHEX_NUM_STRUTS]; + PmCartesian platform_n[GENHEX_NUM_STRUTS]; + double conv_criterion; + unsigned int iter_limit; + double max_error; + double tool_offset; + double spindle_offset; + double screw_lead; + unsigned int last_iterations; + unsigned int max_iterations; + double correction[GENHEX_NUM_STRUTS]; +} genhex_params_t; + +static int genhex_mat_invert(double J[][GENHEX_NUM_STRUTS], + double InvJ[][GENHEX_NUM_STRUTS]) +{ + double JAug[GENHEX_NUM_STRUTS][12], m, temp; + int j, k, n; + for (j = 0; j <= 5; ++j) { + for (k = 0; k <= 5; ++k) JAug[j][k] = J[j][k]; + for (k = 6; k <= 11; ++k) JAug[j][k] = (k - 6 == j) ? 1 : 0; + } + for (k = 0; k <= 4; ++k) { + if ((JAug[k][k] < 0.01) && (JAug[k][k] > -0.01)) { + for (j = k + 1; j <= 5; ++j) { + if ((JAug[j][k] > 0.01) || (JAug[j][k] < -0.01)) { + for (n = 0; n <= 11; ++n) { + temp = JAug[k][n]; JAug[k][n] = JAug[j][n]; JAug[j][n] = temp; + } + break; + } + } + } + for (j = k + 1; j <= 5; ++j) { + m = -JAug[j][k] / JAug[k][k]; + for (n = 0; n <= 11; ++n) { + JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)) JAug[j][n] = 0; + } + } + } + for (j = 0; j <= 5; ++j) { + m = 1 / JAug[j][j]; + for (k = 0; k <= 11; ++k) JAug[j][k] = m * JAug[j][k]; + } + for (k = 5; k >= 0; --k) { + for (j = k - 1; j >= 0; --j) { + m = -JAug[j][k] / JAug[k][k]; + for (n = 0; n <= 11; ++n) JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + } + } + for (j = 0; j <= 5; ++j) + for (k = 0; k <= 5; ++k) InvJ[j][k] = JAug[j][k + 6]; + return 0; +} + +static void genhex_mat_mult(double J[][6], const double x[], double Ans[]) +{ + int j, k; + for (j = 0; j <= 5; ++j) { + Ans[j] = 0; + for (k = 0; k <= 5; ++k) Ans[j] = J[j][k] * x[k] + Ans[j]; + } +} + +static int genhex_strut_length_correction(const genhex_params_t *params, + const PmCartesian *StrutVectUnit, + const PmRotationMatrix *RMatrix, + int strut_number, double *correction) +{ + PmCartesian nb2, nb3, na1, na2; + double dotprod; + pmCartCartCross(¶ms->base_n[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(StrutVectUnit, &nb2, &nb3); + pmCartUnitEq(&nb3); + pmMatCartMult(RMatrix, ¶ms->platform_n[strut_number], &na1); + pmCartCartCross(&na1, StrutVectUnit, &na2); + pmCartUnitEq(&na2); + pmCartCartDot(&nb3, &na2, &dotprod); + *correction = params->screw_lead * asin(dotprod) / PM_2_PI; + return 0; +} + +static int genhex_fwd(genhex_params_t *params, + const double joints[GENHEX_NUM_STRUTS], + EmcPose *pos) +{ + PmCartesian aw, InvKinStrutVect, InvKinStrutVectUnit; + PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; + double Jacobian[GENHEX_NUM_STRUTS][GENHEX_NUM_STRUTS]; + double InverseJacobian[GENHEX_NUM_STRUTS][GENHEX_NUM_STRUTS]; + double InvKinStrutLength, StrutLengthDiff[GENHEX_NUM_STRUTS]; + double delta[GENHEX_NUM_STRUTS]; + double conv_err = 1.0, corr; + PmRotationMatrix RMatrix; + PmRpy q_RPY; + int iterate = 1, i; + unsigned iteration = 0; + + if (joints[0] <= 0.0 || joints[1] <= 0.0 || joints[2] <= 0.0 || + joints[3] <= 0.0 || joints[4] <= 0.0 || joints[5] <= 0.0) + return -1; + + q_RPY.r = pos->a * PM_PI / 180.0; + q_RPY.p = pos->b * PM_PI / 180.0; + q_RPY.y = pos->c * PM_PI / 180.0; + q_trans.x = pos->tran.x; + q_trans.y = pos->tran.y; + q_trans.z = pos->tran.z; + + while (iterate) { + if ((conv_err > params->max_error) || (conv_err < -params->max_error)) + return -2; + iteration++; + if (iteration > params->iter_limit) return -5; + + pmRpyMatConvert(&q_RPY, &RMatrix); + + for (i = 0; i < GENHEX_NUM_STRUTS; i++) { + PmCartesian b_adjusted, a_adjusted; + b_adjusted.x = params->base[i].x; + b_adjusted.y = params->base[i].y; + b_adjusted.z = params->base[i].z + params->spindle_offset + params->tool_offset; + a_adjusted.x = params->platform[i].x; + a_adjusted.y = params->platform[i].y; + a_adjusted.z = params->platform[i].z + params->spindle_offset + params->tool_offset; + + pmMatCartMult(&RMatrix, &a_adjusted, &RMatrix_a); + pmCartCartAdd(&q_trans, &RMatrix_a, &aw); + pmCartCartSub(&aw, &b_adjusted, &InvKinStrutVect); + if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) return -1; + pmCartMag(&InvKinStrutVect, &InvKinStrutLength); + + if (params->screw_lead != 0.0) { + genhex_strut_length_correction(params, &InvKinStrutVectUnit, + &RMatrix, i, &corr); + InvKinStrutLength += corr; + } + StrutLengthDiff[i] = InvKinStrutLength - joints[i]; + pmCartCartCross(&RMatrix_a, &InvKinStrutVectUnit, &RMatrix_a_cross_Strut); + InverseJacobian[i][0] = InvKinStrutVectUnit.x; + InverseJacobian[i][1] = InvKinStrutVectUnit.y; + InverseJacobian[i][2] = InvKinStrutVectUnit.z; + InverseJacobian[i][3] = RMatrix_a_cross_Strut.x; + InverseJacobian[i][4] = RMatrix_a_cross_Strut.y; + InverseJacobian[i][5] = RMatrix_a_cross_Strut.z; + } + genhex_mat_invert(InverseJacobian, Jacobian); + genhex_mat_mult(Jacobian, StrutLengthDiff, delta); + q_trans.x -= delta[0]; q_trans.y -= delta[1]; q_trans.z -= delta[2]; + q_RPY.r -= delta[3]; q_RPY.p -= delta[4]; q_RPY.y -= delta[5]; + conv_err = 0.0; + for (i = 0; i < GENHEX_NUM_STRUTS; i++) conv_err += fabs(StrutLengthDiff[i]); + iterate = 0; + for (i = 0; i < GENHEX_NUM_STRUTS; i++) + if (fabs(StrutLengthDiff[i]) > params->conv_criterion) iterate = 1; + } + + pos->a = q_RPY.r * 180.0 / PM_PI; + pos->b = q_RPY.p * 180.0 / PM_PI; + pos->c = q_RPY.y * 180.0 / PM_PI; + pos->tran.x = q_trans.x; pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; + params->last_iterations = iteration; + if (iteration > params->max_iterations) params->max_iterations = iteration; + return 0; +} + +static int genhex_inv(genhex_params_t *params, + const EmcPose *pos, + double joints[GENHEX_NUM_STRUTS]) +{ + PmCartesian aw, temp, InvKinStrutVect, InvKinStrutVectUnit; + PmRotationMatrix RMatrix; + PmRpy rpy; + int i; + double InvKinStrutLength, corr; + + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); + + for (i = 0; i < GENHEX_NUM_STRUTS; i++) { + PmCartesian b_adjusted, a_adjusted; + b_adjusted.x = params->base[i].x; + b_adjusted.y = params->base[i].y; + b_adjusted.z = params->base[i].z + params->spindle_offset + params->tool_offset; + a_adjusted.x = params->platform[i].x; + a_adjusted.y = params->platform[i].y; + a_adjusted.z = params->platform[i].z + params->spindle_offset + params->tool_offset; + + pmMatCartMult(&RMatrix, &a_adjusted, &temp); + pmCartCartAdd(&pos->tran, &temp, &aw); + pmCartCartSub(&aw, &b_adjusted, &InvKinStrutVect); + pmCartMag(&InvKinStrutVect, &InvKinStrutLength); + + if (params->screw_lead != 0.0) { + if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) return -1; + genhex_strut_length_correction(params, &InvKinStrutVectUnit, + &RMatrix, i, &corr); + params->correction[i] = corr; + InvKinStrutLength += corr; + } + joints[i] = InvKinStrutLength; + } + return 0; +} + +/* ======================================================================== + * RT interface + * ======================================================================== */ + static struct haldata { hal_float_t *basex[NUM_STRUTS]; hal_float_t *basey[NUM_STRUTS]; @@ -159,169 +380,38 @@ static int genhex_gui_forward_kins(EmcPose *pos) return 0; } // genhex_gui_forward_kins -/******************************* MatInvert() ***************************/ - -/*----------------------------------------------------------------------------- - This is a function that inverts a 6x6 matrix. ------------------------------------------------------------------------------*/ - -static int MatInvert(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) -{ - double JAug[NUM_STRUTS][12], m, temp; - int j, k, n; - - /* This function determines the inverse of a 6x6 matrix using - Gauss-Jordan elimination */ - - /* Augment the Identity matrix to the Jacobian matrix */ - - for (j=0; j<=5; ++j){ - for (k=0; k<=5; ++k){ /* Assign J matrix to first 6 columns of AugJ */ - JAug[j][k] = J[j][k]; - } - for(k=6; k<=11; ++k){ /* Assign I matrix to last six columns of AugJ */ - if (k-6 == j){ - JAug[j][k]=1; - } - else{ - JAug[j][k]=0; - } - } - } - - /* Perform Gauss elimination */ - for (k=0; k<=4; ++k){ /* Pivot */ - if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){ - for (j=k+1;j<=5; ++j){ - if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){ - for (n=0; n<=11;++n){ - temp = JAug[k][n]; - JAug[k][n] = JAug[j][n]; - JAug[j][n] = temp; - } - break; - } - } - } - for (j=k+1; j<=5; ++j){ /* Pivot */ - m = -JAug[j][k] / JAug[k][k]; - for (n=0; n<=11; ++n){ - JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */ - if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){ - JAug[j][n] = 0; - } - } - } - } - - /* Normalization of Diagonal Terms */ - for (j=0; j<=5; ++j){ - m=1/JAug[j][j]; - for(k=0; k<=11; ++k){ - JAug[j][k] = m * JAug[j][k]; - } - } - - /* Perform Gauss Jordan Steps */ - for (k=5; k>=0; --k){ - for(j=k-1; j>=0; --j){ - m = -JAug[j][k]/JAug[k][k]; - for (n=0; n<=11; ++n){ - JAug[j][n] = JAug[j][n] + m * JAug[k][n]; - } - } - } - - /* Assign last 6 columns of JAug to InvJ */ - for (j=0; j<=5; ++j){ - for (k=0; k<=5; ++k){ - InvJ[j][k] = JAug[j][k+6]; - - } - } - - return 0; /* FIXME-- check divisors for 0 above */ -} // MatInvert() - -/******************************** MatMult() *********************************/ - -/*--------------------------------------------------------------------------- - This function simply multiplies a 6x6 matrix by a 1x6 vector - ---------------------------------------------------------------------------*/ - -static void MatMult(double J[][6], const double x[], double Ans[]) -{ - int j, k; - for (j=0; j<=5; ++j){ - Ans[j] = 0; - for (k=0; k<=5; ++k){ - Ans[j] = J[j][k]*x[k]+Ans[j]; - } - } -} // MatMult() - -/* declare arrays for base and platform coordinates */ -static PmCartesian b[NUM_STRUTS]; -static PmCartesian a[NUM_STRUTS]; - -/* declare base and platform joint axes vectors */ - -static PmCartesian nb1[NUM_STRUTS]; -static PmCartesian na0[NUM_STRUTS]; - /************************genhex_read_hal_pins**************************/ -static int genhex_read_hal_pins(void) { +static void genhex_read_hal_pins(genhex_params_t *params) { int t; - /* set the base and platform coordinates from hal pin values */ + /* set the base and platform coordinates from hal pin values */ for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = *haldata->basex[t]; - b[t].y = *haldata->basey[t]; - b[t].z = *haldata->basez[t] + *haldata->spindle_offset + *haldata->tool_offset; - a[t].x = *haldata->platformx[t]; - a[t].y = *haldata->platformy[t]; - a[t].z = *haldata->platformz[t] + *haldata->spindle_offset + *haldata->tool_offset; - - nb1[t].x = *haldata->basenx[t]; - nb1[t].y = *haldata->baseny[t]; - nb1[t].z = *haldata->basenz[t]; - na0[t].x = *haldata->platformnx[t]; - na0[t].y = *haldata->platformny[t]; - na0[t].z = *haldata->platformnz[t]; - - } - return 0; -} // genhex_read_hal_pins() - -/***************************StrutLengthCorrection***************************/ - -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, - const PmRotationMatrix * RMatrix, - const int strut_number, - double * correction) -{ - PmCartesian nb2, nb3, na1, na2; - double dotprod; - - /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); - pmCartCartCross(StrutVectUnit, &nb2, &nb3); - pmCartUnitEq(&nb3); + params->base[t].x = *haldata->basex[t]; + params->base[t].y = *haldata->basey[t]; + params->base[t].z = *haldata->basez[t]; - /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); - pmCartCartCross(&na1, StrutVectUnit, &na2); - pmCartUnitEq(&na2); + params->platform[t].x = *haldata->platformx[t]; + params->platform[t].y = *haldata->platformy[t]; + params->platform[t].z = *haldata->platformz[t]; - /* define dot product */ - pmCartCartDot(&nb3, &na2, &dotprod); + params->base_n[t].x = *haldata->basenx[t]; + params->base_n[t].y = *haldata->baseny[t]; + params->base_n[t].z = *haldata->basenz[t]; - *correction = *haldata->screw_lead * asin(dotprod) / PM_2_PI; - - return 0; -} // StrutLengthCorrection() + params->platform_n[t].x = *haldata->platformnx[t]; + params->platform_n[t].y = *haldata->platformny[t]; + params->platform_n[t].z = *haldata->platformnz[t]; + } + /* set iteration and offset parameters */ + params->conv_criterion = *haldata->conv_criterion; + params->iter_limit = *haldata->iter_limit; + params->max_error = *haldata->max_error; + params->tool_offset = *haldata->tool_offset; + params->spindle_offset = *haldata->spindle_offset; + params->screw_lead = *haldata->screw_lead; +} // genhex_read_hal_pins() /**************** genhexKinematicsForward() *****************/ static int genhexKinematicsForward(const double * joints, @@ -329,156 +419,35 @@ static int genhexKinematicsForward(const double * joints, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { - (void)fflags; - (void)iflags; - PmCartesian aw; - PmCartesian InvKinStrutVect,InvKinStrutVectUnit; - PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - - double Jacobian[NUM_STRUTS][NUM_STRUTS]; - double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; - double InvKinStrutLength, StrutLengthDiff[NUM_STRUTS]; - double delta[NUM_STRUTS]; - double conv_err = 1.0; - double corr; - - PmRotationMatrix RMatrix; - PmRpy q_RPY; - - int iterate = 1; - int i; - unsigned iteration = 0; - - genhex_read_hal_pins(); - - /* abort on obvious problems, like joints <= 0 */ - /* FIXME-- should check against triangle inequality, so that joints - are never too short to span shared base and platform sides */ - if (joints[0] <= 0.0 || - joints[1] <= 0.0 || - joints[2] <= 0.0 || - joints[3] <= 0.0 || - joints[4] <= 0.0 || - joints[5] <= 0.0) { - return -1; - } - - /* assign a,b,c to roll, pitch, yaw angles */ - q_RPY.r = pos->a * PM_PI / 180.0; - q_RPY.p = pos->b * PM_PI / 180.0; - q_RPY.y = pos->c * PM_PI / 180.0; - - /* Assign translation values in pos to q_trans */ - q_trans.x = pos->tran.x; - q_trans.y = pos->tran.y; - q_trans.z = pos->tran.z; - - /* Enter Newton-Raphson iterative method */ - while (iterate) { - /* check for large error and return error flag if no convergence */ - if ((conv_err > +(*haldata->max_error)) || - (conv_err < -(*haldata->max_error))) { - /* we can't converge */ - *haldata->fwd_kins_fail = 1; - return -2; - }; - - iteration++; - - /* check iteration to see if the kinematics can reach the - convergence criterion and return error flag if it can't */ - if (iteration > *haldata->iter_limit) { - /* we can't converge */ - *haldata->fwd_kins_fail = 1; - return -5; - } + (void)fflags; + (void)iflags; - /* Convert q_RPY to Rotation Matrix */ - pmRpyMatConvert(&q_RPY, &RMatrix); + genhex_params_t params; + int result; + int i; - /* compute StrutLengthDiff[] by running inverse kins on Cartesian - estimate to get joint estimate, subtract joints to get joint deltas, - and compute inv J while we're at it */ - for (i = 0; i < NUM_STRUTS; i++) { - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); - pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); - if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - *haldata->fwd_kins_fail = 1; - return -1; - } - pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - - if (*haldata->screw_lead != 0.0) { - /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - /* define corrected joint lengths */ - InvKinStrutLength += corr; - } - - StrutLengthDiff[i] = InvKinStrutLength - joints[i]; - - /* Determine RMatrix_a_cross_strut */ - pmCartCartCross(&RMatrix_a, &InvKinStrutVectUnit, &RMatrix_a_cross_Strut); - - /* Build Inverse Jacobian Matrix */ - InverseJacobian[i][0] = InvKinStrutVectUnit.x; - InverseJacobian[i][1] = InvKinStrutVectUnit.y; - InverseJacobian[i][2] = InvKinStrutVectUnit.z; - InverseJacobian[i][3] = RMatrix_a_cross_Strut.x; - InverseJacobian[i][4] = RMatrix_a_cross_Strut.y; - InverseJacobian[i][5] = RMatrix_a_cross_Strut.z; - } - - /* invert Inverse Jacobian */ - MatInvert(InverseJacobian, Jacobian); - - /* multiply Jacobian by LegLengthDiff */ - MatMult(Jacobian, StrutLengthDiff, delta); + genhex_read_hal_pins(¶ms); - /* subtract delta from last iterations pos values */ - q_trans.x -= delta[0]; - q_trans.y -= delta[1]; - q_trans.z -= delta[2]; - q_RPY.r -= delta[3]; - q_RPY.p -= delta[4]; - q_RPY.y -= delta[5]; + result = genhex_fwd(¶ms, joints, pos); - /* determine value of conv_error (used to determine if no convergence) */ - conv_err = 0.0; - for (i = 0; i < NUM_STRUTS; i++) { - conv_err += fabs(StrutLengthDiff[i]); + if (result != 0) { + *haldata->fwd_kins_fail = 1; + return result; } - /* enter loop to determine if a strut needs another iteration */ - iterate = 0; /*assume iteration is done */ + /* update HAL output pins from params */ + *haldata->last_iter = params.last_iterations; + if (params.last_iterations > *haldata->max_iter) { + *haldata->max_iter = params.last_iterations; + } for (i = 0; i < NUM_STRUTS; i++) { - if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { - iterate = 1; - } + *haldata->correction[i] = params.correction[i]; } - } /* exit Newton-Raphson Iterative loop */ + *haldata->fwd_kins_fail = 0; - /* assign r,p,y to a,b,c */ - pos->a = q_RPY.r * 180.0 / PM_PI; - pos->b = q_RPY.p * 180.0 / PM_PI; - pos->c = q_RPY.y * 180.0 / PM_PI; + genhex_gui_forward_kins(pos); - /* assign q_trans to pos */ - pos->tran.x = q_trans.x; - pos->tran.y = q_trans.y; - pos->tran.z = q_trans.z; - - *haldata->last_iter = iteration; - - if (iteration > *haldata->max_iter){ - *haldata->max_iter = iteration; - } - *haldata->fwd_kins_fail = 0; - - genhex_gui_forward_kins(pos); - - return 0; + return 0; } // genhexKinematicsForward() @@ -493,51 +462,23 @@ static int genhexKinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { - (void)iflags; - (void)fflags; - - PmCartesian aw, temp; - PmCartesian InvKinStrutVect, InvKinStrutVectUnit; - PmRotationMatrix RMatrix; - PmRpy rpy; - int i; - double InvKinStrutLength, corr; - - genhex_read_hal_pins(); - - /* define Rotation Matrix */ - rpy.r = pos->a * PM_PI / 180.0; - rpy.p = pos->b * PM_PI / 180.0; - rpy.y = pos->c * PM_PI / 180.0; - pmRpyMatConvert(&rpy, &RMatrix); - - /* enter for loop to calculate joints (strut lengths) */ - for (i = 0; i < NUM_STRUTS; i++) { - /* convert location of platform strut end from platform - to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); - pmCartCartAdd(&pos->tran, &temp, &aw); - - /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); - pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - - if (*haldata->screw_lead != 0.0) { - /* enable strut length correction */ - /* define unit strut vector */ - if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - return -1; - } - /* define correction value and corrected joint lengths */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - *haldata->correction[i] = corr; - InvKinStrutLength += corr; - } + (void)iflags; + (void)fflags; + + genhex_params_t params; + int result; + int i; - joints[i] = InvKinStrutLength; - } + genhex_read_hal_pins(¶ms); - return 0; + result = genhex_inv(¶ms, pos, joints); + + /* update HAL output pins from params */ + for (i = 0; i < NUM_STRUTS; i++) { + *haldata->correction[i] = params.correction[i]; + } + + return result; } //genhexKinematicsInverse() static @@ -737,3 +678,124 @@ int switchkinsSetup(kparms* kp, return 0; } //switchkinsSetup() + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +static void nonrt_build_genhex(const kinematics_params_t *kp, genhex_params_t *p) +{ + int i; + for (i = 0; i < GENHEX_NUM_STRUTS; i++) { + p->base[i].x = kp->params.genhex.basex[i]; + p->base[i].y = kp->params.genhex.basey[i]; + p->base[i].z = kp->params.genhex.basez[i]; + p->platform[i].x = kp->params.genhex.platformx[i]; + p->platform[i].y = kp->params.genhex.platformy[i]; + p->platform[i].z = kp->params.genhex.platformz[i]; + p->base_n[i].x = kp->params.genhex.basenx[i]; + p->base_n[i].y = kp->params.genhex.baseny[i]; + p->base_n[i].z = kp->params.genhex.basenz[i]; + p->platform_n[i].x = kp->params.genhex.platformnx[i]; + p->platform_n[i].y = kp->params.genhex.platformny[i]; + p->platform_n[i].z = kp->params.genhex.platformnz[i]; + p->correction[i] = 0.0; + } + p->conv_criterion = kp->params.genhex.conv_criterion; + p->iter_limit = kp->params.genhex.iter_limit; + p->max_error = kp->params.genhex.max_error; + p->tool_offset = kp->params.genhex.tool_offset; + p->spindle_offset = kp->params.genhex.spindle_offset; + p->screw_lead = kp->params.genhex.screw_lead; + p->last_iterations = 0; + p->max_iterations = kp->params.genhex.max_iter; +} + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + genhex_params_t p; + nonrt_build_genhex(kp, &p); + return genhex_fwd(&p, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + genhex_params_t p; + nonrt_build_genhex(kp, &p); + return genhex_inv(&p, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + int i; + char pin_name[64]; + (void)read_bit; + (void)read_s32; + + for (i = 0; i < KINS_GENHEX_NUM_STRUTS; i++) { + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base.%d.x", i); + read_float(pin_name, &kp->params.genhex.basex[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base.%d.y", i); + read_float(pin_name, &kp->params.genhex.basey[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base.%d.z", i); + read_float(pin_name, &kp->params.genhex.basez[i]); + + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform.%d.x", i); + read_float(pin_name, &kp->params.genhex.platformx[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform.%d.y", i); + read_float(pin_name, &kp->params.genhex.platformy[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform.%d.z", i); + read_float(pin_name, &kp->params.genhex.platformz[i]); + + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base-n.%d.x", i); + read_float(pin_name, &kp->params.genhex.basenx[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base-n.%d.y", i); + read_float(pin_name, &kp->params.genhex.baseny[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.base-n.%d.z", i); + read_float(pin_name, &kp->params.genhex.basenz[i]); + + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform-n.%d.x", i); + read_float(pin_name, &kp->params.genhex.platformnx[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform-n.%d.y", i); + read_float(pin_name, &kp->params.genhex.platformny[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genhexkins.platform-n.%d.z", i); + read_float(pin_name, &kp->params.genhex.platformnz[i]); + } + + read_float("genhexkins.convergence-criterion", &kp->params.genhex.conv_criterion); + read_float("genhexkins.max-error", &kp->params.genhex.max_error); + read_float("genhexkins.tool-offset", &kp->params.genhex.tool_offset); + read_float("genhexkins.spindle-offset", &kp->params.genhex.spindle_offset); + read_float("genhexkins.screw-lead", &kp->params.genhex.screw_lead); + + /* iter_limit and max_iter are u32 pins - read via read_s32 */ + { + int val; + if (read_s32) { + if (read_s32("genhexkins.limit-iterations", &val) == 0) + kp->params.genhex.iter_limit = (unsigned int)val; + if (read_s32("genhexkins.max-iterations", &val) == 0) + kp->params.genhex.max_iter = (unsigned int)val; + } + } + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 261091cf243..d859ec8ae53 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -33,9 +33,6 @@ * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR */ -#include "rtapi_math.h" -#include "gotypes.h" /* go_result, go_integer */ -#include "gomath.h" /* go_pose */ #include "genserkins.h" /* these decls */ #include "kinematics.h" #include "hal.h" @@ -48,8 +45,8 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2576 - #pragma GCC diagnostic warning "-Wframe-larger-than=2600" +// genserKinematicsInverse() is 3168 (increased with 9D kinematics) + #pragma GCC diagnostic warning "-Wframe-larger-than=3200" #endif static struct haldata { @@ -102,13 +99,6 @@ int genser_kin_init(void) { return GO_RESULT_OK; } -/* compute the forward jacobian function: - the jacobian is a linear approximation of the kinematics function. - It is calculated using derivation of the position transformation matrix, - and usually used for feeding velocities through it. - It is analytically possible to calculate the inverse of the jacobian - (sometimes only the pseudoinverse) and to use that for the inverse kinematics. -*/ int compute_jfwd(go_link * link_params, int link_number, go_matrix * Jfwd, @@ -124,17 +114,19 @@ int compute_jfwd(go_link * link_params, go_vector P_ip1_i[3]; int row, col; - /* init matrices to possibly smaller size */ go_matrix_init(Jv, Jvstg, 3, link_number); go_matrix_init(Jw, Jwstg, 3, link_number); go_matrix_init(R_i_ip1, R_i_ip1stg, 3, 3); go_matrix_init(scratch, scratchstg, 3, link_number); go_matrix_init(R_inv, R_invstg, 3, 3); - Jv.el[0][0] = 0, Jv.el[1][0] = 0, Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); - Jw.el[0][0] = 0, Jw.el[1][0] = 0, Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); + Jv.el[0][0] = 0; + Jv.el[1][0] = 0; + Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); + Jw.el[0][0] = 0; + Jw.el[1][0] = 0; + Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); - /* initialize inverse rotational transform */ if (GO_LINK_DH == link_params[0].type) { go_dh_pose_convert(&link_params[0].u.dh, &pose); } else if (GO_LINK_PP == link_params[0].type) { @@ -146,7 +138,6 @@ int compute_jfwd(go_link * link_params, *T_L_0 = pose; for (col = 1; col < link_number; col++) { - /* T_ip1_i */ if (GO_LINK_DH == link_params[col].type) { go_dh_pose_convert(&link_params[col].u.dh, &pose); } else if (GO_LINK_PP == link_params[col].type) { @@ -159,14 +150,18 @@ int compute_jfwd(go_link * link_params, go_quat_inv(&pose.rot, &quat); go_quat_matrix_convert(&quat, &R_i_ip1); - /* Jv */ go_matrix_vector_cross(&Jw, P_ip1_i, &scratch); go_matrix_matrix_add(&Jv, &scratch, &scratch); go_matrix_matrix_mult(&R_i_ip1, &scratch, &Jv); - Jv.el[0][col] = 0, Jv.el[1][col] = 0, Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); - /* Jw */ + Jv.el[0][col] = 0; + Jv.el[1][col] = 0; + Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); + go_matrix_matrix_mult(&R_i_ip1, &Jw, &Jw); - Jw.el[0][col] = 0, Jw.el[1][col] = 0, Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); + Jw.el[0][col] = 0; + Jw.el[1][col] = 0; + Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); + if (GO_LINK_DH == link_params[col].type) { go_dh_pose_convert(&link_params[col].u.dh, &pose); } else if (GO_LINK_PP == link_params[col].type) { @@ -177,12 +172,10 @@ int compute_jfwd(go_link * link_params, go_pose_pose_mult(T_L_0, &pose, T_L_0); } - /* rotate back into {0} frame */ go_quat_matrix_convert(&T_L_0->rot, &R_inv); go_matrix_matrix_mult(&R_inv, &Jv, &Jv); go_matrix_matrix_mult(&R_inv, &Jw, &Jw); - /* put Jv atop Jw in J */ for (row = 0; row < 6; row++) { for (col = 0; col < link_number; col++) { if (row < 3) { @@ -196,20 +189,16 @@ int compute_jfwd(go_link * link_params, return GO_RESULT_OK; } -/* compute the inverse of the jacobian matrix */ int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv) { int retval; GO_MATRIX_DECLARE(JT, JTstg, GENSER_MAX_JOINTS, 6); - /* compute inverse, or pseudo-inverse */ if (Jfwd->rows == Jfwd->cols) { retval = go_matrix_inv(Jfwd, Jinv); if (GO_RESULT_OK != retval) return retval; } else if (Jfwd->rows < Jfwd->cols) { - /* underdetermined, optimize on smallest sum of square of speeds */ - /* JT(JJT)inv */ GO_MATRIX_DECLARE(JJT, JJTstg, 6, 6); go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); @@ -221,8 +210,6 @@ int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv) return retval; go_matrix_matrix_mult(&JT, &JJT, Jinv); } else { - /* overdetermined, do least-squares best fit */ - /* (JTJ)invJT */ GO_MATRIX_DECLARE(JTJ, JTJstg, GENSER_MAX_JOINTS, GENSER_MAX_JOINTS); go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); @@ -238,6 +225,28 @@ int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv) return GO_RESULT_OK; } +static int fwd_internal_links(go_link *links, + int link_num, + const go_real *joints, + go_pose *pos) +{ + go_link linkout[GENSER_MAX_JOINTS]; + int link; + int retval; + + for (link = 0; link < link_num; link++) { + retval = go_link_joint_set(&links[link], joints[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return retval; + } + + retval = go_link_pose_build(linkout, link_num, pos); + if (GO_RESULT_OK != retval) + return retval; + + return GO_RESULT_OK; +} + int genser_kin_jac_inv(void *kins, const go_pose * pos, const go_screw * vel, const go_real * joints, go_real * jointvels) @@ -395,24 +404,10 @@ int genserKinematicsForward(const double *joint, int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) { genser_struct *genser = kins; - go_link linkout[GENSER_MAX_JOINTS] = {}; - - int link; - int retval; genser_kin_init(); - for (link = 0; link < genser->link_num; link++) { - retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); - if (GO_RESULT_OK != retval) - return retval; - } - - retval = go_link_pose_build(linkout, genser->link_num, pos); - if (GO_RESULT_OK != retval) - return retval; - - return GO_RESULT_OK; + return fwd_internal_links(genser->links, genser->link_num, joints, pos); } int genserKinematicsInverse(const EmcPose * world, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 0c75379246f..1055b442478 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -34,7 +34,7 @@ frame-larger-than: #if __GNUC__ && !defined(__clang__) // genserKinematicsInverse() is 5104 with buster amd64 gcc 8.3.0-6 //#pragma GCC diagnostic error "-Wframe-larger-than=6000" - #pragma GCC diagnostic warning "-Wframe-larger-than=6000" + #pragma GCC diagnostic warning "-Wframe-larger-than=11000" #endif #include "rtapi.h" @@ -76,3 +76,373 @@ int switchkinsSetup(kparms* kp, return 0; } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * + * Math functions derived from genserkins_math.h (Jacobian-based + * iterative inverse solver using Denavit-Hartenberg parameters). + * ======================================================================== */ +#include "kinematics_params.h" +#include "emcpos.h" + +#ifndef PM_PI +#define PM_PI 3.14159265358979323846 +#endif + +#define NONRT_GENSERKINS_MAX_JOINTS 9 + +typedef struct { + int link_num; + double a[NONRT_GENSERKINS_MAX_JOINTS]; + double alpha[NONRT_GENSERKINS_MAX_JOINTS]; + double d[NONRT_GENSERKINS_MAX_JOINTS]; + int unrotate[NONRT_GENSERKINS_MAX_JOINTS]; + unsigned int max_iterations; + unsigned int last_iterations; +} nonrt_genserkins_params_t; + +typedef struct { + go_link links[NONRT_GENSERKINS_MAX_JOINTS]; + int link_num; + unsigned int iterations; +} nonrt_genserkins_state_t; + +static int nonrt_genserkins_init_state(nonrt_genserkins_state_t *state, + const nonrt_genserkins_params_t *params) +{ + int i; + state->link_num = params->link_num; + state->iterations = 0; + for (i = 0; i < NONRT_GENSERKINS_MAX_JOINTS; i++) { + state->links[i].u.dh.a = params->a[i]; + state->links[i].u.dh.alpha = params->alpha[i]; + state->links[i].u.dh.d = params->d[i]; + state->links[i].u.dh.theta = 0.0; + state->links[i].type = GO_LINK_DH; + state->links[i].quantity = GO_QUANTITY_ANGLE; + } + return GO_RESULT_OK; +} + +static int nonrt_genserkins_compute_jfwd(go_link *link_params, + int link_number, + go_matrix *Jfwd, + go_pose *T_L_0) +{ + GO_MATRIX_DECLARE(Jv, Jvstg, 3, NONRT_GENSERKINS_MAX_JOINTS); + GO_MATRIX_DECLARE(Jw, Jwstg, 3, NONRT_GENSERKINS_MAX_JOINTS); + GO_MATRIX_DECLARE(R_i_ip1, R_i_ip1stg, 3, 3); + GO_MATRIX_DECLARE(scratch, scratchstg, 3, NONRT_GENSERKINS_MAX_JOINTS); + GO_MATRIX_DECLARE(R_inv, R_invstg, 3, 3); + go_pose pose; + go_quat quat; + go_vector P_ip1_i[3]; + int row, col; + + go_matrix_init(Jv, Jvstg, 3, link_number); + go_matrix_init(Jw, Jwstg, 3, link_number); + go_matrix_init(R_i_ip1, R_i_ip1stg, 3, 3); + go_matrix_init(scratch, scratchstg, 3, link_number); + go_matrix_init(R_inv, R_invstg, 3, 3); + + Jv.el[0][0] = 0; Jv.el[1][0] = 0; + Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); + Jw.el[0][0] = 0; Jw.el[1][0] = 0; + Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); + + if (GO_LINK_DH == link_params[0].type) + go_dh_pose_convert(&link_params[0].u.dh, &pose); + else if (GO_LINK_PP == link_params[0].type) + pose = link_params[0].u.pp.pose; + else + return GO_RESULT_IMPL_ERROR; + + *T_L_0 = pose; + + for (col = 1; col < link_number; col++) { + if (GO_LINK_DH == link_params[col].type) + go_dh_pose_convert(&link_params[col].u.dh, &pose); + else if (GO_LINK_PP == link_params[col].type) + pose = link_params[col].u.pp.pose; + else + return GO_RESULT_IMPL_ERROR; + + go_cart_vector_convert(&pose.tran, P_ip1_i); + go_quat_inv(&pose.rot, &quat); + go_quat_matrix_convert(&quat, &R_i_ip1); + + go_matrix_vector_cross(&Jw, P_ip1_i, &scratch); + go_matrix_matrix_add(&Jv, &scratch, &scratch); + go_matrix_matrix_mult(&R_i_ip1, &scratch, &Jv); + Jv.el[0][col] = 0; Jv.el[1][col] = 0; + Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); + + go_matrix_matrix_mult(&R_i_ip1, &Jw, &Jw); + Jw.el[0][col] = 0; Jw.el[1][col] = 0; + Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); + + if (GO_LINK_DH == link_params[col].type) + go_dh_pose_convert(&link_params[col].u.dh, &pose); + else if (GO_LINK_PP == link_params[col].type) + pose = link_params[col].u.pp.pose; + else + return GO_RESULT_IMPL_ERROR; + go_pose_pose_mult(T_L_0, &pose, T_L_0); + } + + go_quat_matrix_convert(&T_L_0->rot, &R_inv); + go_matrix_matrix_mult(&R_inv, &Jv, &Jv); + go_matrix_matrix_mult(&R_inv, &Jw, &Jw); + + for (row = 0; row < 6; row++) + for (col = 0; col < link_number; col++) + Jfwd->el[row][col] = (row < 3) ? Jv.el[row][col] : Jw.el[row-3][col]; + + return GO_RESULT_OK; +} + +static int nonrt_genserkins_compute_jinv(go_matrix *Jfwd, go_matrix *Jinv) +{ + int retval; + GO_MATRIX_DECLARE(JT, JTstg, NONRT_GENSERKINS_MAX_JOINTS, 6); + + if (Jfwd->rows == Jfwd->cols) { + retval = go_matrix_inv(Jfwd, Jinv); + if (GO_RESULT_OK != retval) return retval; + } else if (Jfwd->rows < Jfwd->cols) { + GO_MATRIX_DECLARE(JJT, JJTstg, 6, 6); + go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); + go_matrix_init(JJT, JJTstg, Jfwd->rows, Jfwd->rows); + go_matrix_transpose(Jfwd, &JT); + go_matrix_matrix_mult(Jfwd, &JT, &JJT); + retval = go_matrix_inv(&JJT, &JJT); + if (GO_RESULT_OK != retval) return retval; + go_matrix_matrix_mult(&JT, &JJT, Jinv); + } else { + GO_MATRIX_DECLARE(JTJ, JTJstg, NONRT_GENSERKINS_MAX_JOINTS, NONRT_GENSERKINS_MAX_JOINTS); + go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); + go_matrix_init(JTJ, JTJstg, Jfwd->cols, Jfwd->cols); + go_matrix_transpose(Jfwd, &JT); + go_matrix_matrix_mult(&JT, Jfwd, &JTJ); + retval = go_matrix_inv(&JTJ, &JTJ); + if (GO_RESULT_OK != retval) return retval; + go_matrix_matrix_mult(&JTJ, &JT, Jinv); + } + return GO_RESULT_OK; +} + +static int nonrt_genserkins_fwd_internal(nonrt_genserkins_state_t *state, + const go_real *joints, + go_pose *pos) +{ + go_link linkout[NONRT_GENSERKINS_MAX_JOINTS]; + int link, retval; + for (link = 0; link < state->link_num; link++) { + retval = go_link_joint_set(&state->links[link], joints[link], &linkout[link]); + if (GO_RESULT_OK != retval) return retval; + } + retval = go_link_pose_build(linkout, state->link_num, pos); + if (GO_RESULT_OK != retval) return retval; + return GO_RESULT_OK; +} + +static int nonrt_genserkins_fwd(nonrt_genserkins_params_t *params, + const double *joints, + EmcPose *world) +{ + nonrt_genserkins_state_t state; + go_pose pos; + go_rpy rpy; + go_real jcopy[NONRT_GENSERKINS_MAX_JOINTS]; + int i, retval; + + nonrt_genserkins_init_state(&state, params); + + for (i = 0; i < state.link_num && i < 6; i++) { + jcopy[i] = joints[i] * PM_PI / 180.0; + if (i > 0 && params->unrotate[i]) + jcopy[i] -= params->unrotate[i] * jcopy[i-1]; + } + + retval = nonrt_genserkins_fwd_internal(&state, jcopy, &pos); + if (retval != GO_RESULT_OK) return retval; + + retval = go_quat_rpy_convert(&pos.rot, &rpy); + if (retval != GO_RESULT_OK) return retval; + + world->tran.x = pos.tran.x; world->tran.y = pos.tran.y; world->tran.z = pos.tran.z; + world->a = rpy.r * 180.0 / PM_PI; + world->b = rpy.p * 180.0 / PM_PI; + world->c = rpy.y * 180.0 / PM_PI; + world->u = (params->link_num > 6) ? joints[6] : 0.0; + world->v = (params->link_num > 7) ? joints[7] : 0.0; + world->w = (params->link_num > 8) ? joints[8] : 0.0; + return 0; +} + +static int nonrt_genserkins_inv(nonrt_genserkins_params_t *params, + const EmcPose *world, + double *joints) +{ + nonrt_genserkins_state_t state; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, NONRT_GENSERKINS_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, NONRT_GENSERKINS_MAX_JOINTS, 6); + go_pose T_L_0; + go_real dvw[6], jest[NONRT_GENSERKINS_MAX_JOINTS], dj[NONRT_GENSERKINS_MAX_JOINTS]; + go_pose pest, pestinv, Tdelta, target_pos; + go_rpy rpy; + go_rvec rvec; + go_cart cart; + go_link linkout[NONRT_GENSERKINS_MAX_JOINTS]; + int link, smalls, retval; + unsigned int iter; + + nonrt_genserkins_init_state(&state, params); + + rpy.y = world->c * PM_PI / 180.0; + rpy.p = world->b * PM_PI / 180.0; + rpy.r = world->a * PM_PI / 180.0; + go_rpy_quat_convert(&rpy, &target_pos.rot); + target_pos.tran.x = world->tran.x; + target_pos.tran.y = world->tran.y; + target_pos.tran.z = world->tran.z; + + go_matrix_init(Jfwd, Jfwd_stg, 6, state.link_num); + go_matrix_init(Jinv, Jinv_stg, state.link_num, 6); + + for (link = 0; link < state.link_num && link < 6; link++) + jest[link] = joints[link] * PM_PI / 180.0; + + for (iter = 0; iter < params->max_iterations; iter++) { + params->last_iterations = iter; + + for (link = 0; link < state.link_num; link++) + go_link_joint_set(&state.links[link], jest[link], &linkout[link]); + + retval = nonrt_genserkins_compute_jfwd(linkout, state.link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) return -1; + + retval = nonrt_genserkins_compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) return -1; + + nonrt_genserkins_fwd_internal(&state, jest, &pest); + go_pose_inv(&pest, &pestinv); + go_pose_pose_mult(&pestinv, &target_pos, &Tdelta); + + go_quat_cart_mult(&pest.rot, &Tdelta.tran, &cart); + dvw[0] = cart.x; dvw[1] = cart.y; dvw[2] = cart.z; + + go_quat_rvec_convert(&Tdelta.rot, &rvec); + cart.x = rvec.x; cart.y = rvec.y; cart.z = rvec.z; + go_quat_cart_mult(&pest.rot, &cart, &cart); + dvw[3] = cart.x; dvw[4] = cart.y; dvw[5] = cart.z; + + go_matrix_vector_mult(&Jinv, dvw, dj); + + smalls = 0; + for (link = 0; link < state.link_num; link++) { + if (GO_QUANTITY_LENGTH == linkout[link].quantity) { + if (GO_TRAN_SMALL(dj[link])) smalls++; + } else { + if (GO_ROT_SMALL(dj[link])) smalls++; + } + } + + if (smalls == state.link_num) { + for (link = 0; link < state.link_num && link < 6; link++) { + joints[link] = jest[link] * 180.0 / PM_PI; + if (link > 0 && params->unrotate[link]) + joints[link] += params->unrotate[link] * joints[link-1]; + } + if (params->link_num > 6) joints[6] = world->u; + if (params->link_num > 7) joints[7] = world->v; + if (params->link_num > 8) joints[8] = world->w; + return 0; + } + + for (link = 0; link < state.link_num; link++) + jest[link] += dj[link]; + } + return -1; +} + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + nonrt_genserkins_params_t p; + int i; + + p.link_num = kp->params.genser.link_num; + p.max_iterations = kp->params.genser.max_iterations; + p.last_iterations = 0; + for (i = 0; i < NONRT_GENSERKINS_MAX_JOINTS; i++) { + p.a[i] = kp->params.genser.a[i]; + p.alpha[i] = kp->params.genser.alpha[i]; + p.d[i] = kp->params.genser.d[i]; + p.unrotate[i] = kp->params.genser.unrotate[i]; + } + + return nonrt_genserkins_fwd(&p, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + nonrt_genserkins_params_t p; + int i; + + p.link_num = kp->params.genser.link_num; + p.max_iterations = kp->params.genser.max_iterations; + p.last_iterations = 0; + for (i = 0; i < NONRT_GENSERKINS_MAX_JOINTS; i++) { + p.a[i] = kp->params.genser.a[i]; + p.alpha[i] = kp->params.genser.alpha[i]; + p.d[i] = kp->params.genser.d[i]; + p.unrotate[i] = kp->params.genser.unrotate[i]; + } + + return nonrt_genserkins_inv(&p, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + int i; + char pin_name[64]; + (void)read_bit; + + for (i = 0; i < KINS_GENSER_MAX_JOINTS; i++) { + rtapi_snprintf(pin_name, sizeof(pin_name), "genserkins.A-%d", i); + read_float(pin_name, &kp->params.genser.a[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genserkins.ALPHA-%d", i); + read_float(pin_name, &kp->params.genser.alpha[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "genserkins.D-%d", i); + read_float(pin_name, &kp->params.genser.d[i]); + } + + if (read_s32) { + int val; + for (i = 0; i < KINS_GENSER_MAX_JOINTS; i++) { + rtapi_snprintf(pin_name, sizeof(pin_name), "genserkins.UNROTATE-%d", i); + if (read_s32(pin_name, &val) == 0) + kp->params.genser.unrotate[i] = val; + } + } + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 87ce7aaf3bc..681cb483a20 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -102,6 +102,11 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* Return the kinematics module name (e.g., "trivkins", "5axiskins"). + This must match the module filename without extension. + Returns NULL if no name is available. */ +extern const char* kinematicsGetName(void); + /* parameters for use with switchkins.c */ typedef struct kinematics_parms { char* sparm; // module string parameter passed to kins @@ -184,7 +189,7 @@ extern int userkKinematicsInverse(const struct EmcPose * world, double *joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); -#endif + //********************************************************************* // xyzac,xyzbc; extern int trtKinematicsSetup(const int comp_id, @@ -213,3 +218,5 @@ extern int xyzbcKinematicsInverse(const EmcPose * pos, KINEMATICS_FORWARD_FLAGS * fflags); //********************************************************************* + +#endif /* KINEMATICS_H */ diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h deleted file mode 100644 index eace3cee862..00000000000 --- a/src/emc/kinematics/lineardeltakins-common.h +++ /dev/null @@ -1,140 +0,0 @@ -#ifndef LINUXCNCLINEARDELTAKINS_COMMON_H -#define LINUXCNCLINEARDELTAKINS_COMMON_H -// Copyright 2013 Jeff Epler -// -// This program is free software; you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation; either version 2 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -/* - * Kinematics for a rostock-style delta robot - * - * Towers 0, 1 and 2 are spaced at 120 degrees around the origin - * at distance R. A rod of length L (L > R) connects each tower to the - * platform. Tower 0 is at (0,R). (note: this is not at zero radians!) - * - * ABCUVW coordinates are passed through in joints[3..8]. - * - * L is like DELTA_DIAGONAL_ROD and R is like DELTA_RADIUS in - * Marlin---remember to account for the effector and carriage offsets - * when changing from the default. - */ - -// common routines used by the userspace kinematics and the realtime kinematics -// user must include a math.h-type header first -// Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 -#include "emcpos.h" - -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; - -#define SQ3 (sqrt(3)) - -#define SIN_60 (SQ3/2) -#define COS_60 (.5) - -static double sq(double x) { return x*x; } - -static void set_geometry(double r_, double l_) -{ - if(L == l_ && R == r_) return; - - L = l_; - R = r_; - - L2 = sq(L); - - Ax = 0.0; - Ay = R; - - Bx = -SIN_60 * R; - By = -COS_60 * R; - - Cx = SIN_60 * R; - Cy = -COS_60 * R; -} - -static int kinematics_inverse(const EmcPose *pos, double *joints) -{ - double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); - joints[3] = pos->a; - joints[4] = pos->b; - joints[5] = pos->c; - joints[6] = pos->u; - joints[7] = pos->v; - joints[8] = pos->w; - - return isnan(joints[0]) || isnan(joints[1]) || isnan(joints[2]) - ? -1 : 0; -} - -static int kinematics_forward(const double *joints, EmcPose *pos) -{ - double q1 = joints[0]; - double q2 = joints[1]; - double q3 = joints[2]; - - double den = (By-Ay)*Cx-(Cy-Ay)*Bx; - - double w1 = Ay*Ay + q1*q1; // n.b. assumption that Ax is 0 all through here - double w2 = Bx*Bx + By*By + q2*q2; - double w3 = Cx*Cx + Cy*Cy + q3*q3; - - double a1 = (q2-q1)*(Cy-Ay)-(q3-q1)*(By-Ay); - double b1 = -((w2-w1)*(Cy-Ay)-(w3-w1)*(By-Ay))/2.0; - - double a2 = -(q2-q1)*Cx+(q3-q1)*Bx; - double b2 = ((w2-w1)*Cx - (w3-w1)*Bx)/2.0; - - // a*z^2 + b*z + c = 0 - double a = a1*a1 + a2*a2 + den*den; - double b = 2*(a1*b1 + a2*(b2-Ay*den) - q1*den*den); - double c = (b2-Ay*den)*(b2-Ay*den) + b1*b1 + den*den*(q1*q1 - L*L); - - double discr = b*b - 4.0*a*c; - if (discr < 0) return -1; // non-existing point - - double z = -0.5*(b+sqrt(discr))/a; - pos->tran.z = z; - pos->tran.x = (a1*z + b1)/den; - pos->tran.y = (a2*z + b2)/den; - pos->a = joints[3]; - pos->b = joints[4]; - pos->c = joints[5]; - pos->u = joints[6]; - pos->v = joints[7]; - pos->w = joints[8]; - - return 0; -} - -// Default values which may correspond to someone's linear delta robot. To -// change these, use halcmd setp rather than rebuilding the software. - -// Center-to-center distance of the holes in the diagonal push rods. -#define DELTA_DIAGONAL_ROD 269.0 // mm - -// Horizontal offset from middle of printer to smooth rod center. -#define DELTA_SMOOTH_ROD_OFFSET 198.25 // mm - -// Horizontal offset of the universal joints on the end effector. -#define DELTA_EFFECTOR_OFFSET 33.0 // mm - -// Horizontal offset of the universal joints on the carriages. -#define DELTA_CARRIAGE_OFFSET 35.0 // mm - -// Effective horizontal distance bridged by diagonal push rods. -#define DELTA_RADIUS (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET) -#endif diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 32e00385c9a..428bcde842a 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -19,7 +19,139 @@ #include "rtapi_math.h" #include "rtapi_app.h" -#include "lineardeltakins-common.h" +/* ---- Math types and functions (from lineardeltakins_math.h) ---- */ + +#ifndef M_SQRT3 +#define M_SQRT3 1.7320508075688772935 +#endif + +#define LINDELTA_SIN_60 (M_SQRT3/2.0) +#define LINDELTA_COS_60 (0.5) + +typedef struct { + double radius; + double rod_length; +} lineardelta_params_t; + +typedef struct { + double ax, ay; + double bx, by; + double cx, cy; + double l2; +} lineardelta_geometry_t; + +#ifndef lindelta_sq +#define lindelta_sq(x) ((x)*(x)) +#endif + +static void lineardelta_compute_geometry(const lineardelta_params_t *params, + lineardelta_geometry_t *geom) +{ + double R = params->radius; + double L = params->rod_length; + + geom->ax = 0.0; + geom->ay = R; + + geom->bx = -LINDELTA_SIN_60 * R; + geom->by = -LINDELTA_COS_60 * R; + + geom->cx = LINDELTA_SIN_60 * R; + geom->cy = -LINDELTA_COS_60 * R; + + geom->l2 = lindelta_sq(L); +} + +static int lineardelta_inverse_math(const lineardelta_params_t *params, + const EmcPose *world, double *joints) +{ + lineardelta_geometry_t geom; + double x = world->tran.x; + double y = world->tran.y; + double z = world->tran.z; + + lineardelta_compute_geometry(params, &geom); + + joints[0] = z + sqrt(geom.l2 - lindelta_sq(geom.ax - x) - lindelta_sq(geom.ay - y)); + joints[1] = z + sqrt(geom.l2 - lindelta_sq(geom.bx - x) - lindelta_sq(geom.by - y)); + joints[2] = z + sqrt(geom.l2 - lindelta_sq(geom.cx - x) - lindelta_sq(geom.cy - y)); + joints[3] = world->a; + joints[4] = world->b; + joints[5] = world->c; + joints[6] = world->u; + joints[7] = world->v; + joints[8] = world->w; + + if (isnan(joints[0]) || isnan(joints[1]) || isnan(joints[2])) + return -1; + + return 0; +} + +static int lineardelta_forward_math(const lineardelta_params_t *params, + const double *joints, EmcPose *world) +{ + lineardelta_geometry_t geom; + double q1, q2, q3; + double den, w1, w2, w3; + double a1, b1, a2, b2; + double a, b, c, discr, z; + + lineardelta_compute_geometry(params, &geom); + + q1 = joints[0]; + q2 = joints[1]; + q3 = joints[2]; + + den = (geom.by - geom.ay) * geom.cx - (geom.cy - geom.ay) * geom.bx; + + /* n.b. assumption that Ax is 0 all through here */ + w1 = lindelta_sq(geom.ay) + lindelta_sq(q1); + w2 = lindelta_sq(geom.bx) + lindelta_sq(geom.by) + lindelta_sq(q2); + w3 = lindelta_sq(geom.cx) + lindelta_sq(geom.cy) + lindelta_sq(q3); + + a1 = (q2 - q1) * (geom.cy - geom.ay) - (q3 - q1) * (geom.by - geom.ay); + b1 = -((w2 - w1) * (geom.cy - geom.ay) - (w3 - w1) * (geom.by - geom.ay)) / 2.0; + + a2 = -(q2 - q1) * geom.cx + (q3 - q1) * geom.bx; + b2 = ((w2 - w1) * geom.cx - (w3 - w1) * geom.bx) / 2.0; + + /* a*z^2 + b*z + c = 0 */ + a = lindelta_sq(a1) + lindelta_sq(a2) + lindelta_sq(den); + b = 2 * (a1 * b1 + a2 * (b2 - geom.ay * den) - q1 * lindelta_sq(den)); + c = lindelta_sq(b2 - geom.ay * den) + lindelta_sq(b1) + + lindelta_sq(den) * (lindelta_sq(q1) - geom.l2); + + discr = lindelta_sq(b) - 4.0 * a * c; + if (discr < 0) + return -1; + + z = -0.5 * (b + sqrt(discr)) / a; + world->tran.z = z; + world->tran.x = (a1 * z + b1) / den; + world->tran.y = (a2 * z + b2) / den; + world->a = joints[3]; + world->b = joints[4]; + world->c = joints[5]; + world->u = joints[6]; + world->v = joints[7]; + world->w = joints[8]; + + return 0; +} + +/* Default values which may correspond to someone's linear delta robot. + * To change these, use halcmd setp rather than rebuilding the software. + */ +#define LINEARDELTA_DEFAULT_ROD_LENGTH 269.0 /* mm (DELTA_DIAGONAL_ROD) */ +#define LINEARDELTA_SMOOTH_ROD_OFFSET 198.25 /* mm */ +#define LINEARDELTA_EFFECTOR_OFFSET 33.0 /* mm */ +#define LINEARDELTA_CARRIAGE_OFFSET 35.0 /* mm */ +#define LINEARDELTA_DEFAULT_RADIUS (LINEARDELTA_SMOOTH_ROD_OFFSET - \ + LINEARDELTA_EFFECTOR_OFFSET - \ + LINEARDELTA_CARRIAGE_OFFSET) /* mm */ + +/* ---- End math functions ---- */ struct haldata { @@ -34,8 +166,10 @@ int kinematicsForward(const double * joints, KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - set_geometry(*haldata->r, *haldata->l); - return kinematics_forward(joints, pos); + lineardelta_params_t params; + params.radius = *haldata->r; + params.rod_length = *haldata->l; + return lineardelta_forward_math(¶ms, joints, pos); } int kinematicsInverse(const EmcPose *pos, double *joints, @@ -43,8 +177,10 @@ int kinematicsInverse(const EmcPose *pos, double *joints, KINEMATICS_FORWARD_FLAGS *fflags) { (void)iflags; (void)fflags; - set_geometry(*haldata->r, *haldata->l); - return kinematics_inverse(pos, joints); + lineardelta_params_t params; + params.radius = *haldata->r; + params.rod_length = *haldata->l; + return lineardelta_inverse_math(¶ms, pos, joints); } KINEMATICS_TYPE kinematicsType() @@ -74,8 +210,8 @@ int rtapi_app_main(void) if(retval == 0) { - *haldata->r = DELTA_RADIUS; - *haldata->l = DELTA_DIAGONAL_ROD; + *haldata->r = LINEARDELTA_DEFAULT_RADIUS; + *haldata->l = LINEARDELTA_DEFAULT_ROD_LENGTH; } if(retval == 0) @@ -91,8 +227,61 @@ void rtapi_app_exit(void) hal_exit(comp_id); } +const char* kinematicsGetName(void) { return "lineardeltakins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); + +/* ---- nonrt interface for userspace planner ---- */ + +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + lineardelta_params_t lp; + lp.radius = kp->params.lineardelta.radius; + lp.rod_length = kp->params.lineardelta.jointradius; + return lineardelta_forward_math(&lp, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + lineardelta_params_t lp; + lp.radius = kp->params.lineardelta.radius; + lp.rod_length = kp->params.lineardelta.jointradius; + return lineardelta_inverse_math(&lp, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + double fval; + (void)read_bit; + (void)read_s32; + if (read_float("lineardeltakins.R", &fval) == 0) + kp->params.lineardelta.radius = fval; + if (read_float("lineardeltakins.L", &fval) == 0) + kp->params.lineardelta.jointradius = fval; + return 0; +} + +int nonrt_is_identity(void) +{ + return 0; +} + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 3899842aff1..23c9c3595c9 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -18,14 +18,111 @@ #include #include using namespace boost::python; -#define isnan(x) std::isnan(x) -#include "lineardeltakins-common.h" +#include "emcpos.h" + +#ifndef M_SQRT3 +#define M_SQRT3 1.7320508075688772935 +#endif + +#define LINDELTA_SIN_60 (M_SQRT3/2.0) +#define LINDELTA_COS_60 (0.5) + +typedef struct { + double radius; + double rod_length; +} lineardelta_params_t; + +typedef struct { + double ax, ay; + double bx, by; + double cx, cy; + double l2; +} lineardelta_geometry_t; + +#define lindelta_sq(x) ((x)*(x)) + +static void lineardelta_compute_geometry(const lineardelta_params_t *params, + lineardelta_geometry_t *geom) +{ + double R = params->radius; + double L = params->rod_length; + geom->ax = 0.0; geom->ay = R; + geom->bx = -LINDELTA_SIN_60 * R; geom->by = -LINDELTA_COS_60 * R; + geom->cx = LINDELTA_SIN_60 * R; geom->cy = -LINDELTA_COS_60 * R; + geom->l2 = lindelta_sq(L); +} + +static int lineardelta_inverse_math(const lineardelta_params_t *params, + const EmcPose *world, double *joints) +{ + lineardelta_geometry_t geom; + double x = world->tran.x, y = world->tran.y, z = world->tran.z; + lineardelta_compute_geometry(params, &geom); + joints[0] = z + sqrt(geom.l2 - lindelta_sq(geom.ax - x) - lindelta_sq(geom.ay - y)); + joints[1] = z + sqrt(geom.l2 - lindelta_sq(geom.bx - x) - lindelta_sq(geom.by - y)); + joints[2] = z + sqrt(geom.l2 - lindelta_sq(geom.cx - x) - lindelta_sq(geom.cy - y)); + joints[3] = world->a; joints[4] = world->b; joints[5] = world->c; + joints[6] = world->u; joints[7] = world->v; joints[8] = world->w; + if (std::isnan(joints[0]) || std::isnan(joints[1]) || std::isnan(joints[2])) + return -1; + return 0; +} + +static int lineardelta_forward_math(const lineardelta_params_t *params, + const double *joints, EmcPose *world) +{ + lineardelta_geometry_t geom; + double q1, q2, q3, den, w1, w2, w3, a1, b1, a2, b2, a, b, c, discr, z; + lineardelta_compute_geometry(params, &geom); + q1 = joints[0]; q2 = joints[1]; q3 = joints[2]; + den = (geom.by - geom.ay) * geom.cx - (geom.cy - geom.ay) * geom.bx; + w1 = lindelta_sq(geom.ay) + lindelta_sq(q1); + w2 = lindelta_sq(geom.bx) + lindelta_sq(geom.by) + lindelta_sq(q2); + w3 = lindelta_sq(geom.cx) + lindelta_sq(geom.cy) + lindelta_sq(q3); + a1 = (q2 - q1) * (geom.cy - geom.ay) - (q3 - q1) * (geom.by - geom.ay); + b1 = -((w2 - w1) * (geom.cy - geom.ay) - (w3 - w1) * (geom.by - geom.ay)) / 2.0; + a2 = -(q2 - q1) * geom.cx + (q3 - q1) * geom.bx; + b2 = ((w2 - w1) * geom.cx - (w3 - w1) * geom.bx) / 2.0; + a = lindelta_sq(a1) + lindelta_sq(a2) + lindelta_sq(den); + b = 2 * (a1 * b1 + a2 * (b2 - geom.ay * den) - q1 * lindelta_sq(den)); + c = lindelta_sq(b2 - geom.ay * den) + lindelta_sq(b1) + + lindelta_sq(den) * (lindelta_sq(q1) - geom.l2); + discr = lindelta_sq(b) - 4.0 * a * c; + if (discr < 0) return -1; + z = -0.5 * (b + sqrt(discr)) / a; + world->tran.z = z; + world->tran.x = (a1 * z + b1) / den; + world->tran.y = (a2 * z + b2) / den; + world->a = joints[3]; world->b = joints[4]; world->c = joints[5]; + world->u = joints[6]; world->v = joints[7]; world->w = joints[8]; + return 0; +} + +#define LINEARDELTA_SMOOTH_ROD_OFFSET 198.25 +#define LINEARDELTA_EFFECTOR_OFFSET 33.0 +#define LINEARDELTA_CARRIAGE_OFFSET 35.0 +#define LINEARDELTA_DEFAULT_RADIUS (LINEARDELTA_SMOOTH_ROD_OFFSET - \ + LINEARDELTA_EFFECTOR_OFFSET - \ + LINEARDELTA_CARRIAGE_OFFSET) +#define LINEARDELTA_DEFAULT_ROD_LENGTH 269.0 + +/* Module-level geometry parameters */ +static lineardelta_params_t g_params = { + LINEARDELTA_DEFAULT_RADIUS, + LINEARDELTA_DEFAULT_ROD_LENGTH +}; + +static void py_set_geometry(double r, double l) +{ + g_params.radius = r; + g_params.rod_length = l; +} static object forward(double j0, double j1, double j2) { - double joints[9] = {j0, j1, j2}; + double joints[9] = {j0, j1, j2, 0, 0, 0, 0, 0, 0}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward_math(&g_params, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +132,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse_math(&g_params, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,13 +140,12 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(g_params.radius, g_params.rod_length); } BOOST_PYTHON_MODULE(lineardeltakins) { - set_geometry(DELTA_RADIUS, DELTA_DIAGONAL_ROD); - def("set_geometry", set_geometry); + def("set_geometry", py_set_geometry); def("get_geometry", get_geometry); def("forward", forward); def("inverse", inverse); diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index ab43b14122b..5b7058015ac 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -23,45 +23,34 @@ #include "rtapi.h" #include "rtapi_math.h" -#define d2r(d) ((d)*PM_PI/180.0) -#define r2d(r) ((r)*180.0/PM_PI) +/* ======================================================================== + * Internal math (was in maxkins_math.h) + * ======================================================================== */ -#ifndef hypot -#define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) +#ifndef PM_PI +#define PM_PI 3.14159265358979323846 #endif +#define maxkins_d2r(d) ((d)*PM_PI/180.0) +#define maxkins_r2d(r) ((r)*180.0/PM_PI) +#define maxkins_hypot(a,b) (sqrt((a)*(a)+(b)*(b))) -struct haldata { - hal_float_t *pivot_length; - hal_bit_t *conventional_directions; //default is false -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int maxkins_forward_impl(double pivot_length, int conventional_directions, + const double *joints, EmcPose *pos) { - (void)fflags; - (void)iflags; + const double con = conventional_directions ? 1.0 : -1.0; - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; + const double zb = (pivot_length + joints[8]) * cos(maxkins_d2r(joints[4])); + const double xb = (pivot_length + joints[8]) * sin(maxkins_d2r(joints[4])); - // B correction - const double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4])); - const double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4])); - - // C correction - const double xyr = hypot(joints[0], joints[1]); - const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]); + const double xyr = maxkins_hypot(joints[0], joints[1]); + const double xytheta = atan2(joints[1], joints[0]) + maxkins_d2r(joints[5]); - // U correction - const double zv = joints[6] * sin(d2r(joints[4])); - const double xv = joints[6] * cos(d2r(joints[4])); - - // V correction is always in joint 1 only + const double zv = joints[6] * sin(maxkins_d2r(joints[4])); + const double xv = joints[6] * cos(maxkins_d2r(joints[4])); pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv; pos->tran.y = xyr * sin(xytheta) - joints[7]; - pos->tran.z = joints[2] - zb - (con * zv) + *(haldata->pivot_length); + pos->tran.z = joints[2] - zb - (con * zv) + pivot_length; pos->a = joints[3]; pos->b = joints[4]; @@ -73,33 +62,23 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int maxkins_inverse_impl(double pivot_length, int conventional_directions, + const EmcPose *pos, double *joints) { - (void)iflags; - (void)fflags; - - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = conventional_directions ? 1.0 : -1.0; - // B correction - const double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b)); - const double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b)); - - // C correction - const double xyr = hypot(pos->tran.x, pos->tran.y); - const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); + const double zb = (pivot_length + pos->w) * cos(maxkins_d2r(pos->b)); + const double xb = (pivot_length + pos->w) * sin(maxkins_d2r(pos->b)); - // U correction - const double zv = pos->u * sin(d2r(pos->b)); - const double xv = pos->u * cos(d2r(pos->b)); + const double xyr = maxkins_hypot(pos->tran.x, pos->tran.y); + const double xytheta = atan2(pos->tran.y, pos->tran.x) - maxkins_d2r(pos->c); - // V correction is always in joint 1 only + const double zv = pos->u * sin(maxkins_d2r(pos->b)); + const double xv = pos->u * cos(maxkins_d2r(pos->b)); joints[0] = xyr * cos(xytheta) + (con * xb) + xv; joints[1] = xyr * sin(xytheta) + pos->v; - joints[2] = pos->tran.z + zb - (con * zv) - *(haldata->pivot_length); + joints[2] = pos->tran.z + zb - (con * zv) - pivot_length; joints[3] = pos->a; joints[4] = pos->b; @@ -111,18 +90,53 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +/* ======================================================================== + * RT interface (reads HAL pins) + * ======================================================================== */ + +struct haldata { + hal_float_t *pivot_length; + hal_bit_t *conventional_directions; //default is false +} *haldata; + +int kinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + return maxkins_forward_impl(*(haldata->pivot_length), + *(haldata->conventional_directions), + joints, pos); +} + +int kinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return maxkins_inverse_impl(*(haldata->pivot_length), + *(haldata->conventional_directions), + pos, joints); +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } -#include "rtapi.h" /* RTAPI realtime OS API */ #include "rtapi_app.h" /* RTAPI realtime module decls */ +const char* kinematicsGetName(void) { return "maxkins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); int comp_id; @@ -150,3 +164,54 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + return maxkins_forward_impl(kp->params.maxkins.pivot_length, + kp->params.maxkins.conventional_directions, + joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + return maxkins_inverse_impl(kp->params.maxkins.pivot_length, + kp->params.maxkins.conventional_directions, + pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_s32; + + if (read_float("maxkins.pivot-length", + &kp->params.maxkins.pivot_length) != 0) + return -1; + + if (read_bit("maxkins.conventional-directions", + &kp->params.maxkins.conventional_directions) != 0) + return -1; + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index a1dcd9923e0..1a0d0a1178f 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -53,188 +53,209 @@ #include "kinematics.h" /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "hal.h" -struct haldata { - hal_float_t basex[NUM_STRUTS]; - hal_float_t basey[NUM_STRUTS]; - hal_float_t basez[NUM_STRUTS]; - hal_float_t effectorr[NUM_STRUTS]; - hal_float_t effectorz[NUM_STRUTS]; - hal_u32_t *last_iter; - hal_u32_t *max_iter; - hal_u32_t *iter_limit; - hal_float_t *max_error; - hal_float_t *conv_criterion; - hal_float_t *tool_offset; -} *haldata; - - -/******************************* MatInvert5() ***************************/ - -/*----------------------------------------------------------------------------- - This is a function that inverts a 5x5 matrix. ------------------------------------------------------------------------------*/ - -static int MatInvert5(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) +/* ======================================================================== + * Math types and functions (was in pentakins_math.h) + * ======================================================================== */ + +#define PENTAKINS_NUM_STRUTS 5 +#define PENTAKINS_NUM_JOINTS 5 + +typedef struct { + PmCartesian base[PENTAKINS_NUM_STRUTS]; + double effector_r[PENTAKINS_NUM_STRUTS]; + double effector_z[PENTAKINS_NUM_STRUTS]; + double conv_criterion; + unsigned int iter_limit; + double max_error; + double tool_offset; + unsigned int last_iterations; + unsigned int max_iterations; +} pentakins_params_t; + +static int pentakins_mat_invert5(double J[][PENTAKINS_NUM_STRUTS], + double InvJ[][PENTAKINS_NUM_STRUTS]) { - double JAug[NUM_STRUTS][10], m, temp; - int j, k, n; - - /* This function determines the inverse of a 6x6 matrix using - Gauss-Jordan elimination */ - - /* Augment the Identity matrix to the Jacobian matrix */ - - for (j=0; j<=4; ++j){ - for (k=0; k<=4; ++k){ /* Assign J matrix to first 6 columns of AugJ */ - JAug[j][k] = J[j][k]; - } - for(k=5; k<=9; ++k){ /* Assign I matrix to last six columns of AugJ */ - if (k-5 == j){ - JAug[j][k]=1; - } - else{ - JAug[j][k]=0; - } + double JAug[PENTAKINS_NUM_STRUTS][10], m, temp; + int j, k, n; + for (j = 0; j <= 4; ++j) { + for (k = 0; k <= 4; ++k) JAug[j][k] = J[j][k]; + for (k = 5; k <= 9; ++k) JAug[j][k] = (k - 5 == j) ? 1 : 0; } - } - - /* Perform Gauss elimination */ - for (k=0; k<=3; ++k){ /* Pivot */ - if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){ - for (j=k+1;j<=4; ++j){ - if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){ - for (n=0; n<=9;++n){ - temp = JAug[k][n]; - JAug[k][n] = JAug[j][n]; - JAug[j][n] = temp; - } - break; + for (k = 0; k <= 3; ++k) { + if ((JAug[k][k] < 0.01) && (JAug[k][k] > -0.01)) { + for (j = k + 1; j <= 4; ++j) { + if ((JAug[j][k] > 0.01) || (JAug[j][k] < -0.01)) { + for (n = 0; n <= 9; ++n) { + temp = JAug[k][n]; JAug[k][n] = JAug[j][n]; JAug[j][n] = temp; + } + break; + } + } } - } - } - for (j=k+1; j<=4; ++j){ /* Pivot */ - m = -JAug[j][k] / JAug[k][k]; - for (n=0; n<=9; ++n){ - JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */ - if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){ - JAug[j][n] = 0; + for (j = k + 1; j <= 4; ++j) { + m = -JAug[j][k] / JAug[k][k]; + for (n = 0; n <= 9; ++n) { + JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)) JAug[j][n] = 0; + } } - } } - } - - /* Normalization of Diagonal Terms */ - for (j=0; j<=4; ++j){ - m=1/JAug[j][j]; - for(k=0; k<=9; ++k){ - JAug[j][k] = m * JAug[j][k]; + for (j = 0; j <= 4; ++j) { + m = 1 / JAug[j][j]; + for (k = 0; k <= 9; ++k) JAug[j][k] = m * JAug[j][k]; } - } - - /* Perform Gauss Jordan Steps */ - for (k=4; k>=0; --k){ - for(j=k-1; j>=0; --j){ - m = -JAug[j][k]/JAug[k][k]; - for (n=0; n<=9; ++n){ - JAug[j][n] = JAug[j][n] + m * JAug[k][n]; - } + for (k = 4; k >= 0; --k) { + for (j = k - 1; j >= 0; --j) { + m = -JAug[j][k] / JAug[k][k]; + for (n = 0; n <= 9; ++n) JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + } } - } - - /* Assign last 4 columns of JAug to InvJ */ - for (j=0; j<=4; ++j){ - for (k=0; k<=4; ++k){ - InvJ[j][k] = JAug[j][k+5]; + for (j = 0; j <= 4; ++j) + for (k = 0; k <= 4; ++k) InvJ[j][k] = JAug[j][k + 5]; + return 0; +} +static void pentakins_mat_mult5(double J[][5], const double x[], double Ans[]) +{ + int j, k; + for (j = 0; j <= 4; ++j) { + Ans[j] = 0; + for (k = 0; k <= 4; ++k) Ans[j] = J[j][k] * x[k] + Ans[j]; } - } - - return 0; /* FIXME-- check divisors for 0 above */ } -/******************************** MatMult() *********************************/ - -/*--------------------------------------------------------------------------- - This function simply multiplies a 6x6 matrix by a 1x6 vector - ---------------------------------------------------------------------------*/ +static double pentakins_sqr(double x) { return x * x; } -static void MatMult5(double J[][5], const double x[], double Ans[]) +static int pentakins_inv_kins(const pentakins_params_t *params, + const double coord[PENTAKINS_NUM_JOINTS], + double struts[PENTAKINS_NUM_STRUTS]) { - int j, k; - for (j=0; j<=4; ++j){ - Ans[j] = 0; - for (k=0; k<=4; ++k){ - Ans[j] = J[j][k]*x[k]+Ans[j]; + PmCartesian xyz, pmcoord, temp; + PmRotationMatrix RMatrix, InvRMatrix; + PmRpy rpy; + int i; + + pmcoord.x = coord[0]; pmcoord.y = coord[1]; pmcoord.z = coord[2]; + rpy.r = coord[3]; rpy.p = coord[4]; rpy.y = 0; + pmRpyMatConvert(&rpy, &RMatrix); + + for (i = 0; i < PENTAKINS_NUM_STRUTS; i++) { + pmCartCartSub(¶ms->base[i], &pmcoord, &temp); + pmMatInv(&RMatrix, &InvRMatrix); + pmMatCartMult(&InvRMatrix, &temp, &xyz); + double r_actual = sqrt(pentakins_sqr(xyz.x) + pentakins_sqr(xyz.y)); + double z_diff = xyz.z - (params->effector_z[i] + params->tool_offset); + double r_diff = r_actual - params->effector_r[i]; + struts[i] = sqrt(pentakins_sqr(z_diff) + pentakins_sqr(r_diff)); } - } + return 0; } -/*-------------- -------square-----*/ +static int pentakins_fwd(pentakins_params_t *params, + const double joints[PENTAKINS_NUM_STRUTS], + EmcPose *pos) +{ + double Jacobian[PENTAKINS_NUM_STRUTS][PENTAKINS_NUM_STRUTS]; + double InverseJacobian[PENTAKINS_NUM_STRUTS][PENTAKINS_NUM_STRUTS]; + double InvKinStrutLength[PENTAKINS_NUM_STRUTS]; + double StrutLengthDiff[PENTAKINS_NUM_STRUTS]; + double delta[PENTAKINS_NUM_STRUTS], jointdelta[PENTAKINS_NUM_STRUTS]; + double coord[PENTAKINS_NUM_JOINTS]; + double conv_err = 1.0; + int iterate = 1, i, j; + unsigned iteration = 0; + + if (joints[0] <= 0.0 || joints[1] <= 0.0 || joints[2] <= 0.0 || + joints[3] <= 0.0 || joints[4] <= 0.0) return -1; + + coord[0] = pos->tran.x; coord[1] = pos->tran.y; coord[2] = pos->tran.z; + coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; + + while (iterate) { + if ((conv_err > params->max_error) || (conv_err < -params->max_error)) return -2; + iteration++; + if (iteration > params->iter_limit) return -5; + + pentakins_inv_kins(params, coord, InvKinStrutLength); + for (i = 0; i < PENTAKINS_NUM_STRUTS; i++) { + StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; + coord[i] += 1e-4; + pentakins_inv_kins(params, coord, jointdelta); + coord[i] -= 1e-4; + for (j = 0; j < PENTAKINS_NUM_STRUTS; j++) + InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; + } + pentakins_mat_invert5(InverseJacobian, Jacobian); + pentakins_mat_mult5(Jacobian, StrutLengthDiff, delta); + for (i = 0; i < PENTAKINS_NUM_JOINTS; i++) coord[i] -= delta[i]; + conv_err = 0.0; + for (i = 0; i < PENTAKINS_NUM_STRUTS; i++) conv_err += fabs(StrutLengthDiff[i]); + iterate = 0; + for (i = 0; i < PENTAKINS_NUM_STRUTS; i++) + if (fabs(StrutLengthDiff[i]) > params->conv_criterion) iterate = 1; + } -static double sqr(double x) + pos->tran.x = coord[0]; pos->tran.y = coord[1]; pos->tran.z = coord[2]; + pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; + params->last_iterations = iteration; + if (iteration > params->max_iterations) params->max_iterations = iteration; + return 0; +} + +static int pentakins_inv(pentakins_params_t *params, + const EmcPose *pos, + double joints[PENTAKINS_NUM_STRUTS]) { - return (x)*(x); + double coord[PENTAKINS_NUM_JOINTS]; + coord[0] = pos->tran.x; coord[1] = pos->tran.y; coord[2] = pos->tran.z; + coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; + return pentakins_inv_kins(params, coord, joints); } -/* declare arrays for base and effector coordinates */ -static PmCartesian b[NUM_STRUTS]; -static double za[NUM_STRUTS], ra[NUM_STRUTS]; +/* ======================================================================== + * RT interface + * ======================================================================== */ + +struct haldata { + hal_float_t basex[NUM_STRUTS]; + hal_float_t basey[NUM_STRUTS]; + hal_float_t basez[NUM_STRUTS]; + hal_float_t effectorr[NUM_STRUTS]; + hal_float_t effectorz[NUM_STRUTS]; + hal_u32_t *last_iter; + hal_u32_t *max_iter; + hal_u32_t *iter_limit; + hal_float_t *max_error; + hal_float_t *conv_criterion; + hal_float_t *tool_offset; +} *haldata; + +/* Global parameters structure for math functions */ +static pentakins_params_t params; /************************pentakins_read_hal_pins**************************/ int pentakins_read_hal_pins(void) { int t; - /* set the base and effector coordinates from hal pin values */ + /* Set the base and effector coordinates from hal pin values */ for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = haldata->basex[t]; - b[t].y = haldata->basey[t]; - b[t].z = haldata->basez[t] + *haldata->tool_offset; - ra[t] = haldata->effectorr[t]; - za[t] = haldata->effectorz[t] + *haldata->tool_offset; + params.base[t].x = haldata->basex[t]; + params.base[t].y = haldata->basey[t]; + params.base[t].z = haldata->basez[t]; + params.effector_r[t] = haldata->effectorr[t]; + params.effector_z[t] = haldata->effectorz[t]; } - return 0; -} -/************************ InvKins() ********************************/ - -int InvKins(const double * coord, - double * struts) -{ - - PmCartesian xyz, pmcoord, temp; - PmRotationMatrix RMatrix, InvRMatrix; - PmRpy rpy; - int i; - -// pentakins_read_hal_pins(); - - /* define Rotation Matrix */ - pmcoord.x = coord[0]; - pmcoord.y = coord[1]; - pmcoord.z = coord[2]; - rpy.r = coord[3]; - rpy.p = coord[4]; - rpy.y = 0; - pmRpyMatConvert(&rpy, &RMatrix); - - /* enter for loop to calculate joints (strut lengths) */ - for (i = 0; i < NUM_STRUTS; i++) { - /* convert location of effector strut end from effector - to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); - pmMatInv(&RMatrix, &InvRMatrix); - pmMatCartMult(&InvRMatrix, &temp, &xyz); - - /* define strut lengths */ - struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); - } + /* Update iteration parameters */ + params.conv_criterion = *haldata->conv_criterion; + params.iter_limit = *haldata->iter_limit; + params.max_error = *haldata->max_error; + params.tool_offset = *haldata->tool_offset; - return 0; + return 0; } - /**************************** kinematicsForward() ***************************/ int kinematicsForward(const double * joints, @@ -244,128 +265,22 @@ int kinematicsForward(const double * joints, { (void)fflags; (void)iflags; - -// PmCartesian aw; -// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; -// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - - double Jacobian[NUM_STRUTS][NUM_STRUTS]; - double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; - double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; - double delta[NUM_STRUTS]; - double jointdelta[NUM_STRUTS]; - double coord[NUM_STRUTS]; - double conv_err = 1.0; - -// PmRotationMatrix RMatrix; -// PmRpy q_RPY; - - int iterate = 1; - int i, j; - unsigned iteration = 0; + int result; pentakins_read_hal_pins(); - /* abort on obvious problems, like joints <= 0 */ - if (joints[0] <= 0.0 || - joints[1] <= 0.0 || - joints[2] <= 0.0 || - joints[3] <= 0.0 || - joints[4] <= 0.0 ) { - return -1; - } + /* Call pure math function */ + result = pentakins_fwd(¶ms, joints, pos); - /* assign a,b,c to roll, pitch, yaw angles */ - coord[0] = pos->tran.x; - coord[1] = pos->tran.y; - coord[2] = pos->tran.z; - coord[3] = pos->a * PM_PI / 180.0; - coord[4] = pos->b * PM_PI / 180.0; - - /* Enter Newton-Raphson iterative method */ - while (iterate) { - /* check for large error and return error flag if no convergence */ - if ((conv_err > +(*haldata->max_error)) || - (conv_err < -(*haldata->max_error))) { - /* we can't converge */ - return -2; - }; - - iteration++; - - /* check iteration to see if the kinematics can reach the - convergence criterion and return error flag if it can't */ - if (iteration > *haldata->iter_limit) { - /* we can't converge */ - return -5; - } - - /* compute StrutLengthDiff[] by running inverse kins on Cartesian - estimate to get joint estimate, subtract joints to get joint deltas, - and compute inv J while we're at it */ - InvKins(coord, InvKinStrutLength); - - for (i = 0; i < NUM_STRUTS; i++) { - StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; - - /* Build Inverse Jacobian Matrix */ - coord[i] += 1e-4; - InvKins(coord, jointdelta); - coord[i] -= 1e-4; - for (j = 0; j < NUM_STRUTS; j++) { - InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; - } - } - - /* invert Inverse Jacobian */ - MatInvert5(InverseJacobian, Jacobian); - - /* multiply Jacobian by LegLengthDiff */ - MatMult5(Jacobian, StrutLengthDiff, delta); - - /* subtract delta from last iterations pos values */ - coord[0] -= delta[0]; - coord[1] -= delta[1]; - coord[2] -= delta[2]; - coord[3] -= delta[3]; - coord[4] -= delta[4]; - - /* determine value of conv_error (used to determine if no convergence) */ - conv_err = 0.0; - for (i = 0; i < NUM_STRUTS; i++) { - conv_err += fabs(StrutLengthDiff[i]); - } - - /* enter loop to determine if a strut needs another iteration */ - iterate = 0; /*assume iteration is done */ - for (i = 0; i < NUM_STRUTS; i++) { - if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { - iterate = 1; - } - } - } /* exit Newton-Raphson Iterative loop */ - - /* assign coord to pos */ - pos->tran.x = coord[0]; - pos->tran.y = coord[1]; - pos->tran.z = coord[2]; - pos->a = coord[3] * 180.0 / PM_PI; - pos->b = coord[4] * 180.0 / PM_PI; - - *haldata->last_iter = iteration; - - if (iteration > *haldata->max_iter){ - *haldata->max_iter = iteration; + /* Update HAL pins with iteration statistics */ + *haldata->last_iter = params.last_iterations; + if (params.last_iterations > *haldata->max_iter) { + *haldata->max_iter = params.last_iterations; } - return 0; -} + return result; +} -/************************ kinematicsInverse() ********************************/ -/* the inverse kinematics take world coordinates and determine joint values, - given the inverse kinematics flags to resolve any ambiguities. The forward - flags are set to indicate their value appropriate to the world coordinates - passed in. */ /************************ kinematicsInverse() ********************************/ @@ -377,21 +292,10 @@ int kinematicsInverse(const EmcPose * pos, (void)iflags; (void)fflags; - double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); - coord[0] = pos->tran.x; - coord[1] = pos->tran.y; - coord[2] = pos->tran.z; - coord[3] = pos->a * PM_PI / 180.0; - coord[4] = pos->b * PM_PI / 180.0; - - if (0 != InvKins(coord,joints)) { - return -1; - } - - return 0; + /* Call pure math function */ + return pentakins_inv(¶ms, pos, joints); } KINEMATICS_TYPE kinematicsType() @@ -403,10 +307,13 @@ KINEMATICS_TYPE kinematicsType() #include "rtapi.h" /* RTAPI realtime OS API */ #include "rtapi_app.h" /* RTAPI realtime module decls */ +const char* kinematicsGetName(void) { return "pentakins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); @@ -519,3 +426,87 @@ void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +static void nonrt_build_penta(const kinematics_params_t *kp, pentakins_params_t *p) +{ + int i; + for (i = 0; i < PENTAKINS_NUM_STRUTS; i++) { + p->base[i].x = kp->params.penta.basex[i]; + p->base[i].y = kp->params.penta.basey[i]; + p->base[i].z = kp->params.penta.basez[i]; + p->effector_r[i] = kp->params.penta.effectorr[i]; + p->effector_z[i] = kp->params.penta.effectorz[i]; + } + p->conv_criterion = kp->params.penta.conv_criterion; + p->iter_limit = kp->params.penta.iter_limit; + p->max_error = 100.0; + p->tool_offset = 0.0; + p->last_iterations = 0; + p->max_iterations = 0; +} + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + pentakins_params_t p; + nonrt_build_penta(kp, &p); + return pentakins_fwd(&p, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + pentakins_params_t p; + nonrt_build_penta(kp, &p); + return pentakins_inv(&p, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + int i; + char pin_name[64]; + (void)read_bit; + + for (i = 0; i < KINS_PENTA_NUM_STRUTS; i++) { + rtapi_snprintf(pin_name, sizeof(pin_name), "pentakins.base.%d.x", i); + read_float(pin_name, &kp->params.penta.basex[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "pentakins.base.%d.y", i); + read_float(pin_name, &kp->params.penta.basey[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "pentakins.base.%d.z", i); + read_float(pin_name, &kp->params.penta.basez[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "pentakins.effector.%d.r", i); + read_float(pin_name, &kp->params.penta.effectorr[i]); + rtapi_snprintf(pin_name, sizeof(pin_name), "pentakins.effector.%d.z", i); + read_float(pin_name, &kp->params.penta.effectorz[i]); + } + + read_float("pentakins.convergence-criterion", &kp->params.penta.conv_criterion); + + if (read_s32) { + int val; + if (read_s32("pentakins.limit-iterations", &val) == 0) + kp->params.penta.iter_limit = (unsigned int)val; + } + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 1ec29dd43e0..b0a278f6eac 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -25,22 +25,34 @@ #include "kinematics.h" #include "switchkins.h" -struct haldata { - hal_float_t *a2, *a3, *d3, *d4, *d6; -} *haldata = 0; - -#define PUMA_A2 (*(haldata->a2)) -#define PUMA_A3 (*(haldata->a3)) -#define PUMA_D3 (*(haldata->d3)) -#define PUMA_D4 (*(haldata->d4)) -#define PUMA_D6 (*(haldata->d6)) - -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/* ======================================================================== + * Internal math (was in pumakins_math.h) + * ======================================================================== */ + +/* Parameters struct - matches kinematics_params.h kins_puma_params_t */ +typedef struct { + double a2; /* Link length A2 */ + double a3; /* Link length A3 */ + double d3; /* Link offset D3 */ + double d4; /* Link offset D4 */ + double d6; /* Tool offset D6 */ +} puma_params_t; + +/* + * Pure forward kinematics - joints to world coordinates + * + * params: kinematics parameters (a2, a3, d3, d4, d6) + * joints: input joint positions array (6 joints, in degrees) + * world: output world position (EmcPose) + * iflags: output inverse kinematics flags (can be NULL) + * + * Returns 0 on success + */ +static int puma_forward_math(const puma_params_t *params, + const double *joints, + EmcPose *world, + int *iflags) { - (void)fflags; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; double s23; @@ -50,22 +62,23 @@ static int pumaKinematicsForward(const double * joint, PmHomogeneous hom; PmPose worldPose; PmRpy rpy; + int flags = 0; /* Calculate sin of joints for future use */ - s1 = sin(joint[0]*PM_PI/180); - s2 = sin(joint[1]*PM_PI/180); - s3 = sin(joint[2]*PM_PI/180); - s4 = sin(joint[3]*PM_PI/180); - s5 = sin(joint[4]*PM_PI/180); - s6 = sin(joint[5]*PM_PI/180); + s1 = sin(joints[0]*PM_PI/180.0); + s2 = sin(joints[1]*PM_PI/180.0); + s3 = sin(joints[2]*PM_PI/180.0); + s4 = sin(joints[3]*PM_PI/180.0); + s5 = sin(joints[4]*PM_PI/180.0); + s6 = sin(joints[5]*PM_PI/180.0); /* Calculate cos of joints for future use */ - c1 = cos(joint[0]*PM_PI/180); - c2 = cos(joint[1]*PM_PI/180); - c3 = cos(joint[2]*PM_PI/180); - c4 = cos(joint[3]*PM_PI/180); - c5 = cos(joint[4]*PM_PI/180); - c6 = cos(joint[5]*PM_PI/180); + c1 = cos(joints[0]*PM_PI/180.0); + c2 = cos(joints[1]*PM_PI/180.0); + c3 = cos(joints[2]*PM_PI/180.0); + c4 = cos(joints[3]*PM_PI/180.0); + c5 = cos(joints[4]*PM_PI/180.0); + c6 = cos(joints[5]*PM_PI/180.0); s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; @@ -107,37 +120,34 @@ static int pumaKinematicsForward(const double * joint, /* Calculate term to be used in definition of... */ /* position vector. */ - t1 = PUMA_A2 * c2 + PUMA_A3 * c23 - PUMA_D4 * s23; + t1 = params->a2 * c2 + params->a3 * c23 - params->d4 * s23; /* Define position vector */ - hom.tran.x = c1 * t1 - PUMA_D3 * s1; - hom.tran.y = s1 * t1 + PUMA_D3 * c1; - hom.tran.z = -PUMA_A3 * s23 - PUMA_A2 * s2 - PUMA_D4 * c23; + hom.tran.x = c1 * t1 - params->d3 * s1; + hom.tran.y = s1 * t1 + params->d3 * c1; + hom.tran.z = -params->a3 * s23 - params->a2 * s2 - params->d4 * c23; /* Calculate terms to be used to... */ /* determine flags. */ sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - - PUMA_D3 * PUMA_D3; - k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 - - PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / - (2.0 * PUMA_A2); - - /* reset flags */ - *iflags = 0; + params->d3 * params->d3; + k = (sumSq + hom.tran.z * hom.tran.z - params->a2 * params->a2 - + params->a3 * params->a3 - params->d4 * params->d4) / + (2.0 * params->a2); /* Set shoulder-up flag if necessary */ - if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + - atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ) + if (fabs(joints[0]*PM_PI/180.0 - atan2(hom.tran.y, hom.tran.x) + + atan2(params->d3, -sqrt(sumSq))) < FLAG_FUZZ) { - *iflags |= PUMA_SHOULDER_RIGHT; + flags |= PUMA_SHOULDER_RIGHT; } /* Set elbow down flag if necessary */ - if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) + - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + - PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ) + if (fabs(joints[2]*PM_PI/180.0 - atan2(params->a3, params->d4) + + atan2(k, -sqrt(params->a3 * params->a3 + + params->d4 * params->d4 - k * k))) < FLAG_FUZZ) { - *iflags |= PUMA_ELBOW_DOWN; + flags |= PUMA_ELBOW_DOWN; } /* set singular flag if necessary */ @@ -146,20 +156,21 @@ static int pumaKinematicsForward(const double * joint, hom.rot.z.z * s23; if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ) { - *iflags |= PUMA_SINGULAR; + flags |= PUMA_SINGULAR; } /* if not singular set wrist flip flag if necessary */ else{ - if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + if (! (fabs(joints[3]*PM_PI/180.0 - atan2(t1, t2)) < FLAG_FUZZ)) { - *iflags |= PUMA_WRIST_FLIP; + flags |= PUMA_WRIST_FLIP; } } - /* add effect of d6 parameter */ - hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; - hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; - hom.tran.z = hom.tran.z + hom.rot.z.z*PUMA_D6; + + /* add effect of d6 parameter */ + hom.tran.x = hom.tran.x + hom.rot.z.x*params->d6; + hom.tran.y = hom.tran.y + hom.rot.z.y*params->d6; + hom.tran.z = hom.tran.z + hom.rot.z.z*params->d6; /* convert hom.rot to world->quat */ pmHomPoseConvert(&hom, &worldPose); @@ -168,16 +179,36 @@ static int pumaKinematicsForward(const double * joint, world->a = rpy.r * 180.0/PM_PI; world->b = rpy.p * 180.0/PM_PI; world->c = rpy.y * 180.0/PM_PI; + world->u = 0.0; + world->v = 0.0; + world->w = 0.0; + /* Store flags if requested */ + if (iflags != NULL) { + *iflags = flags; + } - /* return 0 and exit */ return 0; } -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +/* + * Pure inverse kinematics - world coordinates to joints + * + * params: kinematics parameters (a2, a3, d3, d4, d6) + * world: input world position (EmcPose) + * joints: output joint positions array (6 joints, in degrees) + * current_joints: current joint positions for singularity resolution (can be NULL) + * iflags: input inverse kinematics flags (shoulder/elbow/wrist configuration) + * fflags: output forward kinematics flags (can be NULL) + * + * Returns 0 on success, -1 on failure + */ +static int puma_inverse_math(const puma_params_t *params, + const EmcPose *world, + double *joints, + const double *current_joints, + int iflags, + int *fflags) { PmHomogeneous hom; PmPose worldPose; @@ -202,9 +233,7 @@ static int pumaKinematicsInverse(const EmcPose * world, double s5, c5; double s6, c6; double px, py, pz; - - /* reset flags */ - *fflags = 0; + int flags = 0; /* convert pose to hom */ worldPose.tran = world->tran; @@ -214,23 +243,21 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - /* remove effect of d6 parameter */ - px = hom.tran.x - PUMA_D6*hom.rot.z.x; - py = hom.tran.y - PUMA_D6*hom.rot.z.y; - pz = hom.tran.z - PUMA_D6*hom.rot.z.z; + /* remove effect of d6 parameter */ + px = hom.tran.x - params->d6*hom.rot.z.x; + py = hom.tran.y - params->d6*hom.rot.z.y; + pz = hom.tran.z - params->d6*hom.rot.z.z; /* Joint 1 (2 independent solutions) */ /* save sum of squares for this and subsequent calcs */ - sumSq = px * px + py * py - - PUMA_D3 * PUMA_D3; + sumSq = px * px + py * py - params->d3 * params->d3; - /* FIXME-- is use of + sqrt shoulder right or left? */ - if (*iflags & PUMA_SHOULDER_RIGHT){ - th1 = atan2(py, px) - atan2(PUMA_D3, -sqrt(sumSq)); + if (iflags & PUMA_SHOULDER_RIGHT){ + th1 = atan2(py, px) - atan2(params->d3, -sqrt(sumSq)); } else{ - th1 = atan2(py, px) - atan2(PUMA_D3, sqrt(sumSq)); + th1 = atan2(py, px) - atan2(params->d3, sqrt(sumSq)); } /* save sin, cos for later calcs */ @@ -239,16 +266,15 @@ static int pumaKinematicsInverse(const EmcPose * world, /* Joint 3 (2 independent solutions) */ - k = (sumSq + pz * pz - PUMA_A2 * PUMA_A2 - - PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / (2.0 * PUMA_A2); + k = (sumSq + pz * pz - params->a2 * params->a2 - + params->a3 * params->a3 - params->d4 * params->d4) / (2.0 * params->a2); - /* FIXME-- is use of + sqrt elbow up or down? */ - if (*iflags & PUMA_ELBOW_DOWN){ - th3 = atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k)); + if (iflags & PUMA_ELBOW_DOWN){ + th3 = atan2(params->a3, params->d4) - atan2(k, -sqrt(params->a3 * params->a3 + params->d4 * params->d4 - k * k)); } else{ - th3 = atan2(PUMA_A3, PUMA_D4) - - atan2(k, sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k)); + th3 = atan2(params->a3, params->d4) - + atan2(k, sqrt(params->a3 * params->a3 + params->d4 * params->d4 - k * k)); } /* compute sin, cos for later calcs */ @@ -257,10 +283,10 @@ static int pumaKinematicsInverse(const EmcPose * world, /* Joint 2 */ - t1 = (-PUMA_A3 - PUMA_A2 * c3) * pz + - (c1 * px + s1 * py) * (PUMA_A2 * s3 - PUMA_D4); - t2 = (PUMA_A2 * s3 - PUMA_D4) * pz + - (PUMA_A3 + PUMA_A2 * c3) * (c1 * px + s1 * py); + t1 = (-params->a3 - params->a2 * c3) * pz + + (c1 * px + s1 * py) * (params->a2 * s3 - params->d4); + t2 = (params->a2 * s3 - params->d4) * pz + + (params->a3 + params->a2 * c3) * (c1 * px + s1 * py); t3 = pz * pz + (c1 * px + s1 * py) * (c1 * px + s1 * py); @@ -276,8 +302,9 @@ static int pumaKinematicsInverse(const EmcPose * world, t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23; if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ){ - *fflags |= PUMA_REACH; - th4 = joint[3]*PM_PI/180; /* use current value */ + flags |= PUMA_REACH; + /* use current value if available */ + th4 = (current_joints != NULL) ? current_joints[3]*PM_PI/180.0 : 0.0; } else{ th4 = atan2(t1, t2); @@ -307,21 +334,82 @@ static int pumaKinematicsInverse(const EmcPose * world, hom.rot.x.z * (s23 * c4 * c5 + c23 * s5); th6 = atan2(s6, c6); - /* FIXME-- is wrist flip the normal or offset results? */ - if (*iflags & PUMA_WRIST_FLIP){ + if (iflags & PUMA_WRIST_FLIP){ th4 = th4 + PM_PI; th5 = -th5; th6 = th6 + PM_PI; } /* copy out */ - joint[0] = th1*180/PM_PI; - joint[1] = th2*180/PM_PI; - joint[2] = th3*180/PM_PI; - joint[3] = th4*180/PM_PI; - joint[4] = th5*180/PM_PI; - joint[5] = th6*180/PM_PI; + joints[0] = th1*180.0/PM_PI; + joints[1] = th2*180.0/PM_PI; + joints[2] = th3*180.0/PM_PI; + joints[3] = th4*180.0/PM_PI; + joints[4] = th5*180.0/PM_PI; + joints[5] = th6*180.0/PM_PI; + + /* Store flags if requested */ + if (fflags != NULL) { + *fflags = flags; + } + + return 0; +} + +/* ======================================================================== + * RT interface (reads HAL pins) + * ======================================================================== */ + +struct haldata { + hal_float_t *a2, *a3, *d3, *d4, *d6; +} *haldata = 0; + +#define PUMA_A2 (*(haldata->a2)) +#define PUMA_A3 (*(haldata->a3)) +#define PUMA_D3 (*(haldata->d3)) +#define PUMA_D4 (*(haldata->d4)) +#define PUMA_D6 (*(haldata->d6)) + +static int pumaKinematicsForward(const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + puma_params_t params; + int flags_out = 0; + + params.a2 = PUMA_A2; + params.a3 = PUMA_A3; + params.d3 = PUMA_D3; + params.d4 = PUMA_D4; + params.d6 = PUMA_D6; + + puma_forward_math(¶ms, joint, world, &flags_out); + + *iflags = flags_out; + return 0; +} + +static int pumaKinematicsInverse(const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + puma_params_t params; + int flags_out = 0; + params.a2 = PUMA_A2; + params.a3 = PUMA_A3; + params.d3 = PUMA_D3; + params.d4 = PUMA_D4; + params.d6 = PUMA_D6; + + *fflags = 0; + + puma_inverse_math(¶ms, world, joint, joint, *iflags, &flags_out); + + *fflags = flags_out; return 0; } @@ -382,3 +470,61 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + puma_params_t p; + p.a2 = kp->params.puma.a2; + p.a3 = kp->params.puma.a3; + p.d3 = kp->params.puma.d3; + p.d4 = kp->params.puma.d4; + p.d6 = kp->params.puma.d6; + return puma_forward_math(&p, joints, pos, NULL); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + puma_params_t p; + p.a2 = kp->params.puma.a2; + p.a3 = kp->params.puma.a3; + p.d3 = kp->params.puma.d3; + p.d4 = kp->params.puma.d4; + p.d6 = kp->params.puma.d6; + return puma_inverse_math(&p, pos, joints, NULL, 0, NULL); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_bit; + (void)read_s32; + + read_float("pumakins.A2", &kp->params.puma.a2); + read_float("pumakins.A3", &kp->params.puma.a3); + read_float("pumakins.D3", &kp->params.puma.d3); + read_float("pumakins.D4", &kp->params.puma.d4); + read_float("pumakins.D6", &kp->params.puma.d6); + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index 63ed5bf7850..3997625c5aa 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -23,34 +23,49 @@ #include "rtapi_math.h" #include "rtapi_app.h" +const char* kinematicsGetName(void) { return "rosekins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); -#ifndef hypot -#define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) +/* ---- Math types and functions (from rosekins_math.h) ---- */ + +#ifndef PM_PI +#define PM_PI 3.14159265358979323846 +#endif +#ifndef PM_2_PI +#define PM_2_PI 6.28318530717958647692 +#endif +#ifndef TO_RAD +#define TO_RAD (PM_PI / 180.0) +#endif +#ifndef TO_DEG +#define TO_DEG (180.0 / PM_PI) +#endif +#ifndef rosekins_hypot +#define rosekins_hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -struct haldata { - hal_float_t *revolutions; - hal_float_t *theta_degrees; - hal_float_t *bigtheta_degrees; -} *haldata; +typedef struct { + int oldquad; + int revolutions; +} rosekins_state_t; -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - (void)fflags; - (void)iflags; - double radius,z,theta; +typedef struct { + double revolutions; + double theta_degrees; + double bigtheta_degrees; +} rosekins_output_t; - radius = joints[0]; - z = joints[1]; - theta = TO_RAD * joints[2]; +static int rosekins_forward_math(const double *joints, EmcPose *pos) +{ + double radius = joints[0]; + double z = joints[1]; + double theta = TO_RAD * joints[2]; pos->tran.x = radius * cos(theta); pos->tran.y = radius * sin(theta); @@ -65,41 +80,34 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rosekins_inverse_math(const EmcPose *pos, double *joints, + rosekins_state_t *state, + rosekins_output_t *output) { - (void)iflags; - (void)fflags; -// There is a potential problem when accumulating bigtheta -- loss of -// precision based on size of mantissa -- but in practice, it is probably ok - - static int oldquad; - static int revolutions; - - double theta,bigtheta; - int nowquad = 0; - double x = pos->tran.x; - double y = pos->tran.y; - double z = pos->tran.z; + double theta, bigtheta; + int nowquad = 0; + double x = pos->tran.x; + double y = pos->tran.y; + double z = pos->tran.z; if (x >= 0 && y >= 0) nowquad = 1; else if (x < 0 && y >= 0) nowquad = 2; else if (x < 0 && y < 0) nowquad = 3; else if (x >= 0 && y < 0) nowquad = 4; - if (oldquad == 2 && nowquad == 3) {revolutions += 1;} - if (oldquad == 3 && nowquad == 2) {revolutions -= 1;} + if (state->oldquad == 2 && nowquad == 3) { state->revolutions += 1; } + if (state->oldquad == 3 && nowquad == 2) { state->revolutions -= 1; } - theta = atan2(y,x); - bigtheta = theta + PM_2_PI * revolutions; + theta = atan2(y, x); + bigtheta = theta + PM_2_PI * state->revolutions; - *(haldata->revolutions) = revolutions; - *(haldata->theta_degrees) = theta * TO_DEG; - *(haldata->bigtheta_degrees) = bigtheta * TO_DEG; + if (output) { + output->revolutions = state->revolutions; + output->theta_degrees = theta * TO_DEG; + output->bigtheta_degrees = bigtheta * TO_DEG; + } - joints[0] = hypot(x,y); + joints[0] = rosekins_hypot(x, y); joints[1] = z; joints[2] = TO_DEG * bigtheta; joints[3] = 0; @@ -109,10 +117,49 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + state->oldquad = nowquad; return 0; } +/* ---- End math functions ---- */ + +struct haldata { + hal_float_t *revolutions; + hal_float_t *theta_degrees; + hal_float_t *bigtheta_degrees; +} *haldata; + +/* State for revolution tracking */ +static rosekins_state_t kins_state; + +int kinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + return rosekins_forward_math(joints, pos); +} + +int kinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + rosekins_output_t output; + int result = rosekins_inverse_math(pos, joints, &kins_state, &output); + + /* Write diagnostic values to HAL pins */ + *(haldata->revolutions) = output.revolutions; + *(haldata->theta_degrees) = output.theta_degrees; + *(haldata->bigtheta_degrees) = output.bigtheta_degrees; + + return result; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -142,3 +189,45 @@ int rtapi_app_main(void) { error: return ans; } + +/* ---- nonrt interface for userspace planner ---- */ + +#include "kinematics_params.h" + +static rosekins_state_t nonrt_state = {0, 0}; + +int nonrt_kinematicsForward(const void *params, + const double *joints, EmcPose *pos) +{ + (void)params; + return rosekins_forward_math(joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, double *joints) +{ + (void)params; + return rosekins_inverse_math(pos, joints, &nonrt_state, NULL); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + (void)params; + (void)read_float; + (void)read_bit; + (void)read_s32; + return 0; +} + +int nonrt_is_identity(void) +{ + return 0; +} + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h deleted file mode 100644 index cc913760462..00000000000 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ /dev/null @@ -1,194 +0,0 @@ - -// Copyright 2013 Chris Radek -// -// This program is free software; you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation; either version 2 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - - -/* - Based on work by "mzavatsky" at: - http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ - - "You can freely use this code in your applications." - - which was based on: - - Descriptive Geometric Kinematic Analysis of Clavel's "Delta" Robot - P.J. Zsombor-Murray, McGill University - - "... the purpose of this article: to provide a clear kinematic - analysis useful to those who may wish to program and employ nice - little three legged robots ..." - - - The platform is on "top", the origin is in the center of the plane - containing the three hip joints. Z points upward, so Z coordinates - are always negative. Thighs always point outward, straight out - (knee at Z=0) is considered zero degrees for the angular hip joint. - Positive rotation is knee-downward, so if you rotate all knees - positive, the Z coordinate will get more negative. - - Joint zero is the one whose thigh swings in the YZ plane. -*/ - -#ifndef LINUXCNCROTARYDELTAKINS_COMMON_H -#define LINUXCNCROTARYDELTAKINS_COMMON_H - -#include "emcpos.h" -// distance from origin to a hip joint -static double platformradius; - -// thigh connects the hip to the knee -static double thighlength; - -// shin (the parallelogram) connects the knee to the foot -static double shinlength; - -// distance from center of foot (controlled point) to an ankle joint -static double footradius; - -#ifndef sq -#define sq(a) ((a)*(a)) -#endif -#ifndef D2R -#define D2R(d) ((d)*M_PI/180.) -#endif - -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; -} - -// Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { - double - j0 = joints[0], - j1 = joints[1], - j2 = joints[2], - y1, z1, // x1 is 0 - x2, y2, z2, x3, y3, z3, - a1, b1, a2, b2, - w1, w2, w3, - denom, - a, b, c, d; - - j0 = D2R(j0); - j1 = D2R(j1); - j2 = D2R(j2); - - y1 = -(platformradius - footradius + thighlength * cos(j0)); - z1 = -thighlength * sin(j0); - - y2 = (platformradius - footradius + thighlength * cos(j1)) * 0.5; - x2 = y2 * sqrt(3); - z2 = -thighlength * sin(j1); - - y3 = (platformradius - footradius + thighlength * cos(j2)) * 0.5; - x3 = -y3 * sqrt(3); - z3 = -thighlength * sin(j2); - - denom = x3 * (y2 - y1) - x2 * (y3 - y1); - - w1 = sq(y1) + sq(z1); - w2 = sq(x2) + sq(y2) + sq(z2); - w3 = sq(x3) + sq(y3) + sq(z3); - - a1 = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1); - b1 = -((w2-w1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0; - - a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; - b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; - - // a*z^2 + b*z + c = 0 - a = sq(a1) + sq(a2) + sq(denom); - b = 2 * (a1 * b1 + a2 * (b2 - y1 * denom) - z1 * sq(denom)); - c = (b2 - y1 * denom) * (b2 - y1 * denom) + - sq(b1) + sq(denom) * (sq(z1) - sq(shinlength)); - - d = sq(b) - 4 * a * c; - if (d < 0) return -1; - - pos->tran.z = (-b - sqrt(d)) / (2 * a); - pos->tran.x = (a1 * pos->tran.z + b1) / denom; - pos->tran.y = (a2 * pos->tran.z + b2) / denom; - - pos->a = joints[3]; - pos->b = joints[4]; - pos->c = joints[5]; - pos->u = joints[6]; - pos->v = joints[7]; - pos->w = joints[8]; - - return 0; -} - - -// Given controlled point, find joint zero's angle -// (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { - double a, b, d, knee_y, knee_z; - - a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - - sq(shinlength) - sq(platformradius)) / z; - b = (footradius - platformradius - y) / z; - - d = sq(thighlength) * (sq(b) + 1) - sq(a - b * platformradius); - if (d < 0) return -1; - - knee_y = (platformradius + a*b + sqrt(d)) / (sq(b) + 1); - knee_z = b * knee_y - a; - - *theta = atan2(knee_z, knee_y - platformradius); - *theta *= 180.0/M_PI; - return 0; -} - -static void rotate(double *x, double *y, double theta) { - double xx, yy; - xx = *x, yy = *y; - *x = xx * cos(theta) - yy * sin(theta); - *y = xx * sin(theta) + yy * cos(theta); -} - -static int kinematics_inverse(const EmcPose *pos, double *joints) { - double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; - - // now use symmetry property to get the other two just as easily... - xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; - - xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; - - joints[3] = pos->a; - joints[4] = pos->b; - joints[5] = pos->c; - joints[6] = pos->u; - joints[7] = pos->v; - joints[8] = pos->w; - - return 0; -} - -#define RDELTA_PFR 10.0 -#define RDELTA_TL 10.0 -#define RDELTA_SL 14.0 -#define RDELTA_FR 6.0 - -#endif diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 3ab93723114..e7f703c082a 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -20,7 +20,164 @@ #include "rtapi_math.h" #include "rtapi_app.h" -#include "rotarydeltakins-common.h" +/* ---- Math types and functions (from rotarydeltakins_math.h) ---- */ + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef rotdelta_sq +#define rotdelta_sq(a) ((a)*(a)) +#endif + +#ifndef rotdelta_D2R +#define rotdelta_D2R(d) ((d)*M_PI/180.0) +#endif + +typedef struct { + double platformradius; + double thighlength; + double shinlength; + double footradius; +} rotarydelta_params_t; + +static void rotarydelta_rotate(double *x, double *y, double theta) +{ + double xx = *x; + double yy = *y; + *x = xx * cos(theta) - yy * sin(theta); + *y = xx * sin(theta) + yy * cos(theta); +} + +static int rotarydelta_inverse_j0(double x, double y, double z, + const rotarydelta_params_t *params, + double *theta) +{ + double a, b, d, knee_y, knee_z; + double pfr = params->platformradius; + double tl = params->thighlength; + double sl = params->shinlength; + double fr = params->footradius; + + a = 0.5 * (rotdelta_sq(x) + rotdelta_sq(y - fr) + rotdelta_sq(z) + + rotdelta_sq(tl) - rotdelta_sq(sl) - rotdelta_sq(pfr)) / z; + b = (fr - pfr - y) / z; + + d = rotdelta_sq(tl) * (rotdelta_sq(b) + 1) - rotdelta_sq(a - b * pfr); + if (d < 0) return -1; + + knee_y = (pfr + a*b + sqrt(d)) / (rotdelta_sq(b) + 1); + knee_z = b * knee_y - a; + + *theta = atan2(knee_z, knee_y - pfr); + *theta *= 180.0/M_PI; + return 0; +} + +static int rotarydelta_inverse_math(const rotarydelta_params_t *params, + const EmcPose *world, double *joints) +{ + double xr, yr; + + if (rotarydelta_inverse_j0(world->tran.x, world->tran.y, world->tran.z, + params, &joints[0])) + return -1; + + xr = world->tran.x; + yr = world->tran.y; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if (rotarydelta_inverse_j0(xr, yr, world->tran.z, params, &joints[1])) + return -1; + + xr = world->tran.x; + yr = world->tran.y; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if (rotarydelta_inverse_j0(xr, yr, world->tran.z, params, &joints[2])) + return -1; + + joints[3] = world->a; + joints[4] = world->b; + joints[5] = world->c; + joints[6] = world->u; + joints[7] = world->v; + joints[8] = world->w; + + return 0; +} + +static int rotarydelta_forward_math(const rotarydelta_params_t *params, + const double *joints, EmcPose *world) +{ + double pfr = params->platformradius; + double tl = params->thighlength; + double sl = params->shinlength; + double fr = params->footradius; + + double j0, j1, j2; + double y1, z1; + double x2, y2, z2; + double x3, y3, z3; + double a1, b1, a2, b2; + double w1, w2, w3; + double denom; + double a, b, c, d; + + j0 = rotdelta_D2R(joints[0]); + j1 = rotdelta_D2R(joints[1]); + j2 = rotdelta_D2R(joints[2]); + + y1 = -(pfr - fr + tl * cos(j0)); + z1 = -tl * sin(j0); + + y2 = (pfr - fr + tl * cos(j1)) * 0.5; + x2 = y2 * sqrt(3); + z2 = -tl * sin(j1); + + y3 = (pfr - fr + tl * cos(j2)) * 0.5; + x3 = -y3 * sqrt(3); + z3 = -tl * sin(j2); + + denom = x3 * (y2 - y1) - x2 * (y3 - y1); + + w1 = rotdelta_sq(y1) + rotdelta_sq(z1); + w2 = rotdelta_sq(x2) + rotdelta_sq(y2) + rotdelta_sq(z2); + w3 = rotdelta_sq(x3) + rotdelta_sq(y3) + rotdelta_sq(z3); + + a1 = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1); + b1 = -((w2-w1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0; + + a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; + b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; + + /* a*z^2 + b*z + c = 0 */ + a = rotdelta_sq(a1) + rotdelta_sq(a2) + rotdelta_sq(denom); + b = 2 * (a1 * b1 + a2 * (b2 - y1 * denom) - z1 * rotdelta_sq(denom)); + c = (b2 - y1 * denom) * (b2 - y1 * denom) + + rotdelta_sq(b1) + rotdelta_sq(denom) * (rotdelta_sq(z1) - rotdelta_sq(sl)); + + d = rotdelta_sq(b) - 4 * a * c; + if (d < 0) return -1; + + world->tran.z = (-b - sqrt(d)) / (2 * a); + world->tran.x = (a1 * world->tran.z + b1) / denom; + world->tran.y = (a2 * world->tran.z + b2) / denom; + + world->a = joints[3]; + world->b = joints[4]; + world->c = joints[5]; + world->u = joints[6]; + world->v = joints[7]; + world->w = joints[8]; + + return 0; +} + +#define ROTARYDELTA_DEFAULT_PLATFORMRADIUS 10.0 +#define ROTARYDELTA_DEFAULT_THIGHLENGTH 10.0 +#define ROTARYDELTA_DEFAULT_SHINLENGTH 14.0 +#define ROTARYDELTA_DEFAULT_FOOTRADIUS 6.0 + +/* ---- End math functions ---- */ struct haldata { @@ -38,8 +195,12 @@ int kinematicsForward(const double * joints, KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr); - return kinematics_forward(joints, pos); + rotarydelta_params_t params; + params.platformradius = *haldata->pfr; + params.thighlength = *haldata->tl; + params.shinlength = *haldata->sl; + params.footradius = *haldata->fr; + return rotarydelta_forward_math(¶ms, joints, pos); } int kinematicsInverse(const EmcPose *pos, double *joints, @@ -47,8 +208,12 @@ int kinematicsInverse(const EmcPose *pos, double *joints, KINEMATICS_FORWARD_FLAGS *fflags) { (void)iflags; (void)fflags; - set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr); - return kinematics_inverse(pos, joints); + rotarydelta_params_t params; + params.platformradius = *haldata->pfr; + params.thighlength = *haldata->tl; + params.shinlength = *haldata->sl; + params.footradius = *haldata->fr; + return rotarydelta_inverse_math(¶ms, pos, joints); } KINEMATICS_TYPE kinematicsType() @@ -84,10 +249,10 @@ int rtapi_app_main(void) if(retval == 0) { - *haldata->pfr = RDELTA_PFR; - *haldata->tl = RDELTA_TL; - *haldata->sl = RDELTA_SL; - *haldata->fr = RDELTA_FR; + *haldata->pfr = ROTARYDELTA_DEFAULT_PLATFORMRADIUS; + *haldata->tl = ROTARYDELTA_DEFAULT_THIGHLENGTH; + *haldata->sl = ROTARYDELTA_DEFAULT_SHINLENGTH; + *haldata->fr = ROTARYDELTA_DEFAULT_FOOTRADIUS; } if(retval == 0) @@ -103,8 +268,69 @@ void rtapi_app_exit(void) hal_exit(comp_id); } +const char* kinematicsGetName(void) { return "rotarydeltakins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); + +/* ---- nonrt interface for userspace planner ---- */ + +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + rotarydelta_params_t rp; + rp.platformradius = kp->params.rotarydelta.platformradius; + rp.thighlength = kp->params.rotarydelta.thighlength; + rp.shinlength = kp->params.rotarydelta.shinlength; + rp.footradius = kp->params.rotarydelta.footradius; + return rotarydelta_forward_math(&rp, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + rotarydelta_params_t rp; + rp.platformradius = kp->params.rotarydelta.platformradius; + rp.thighlength = kp->params.rotarydelta.thighlength; + rp.shinlength = kp->params.rotarydelta.shinlength; + rp.footradius = kp->params.rotarydelta.footradius; + return rotarydelta_inverse_math(&rp, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + double fval; + (void)read_bit; + (void)read_s32; + if (read_float("rotarydeltakins.platformradius", &fval) == 0) + kp->params.rotarydelta.platformradius = fval; + if (read_float("rotarydeltakins.thighlength", &fval) == 0) + kp->params.rotarydelta.thighlength = fval; + if (read_float("rotarydeltakins.shinlength", &fval) == 0) + kp->params.rotarydelta.shinlength = fval; + if (read_float("rotarydeltakins.footradius", &fval) == 0) + kp->params.rotarydelta.footradius = fval; + return 0; +} + +int nonrt_is_identity(void) +{ + return 0; +} + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 00e605cf6c7..43608ca5ad6 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -16,15 +16,128 @@ #define BOOST_PYTHON_MAX_ARITY 4 #include -#include "rotarydeltakins-common.h" #include using namespace boost::python; +#include "emcpos.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define rotdelta_sq(a) ((a)*(a)) +#define rotdelta_D2R(d) ((d)*M_PI/180.0) + +typedef struct { + double platformradius; + double thighlength; + double shinlength; + double footradius; +} rotarydelta_params_t; + +static void rotarydelta_rotate(double *x, double *y, double theta) +{ + double xx = *x, yy = *y; + *x = xx * cos(theta) - yy * sin(theta); + *y = xx * sin(theta) + yy * cos(theta); +} + +static int rotarydelta_inverse_j0(double x, double y, double z, + const rotarydelta_params_t *params, + double *theta) +{ + double a, b, d, knee_y, knee_z; + double pfr = params->platformradius, tl = params->thighlength; + double sl = params->shinlength, fr = params->footradius; + a = 0.5 * (rotdelta_sq(x) + rotdelta_sq(y - fr) + rotdelta_sq(z) + + rotdelta_sq(tl) - rotdelta_sq(sl) - rotdelta_sq(pfr)) / z; + b = (fr - pfr - y) / z; + d = rotdelta_sq(tl) * (rotdelta_sq(b) + 1) - rotdelta_sq(a - b * pfr); + if (d < 0) return -1; + knee_y = (pfr + a*b + sqrt(d)) / (rotdelta_sq(b) + 1); + knee_z = b * knee_y - a; + *theta = atan2(knee_z, knee_y - pfr) * 180.0/M_PI; + return 0; +} + +static int rotarydelta_inverse_math(const rotarydelta_params_t *params, + const EmcPose *world, double *joints) +{ + double xr, yr; + if (rotarydelta_inverse_j0(world->tran.x, world->tran.y, world->tran.z, + params, &joints[0])) + return -1; + xr = world->tran.x; yr = world->tran.y; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if (rotarydelta_inverse_j0(xr, yr, world->tran.z, params, &joints[1])) + return -1; + xr = world->tran.x; yr = world->tran.y; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if (rotarydelta_inverse_j0(xr, yr, world->tran.z, params, &joints[2])) + return -1; + joints[3] = world->a; joints[4] = world->b; joints[5] = world->c; + joints[6] = world->u; joints[7] = world->v; joints[8] = world->w; + return 0; +} + +static int rotarydelta_forward_math(const rotarydelta_params_t *params, + const double *joints, EmcPose *world) +{ + double pfr = params->platformradius, tl = params->thighlength; + double sl = params->shinlength, fr = params->footradius; + double j0, j1, j2, y1, z1, x2, y2, z2, x3, y3, z3; + double a1, b1, a2, b2, w1, w2, w3, denom, a, b, c, d; + j0 = rotdelta_D2R(joints[0]); j1 = rotdelta_D2R(joints[1]); j2 = rotdelta_D2R(joints[2]); + y1 = -(pfr - fr + tl * cos(j0)); z1 = -tl * sin(j0); + y2 = (pfr - fr + tl * cos(j1)) * 0.5; x2 = y2 * sqrt(3); z2 = -tl * sin(j1); + y3 = (pfr - fr + tl * cos(j2)) * 0.5; x3 = -y3 * sqrt(3); z3 = -tl * sin(j2); + denom = x3 * (y2 - y1) - x2 * (y3 - y1); + w1 = rotdelta_sq(y1) + rotdelta_sq(z1); + w2 = rotdelta_sq(x2) + rotdelta_sq(y2) + rotdelta_sq(z2); + w3 = rotdelta_sq(x3) + rotdelta_sq(y3) + rotdelta_sq(z3); + a1 = (z2-z1)*(y3-y1) - (z3-z1)*(y2-y1); + b1 = -((w2-w1)*(y3-y1) - (w3-w1)*(y2-y1)) / 2.0; + a2 = -(z2 - z1)*x3 + (z3 - z1)*x2; + b2 = ((w2 - w1)*x3 - (w3 - w1)*x2) / 2.0; + a = rotdelta_sq(a1) + rotdelta_sq(a2) + rotdelta_sq(denom); + b = 2 * (a1*b1 + a2*(b2 - y1*denom) - z1*rotdelta_sq(denom)); + c = (b2 - y1*denom)*(b2 - y1*denom) + + rotdelta_sq(b1) + rotdelta_sq(denom)*(rotdelta_sq(z1) - rotdelta_sq(sl)); + d = rotdelta_sq(b) - 4*a*c; + if (d < 0) return -1; + world->tran.z = (-b - sqrt(d)) / (2*a); + world->tran.x = (a1*world->tran.z + b1) / denom; + world->tran.y = (a2*world->tran.z + b2) / denom; + world->a = joints[3]; world->b = joints[4]; world->c = joints[5]; + world->u = joints[6]; world->v = joints[7]; world->w = joints[8]; + return 0; +} + +#define ROTARYDELTA_DEFAULT_PLATFORMRADIUS 10.0 +#define ROTARYDELTA_DEFAULT_THIGHLENGTH 10.0 +#define ROTARYDELTA_DEFAULT_SHINLENGTH 14.0 +#define ROTARYDELTA_DEFAULT_FOOTRADIUS 6.0 + +/* Global parameters for Python bindings */ +static rotarydelta_params_t params = { + ROTARYDELTA_DEFAULT_PLATFORMRADIUS, + ROTARYDELTA_DEFAULT_THIGHLENGTH, + ROTARYDELTA_DEFAULT_SHINLENGTH, + ROTARYDELTA_DEFAULT_FOOTRADIUS +}; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + params.platformradius = pfr; + params.thighlength = tl; + params.shinlength = sl; + params.footradius = fr; +} static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward_math(¶ms, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +147,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse_math(¶ms, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,12 +155,12 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(params.platformradius, params.thighlength, + params.shinlength, params.footradius); } BOOST_PYTHON_MODULE(rotarydeltakins) { - set_geometry(RDELTA_PFR, RDELTA_TL, RDELTA_SL, RDELTA_FR); def("set_geometry", set_geometry); def("get_geometry", get_geometry); def("forward", forward); diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 2bee81eccc9..c4c3f4c6b98 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -12,9 +12,60 @@ * ********************************************************************/ -#include "rtapi_math.h" #include "kinematics.h" /* these decls */ +#ifdef RTAPI +#include "rtapi_math.h" +#else +#include +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +/* + * Rotatekins - simple rotary table kinematics + * + * The C axis (joint 5) rotates the XY plane. + * + * Forward: Rotate (J0, J1) by -C to get (X, Y) + * Inverse: Rotate (X, Y) by +C to get (J0, J1) + */ +static int rotate_forward_math(const double *joints, EmcPose *world) +{ + double c_rad = -joints[5] * M_PI / 180.0; + + world->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad); + world->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad); + world->tran.z = joints[2]; + world->a = joints[3]; + world->b = joints[4]; + world->c = joints[5]; + world->u = joints[6]; + world->v = joints[7]; + world->w = joints[8]; + + return 0; +} + +static int rotate_inverse_math(const EmcPose *world, double *joints) +{ + double c_rad = world->c * M_PI / 180.0; + + joints[0] = world->tran.x * cos(c_rad) - world->tran.y * sin(c_rad); + joints[1] = world->tran.x * sin(c_rad) + world->tran.y * cos(c_rad); + joints[2] = world->tran.z; + joints[3] = world->a; + joints[4] = world->b; + joints[5] = world->c; + joints[6] = world->u; + joints[7] = world->v; + joints[8] = world->w; + + return 0; +} + int kinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -22,18 +73,7 @@ int kinematicsForward(const double *joints, { (void)fflags; (void)iflags; - double c_rad = -joints[5]*M_PI/180; - pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad); - pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad); - pos->tran.z = joints[2]; - pos->a = joints[3]; - pos->b = joints[4]; - pos->c = joints[5]; - pos->u = joints[6]; - pos->v = joints[7]; - pos->w = joints[8]; - - return 0; + return rotate_forward_math(joints, pos); } int kinematicsInverse(const EmcPose * pos, @@ -43,18 +83,7 @@ int kinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - double c_rad = pos->c*M_PI/180; - joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad); - joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad); - joints[2] = pos->tran.z; - joints[3] = pos->a; - joints[4] = pos->b; - joints[5] = pos->c; - joints[6] = pos->u; - joints[7] = pos->v; - joints[8] = pos->w; - - return 0; + return rotate_inverse_math(pos, joints); } /* implemented for these kinematics as giving joints preference */ @@ -78,10 +107,13 @@ KINEMATICS_TYPE kinematicsType() #include "rtapi_app.h" /* RTAPI realtime module decls */ #include "hal.h" +const char* kinematicsGetName(void) { return "rotatekins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); int comp_id; @@ -95,3 +127,40 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + (void)params; + return rotate_forward_math(joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + (void)params; + return rotate_inverse_math(pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + (void)params; (void)read_float; (void)read_bit; (void)read_s32; + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index c2e2a1e6dae..af7194971dc 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -23,6 +23,164 @@ #include "kinematics.h" #include "switchkins.h" +/* ======================================================================== + * Internal math (was in scarakins_math.h) + * ======================================================================== */ + +#ifndef PM_PI +#define PM_PI 3.14159265358979323846 +#endif + +/* Parameters struct - matches kinematics_params.h kins_scara_params_t */ +typedef struct { + double d1; /* Vertical distance from ground to inner arm center */ + double d2; /* Length of inner arm */ + double d3; /* Vertical offset between inner and outer arm */ + double d4; /* Length of outer arm */ + double d5; /* Vertical distance from end effector to tooltip */ + double d6; /* Horizontal offset from end effector axis to tooltip */ +} scara_params_t; + +/* Flag for elbow configuration (joint[1] < 90 degrees) */ +#define SCARA_ELBOW_UP 0x01 + +/* + * Pure forward kinematics - joints to world coordinates + * + * params: kinematics parameters (d1-d6) + * joints: input joint positions array (6 joints: j0,j1 in degrees, j2 in length, j3-j5 in degrees) + * world: output world position (EmcPose) + * iflags: output inverse kinematics flags (can be NULL) + * + * Returns 0 on success + */ +static int scara_forward_math(const scara_params_t *params, + const double *joints, + EmcPose *world, + int *iflags) +{ + double a0, a1, a3; + double x, y, z, c; + int flags = 0; + + /* convert joint angles to radians for sin() and cos() */ + a0 = joints[0] * (PM_PI / 180.0); + a1 = joints[1] * (PM_PI / 180.0); + a3 = joints[3] * (PM_PI / 180.0); + + /* convert angles into world coords */ + a1 = a1 + a0; + a3 = a3 + a1; + + x = params->d2*cos(a0) + params->d4*cos(a1) + params->d6*cos(a3); + y = params->d2*sin(a0) + params->d4*sin(a1) + params->d6*sin(a3); + z = params->d1 + params->d3 - joints[2] - params->d5; + c = a3; + + if (joints[1] < 90.0) { + flags = SCARA_ELBOW_UP; + } + + world->tran.x = x; + world->tran.y = y; + world->tran.z = z; + world->c = c * 180.0 / PM_PI; + + world->a = joints[4]; + world->b = joints[5]; + world->u = 0.0; + world->v = 0.0; + world->w = 0.0; + + /* Store flags if requested */ + if (iflags != NULL) { + *iflags = flags; + } + + return 0; +} + +/* + * Pure inverse kinematics - world coordinates to joints + * + * params: kinematics parameters (d1-d6) + * world: input world position (EmcPose) + * joints: output joint positions array (6 joints) + * iflags: input inverse kinematics flags (elbow configuration) + * fflags: output forward kinematics flags (can be NULL) + * + * Returns 0 on success, -1 on failure + */ +static int scara_inverse_math(const scara_params_t *params, + const EmcPose *world, + double *joints, + int iflags, + int *fflags) +{ + double a3; + double q0, q1; + double xt, yt, rsq, cc; + double x, y, z, c; + + x = world->tran.x; + y = world->tran.y; + z = world->tran.z; + c = world->c; + + /* convert degrees to radians */ + a3 = c * (PM_PI / 180.0); + + /* center of end effector (correct for D6) */ + xt = x - params->d6*cos(a3); + yt = y - params->d6*sin(a3); + + /* horizontal distance (squared) from end effector centerline + to main column centerline */ + rsq = xt*xt + yt*yt; + + /* joint 1 angle needed to make arm length match sqrt(rsq) */ + cc = (rsq - params->d2*params->d2 - params->d4*params->d4) / (2.0*params->d2*params->d4); + if (cc < -1.0) cc = -1.0; + if (cc > 1.0) cc = 1.0; + q1 = acos(cc); + + if (iflags & SCARA_ELBOW_UP) { + q1 = -q1; + } + + /* angle to end effector */ + q0 = atan2(yt, xt); + + /* end effector coords in inner arm coord system */ + xt = params->d2 + params->d4*cos(q1); + yt = params->d4*sin(q1); + + /* inner arm angle */ + q0 = q0 - atan2(yt, xt); + + /* q0 and q1 are still in radians. convert them to degrees */ + q0 = q0 * (180.0 / PM_PI); + q1 = q1 * (180.0 / PM_PI); + + joints[0] = q0; + joints[1] = q1; + joints[2] = params->d1 + params->d3 - params->d5 - z; + joints[3] = c - (q0 + q1); + joints[4] = world->a; + joints[5] = world->b; + + /* Store flags if requested */ + if (fflags != NULL) { + *fflags = 0; + } + + return 0; +} + +/* ======================================================================== + * RT interface (reads HAL pins) + * ======================================================================== */ + struct scara_data { hal_float_t *d1, *d2, *d3, *d4, *d5, *d6; } *haldata = 0; @@ -78,37 +236,20 @@ int scaraKinematicsForward(const double * joint, KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double a0, a1, a3; - double x, y, z, c; - -/* convert joint angles to radians for sin() and cos() */ - - a0 = joint[0] * ( PM_PI / 180 ); - a1 = joint[1] * ( PM_PI / 180 ); - a3 = joint[3] * ( PM_PI / 180 ); -/* convert angles into world coords */ - - a1 = a1 + a0; - a3 = a3 + a1; + scara_params_t params; + int flags_out = 0; - x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); - y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); - z = D1 + D3 - joint[2] - D5; - c = a3; + params.d1 = D1; + params.d2 = D2; + params.d3 = D3; + params.d4 = D4; + params.d5 = D5; + params.d6 = D6; - *iflags = 0; - if (joint[1] < 90) - *iflags = 1; + scara_forward_math(¶ms, joint, world, &flags_out); - world->tran.x = x; - world->tran.y = y; - world->tran.z = z; - world->c = c * 180 / PM_PI; - - world->a = joint[4]; - world->b = joint[5]; - - return (0); + *iflags = flags_out; + return 0; } //scaraKinematicsForward() static int scaraKinematicsInverse(const EmcPose * world, @@ -116,59 +257,20 @@ static int scaraKinematicsInverse(const EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { - double a3; - double q0, q1; - double xt, yt, rsq, cc; - double x, y, z, c; - - x = world->tran.x; - y = world->tran.y; - z = world->tran.z; - c = world->c; - - /* convert degrees to radians */ - a3 = c * ( PM_PI / 180 ); - - /* center of end effector (correct for D6) */ - xt = x - D6*cos(a3); - yt = y - D6*sin(a3); - - /* horizontal distance (squared) from end effector centerline - to main column centerline */ - rsq = xt*xt + yt*yt; - /* joint 1 angle needed to make arm length match sqrt(rsq) */ - cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4); - if(cc < -1) cc = -1; - if(cc > 1) cc = 1; - q1 = acos(cc); - - if (*iflags) - q1 = -q1; - - /* angle to end effector */ - q0 = atan2(yt, xt); - - /* end effector coords in inner arm coord system */ - xt = D2 + D4*cos(q1); - yt = D4*sin(q1); - - /* inner arm angle */ - q0 = q0 - atan2(yt, xt); - - /* q0 and q1 are still in radians. convert them to degrees */ - q0 = q0 * (180 / PM_PI); - q1 = q1 * (180 / PM_PI); + scara_params_t params; + int flags_out = 0; - joint[0] = q0; - joint[1] = q1; - joint[2] = D1 + D3 - D5 - z; - joint[3] = c - ( q0 + q1); - joint[4] = world->a; - joint[5] = world->b; + params.d1 = D1; + params.d2 = D2; + params.d3 = D3; + params.d4 = D4; + params.d5 = D5; + params.d6 = D6; - *fflags = 0; + scara_inverse_math(¶ms, world, joint, *iflags, &flags_out); - return (0); + *fflags = flags_out; + return 0; } // scaraKinematicsInverse() #define DEFAULT_D1 490 @@ -236,3 +338,64 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + scara_params_t p; + p.d1 = kp->params.scara.d1; + p.d2 = kp->params.scara.d2; + p.d3 = kp->params.scara.d3; + p.d4 = kp->params.scara.d4; + p.d5 = kp->params.scara.d5; + p.d6 = kp->params.scara.d6; + return scara_forward_math(&p, joints, pos, NULL); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + scara_params_t p; + p.d1 = kp->params.scara.d1; + p.d2 = kp->params.scara.d2; + p.d3 = kp->params.scara.d3; + p.d4 = kp->params.scara.d4; + p.d5 = kp->params.scara.d5; + p.d6 = kp->params.scara.d6; + return scara_inverse_math(&p, pos, joints, 0, NULL); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_bit; + (void)read_s32; + + read_float("scarakins.D1", &kp->params.scara.d1); + read_float("scarakins.D2", &kp->params.scara.d2); + read_float("scarakins.D3", &kp->params.scara.d3); + read_float("scarakins.D4", &kp->params.scara.d4); + read_float("scarakins.D5", &kp->params.scara.d5); + read_float("scarakins.D6", &kp->params.scara.d6); + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index ee00c709ac4..86f853e40e5 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -1,4 +1,3 @@ - // // This is a kinematics module for the Scorbot ER 3. // @@ -39,37 +38,183 @@ #include "kinematics.h" + +#ifdef RTAPI #include "rtapi_math.h" -#include "gotypes.h" +#else +#include +#endif +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif -// -// linkage constants, in mm & degrees -// +#ifndef TO_RAD +#define TO_RAD (M_PI / 180.0) +#endif + +#ifndef TO_DEG +#define TO_DEG (180.0 / M_PI) +#endif + +/* Parameters struct for Scorbot kinematics */ +typedef struct { + double l0_horizontal; /* Horizontal distance from J0 to J1 */ + double l0_vertical; /* Vertical distance from ground to J1 */ + double l1_length; /* Link 1: J1 (shoulder) to J2 (elbow) */ + double l2_length; /* Link 2: J2 (elbow) to wrist */ +} scorbot_params_t; + +/* Default values for Scorbot ER-3 (in mm) */ +#define SCORBOT_DEFAULT_L0_HORIZONTAL 16.0 +#define SCORBOT_DEFAULT_L0_VERTICAL 140.0 +#define SCORBOT_DEFAULT_L1_LENGTH 221.0 +#define SCORBOT_DEFAULT_L2_LENGTH 221.0 + +/* + * Pure forward kinematics - joints to world coordinates + * + * Returns 0 on success + */ +static int scorbot_forward_math(const scorbot_params_t *params, + const double *joints, + EmcPose *world) +{ + double j0_rad = joints[0] * TO_RAD; + double j1_rad = joints[1] * TO_RAD; + double j2_rad = joints[2] * TO_RAD; + + /* J1 location (shoulder) - fixed offset from base */ + double j1_x = params->l0_horizontal * cos(j0_rad); + double j1_y = params->l0_horizontal * sin(j0_rad); + double j1_z = params->l0_vertical; + + /* J2 location (elbow) - Link 1 from shoulder */ + double r1 = params->l1_length * cos(j1_rad); + double j2_x = r1 * cos(j0_rad); + double j2_y = r1 * sin(j0_rad); + double j2_z = params->l1_length * sin(j1_rad); + + /* Wrist location (controlled point) - Link 2 from elbow */ + double r2 = params->l2_length * cos(j2_rad); + double j3_x = r2 * cos(j0_rad); + double j3_y = r2 * sin(j0_rad); + double j3_z = params->l2_length * sin(j2_rad); + + /* End-effector is sum of all linkage vectors */ + world->tran.x = j1_x + j2_x + j3_x; + world->tran.y = j1_y + j2_y + j3_y; + world->tran.z = j1_z + j2_z + j3_z; + + /* Wrist pitch and roll passed through */ + world->a = joints[3]; + world->b = joints[4]; + world->c = 0.0; + world->u = 0.0; + world->v = 0.0; + world->w = 0.0; + + return 0; +} + +/* + * Pure inverse kinematics - world coordinates to joints + * + * Returns 0 on success, -1 on error (out of reach) + */ +static int scorbot_inverse_math(const scorbot_params_t *params, + const EmcPose *world, + double *joints) +{ + double r_j1, z_j1; /* J1 location in RZ plane */ + double r_cp, z_cp; /* Controlled point in RZ plane */ + double distance_to_cp, distance_to_center; + double angle_to_cp; + double j1_angle; + double z_j2; + + /* J0: base rotation - project pose onto XY plane */ + joints[0] = TO_DEG * atan2(world->tran.y, world->tran.x); + + /* Work in the RZ plane (vertical plane defined by J0 angle) */ + /* J1 location is a known, static vector */ + r_j1 = params->l0_horizontal; + z_j1 = params->l0_vertical; + + /* Controlled point in RZ plane */ + r_cp = sqrt(world->tran.x * world->tran.x + world->tran.y * world->tran.y); + z_cp = world->tran.z; + + /* Translate so J1 is the origin */ + r_cp -= r_j1; + z_cp -= z_j1; + + /* + * Now the origin (J1), J2, and CP define a triangle. + * Bisect the base and use law of cosines. + */ + distance_to_cp = sqrt(r_cp * r_cp + z_cp * z_cp); + distance_to_center = distance_to_cp / 2.0; + + /* Check reach limits */ + if (distance_to_cp > (params->l1_length + params->l2_length)) { + /* Out of reach - too far */ + return -1; + } + if (distance_to_cp < fabs(params->l1_length - params->l2_length)) { + /* Out of reach - too close (armpit) */ + return -1; + } -// Link 0 connects the origin to J1 (shoulder) -// These dimensions come off a drawing I got from Intelitek. -#define L0_HORIZONTAL_DISTANCE 16 -#define L0_VERTICAL_DISTANCE 140 - -#define L1_LENGTH 221 // Link 1 connects J1 (shoulder) to J2 (elbow) -#define L2_LENGTH 221 // Link 2 connects J2 (shoulder) to the wrist - - -// Compute the cartesian coordinates of J1, given the J0 angle (and the -// fixed, known link L0 between J0 and J1). -static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { - j1_cart->tran.x = L0_HORIZONTAL_DISTANCE * cos(TO_RAD * j0); - j1_cart->tran.y = L0_HORIZONTAL_DISTANCE * sin(TO_RAD * j0); - j1_cart->tran.z = L0_VERTICAL_DISTANCE; - j1_cart->a = 0; - j1_cart->b = 0; - j1_cart->c = 0; - j1_cart->u = 0; - j1_cart->v = 0; - j1_cart->w = 0; + /* Angle from J1 to controlled point */ + if (distance_to_cp > 0.0) { + angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); + if (z_cp < 0.0) { + angle_to_cp = -angle_to_cp; + } + } else { + angle_to_cp = 0.0; + } + + /* Angle at J1 in the (J1, Center, J2) right triangle */ + if (params->l1_length > 0.0) { + double cos_val = distance_to_center / params->l1_length; + if (cos_val > 1.0) cos_val = 1.0; + if (cos_val < -1.0) cos_val = -1.0; + j1_angle = TO_DEG * acos(cos_val); + } else { + j1_angle = 0.0; + } + + joints[1] = angle_to_cp + j1_angle; + + /* Compute J2 location to find J2 angle */ + z_j2 = params->l1_length * sin(joints[1] * TO_RAD); + + if (params->l2_length > 0.0) { + double sin_val = (z_j2 - z_cp) / params->l2_length; + if (sin_val > 1.0) sin_val = 1.0; + if (sin_val < -1.0) sin_val = -1.0; + joints[2] = -1.0 * TO_DEG * asin(sin_val); + } else { + joints[2] = 0.0; + } + + /* Wrist pitch and roll passed through */ + joints[3] = world->a; + joints[4] = world->b; + + return 0; } +// Static params struct with default Scorbot ER-3 dimensions +static scorbot_params_t scorbot_params = { + .l0_horizontal = SCORBOT_DEFAULT_L0_HORIZONTAL, + .l0_vertical = SCORBOT_DEFAULT_L0_VERTICAL, + .l1_length = SCORBOT_DEFAULT_L1_LENGTH, + .l2_length = SCORBOT_DEFAULT_L2_LENGTH +}; + // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. @@ -81,42 +226,7 @@ int kinematicsForward( ) { (void)fflags; (void)iflags; - EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) - EmcPose j2_vector; // the vector from j1 ("shoulder") to joint 2 ("elbow", end of link 1) - EmcPose j3_vector; // the vector from j2 ("elbow") to joint 3 ("wrist", end of link 2) - - double r; - - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); - compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); - - // Link 1 connects j1 (shoulder) to j2 (elbow). - r = L1_LENGTH * cos(TO_RAD * joints[1]); - j2_vector.tran.x = r * cos(TO_RAD * joints[0]); - j2_vector.tran.y = r * sin(TO_RAD * joints[0]); - j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); - - // Link 2 connects j2 (elbow) to j3 (wrist). - // J3 is the controlled point. - r = L2_LENGTH * cos(TO_RAD * joints[2]); - j3_vector.tran.x = r * cos(TO_RAD * joints[0]); - j3_vector.tran.y = r * sin(TO_RAD * joints[0]); - j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); - - // The end-effector location is the sum of the linkage vectors. - pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; - pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; - pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); - - // A and B are wrist roll and pitch, handled in hal by external kinematics - pose->a = joints[3]; - pose->b = joints[4]; - - return 0; + return scorbot_forward_math(&scorbot_params, joints, pose); } @@ -140,155 +250,7 @@ int kinematicsInverse( ) { (void)iflags; (void)fflags; - // EmcPose j1_cart; - double distance_to_cp, distance_to_center; - double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane - double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane - double angle_to_cp; - double j1_angle; - - // the location of J2, this is what we're trying to find - double z_j2; - - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. - // J0 points at the projected (X, Y) point. tan(J0) = Y/X - // J0 then defines the plane that the rest of the arm operates in. - joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); - - // FIXME: Until i figure the wrist differential out, the controlled - // point will be the location of the wrist joint, J3/J4. - - // The location of J1 (computed above) and the location of the - // controlled point are separated by J1, L1, J2, and L2. L1 and L2 are - // known, but J1 and J2 are not. - - // (r_j1, z_j1) is the location of J1 in the RZ plane (the vertical - // plane defined by the angle of J0, with the origin at the location - // of J0. This is just a known, static vector. - r_j1 = L0_HORIZONTAL_DISTANCE; - z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); - - // (r_cp, z_cp) is the location of J3 (the controlled point), again in - // the plane defined by the angle of J0, with the origin of the - // machine. - r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); - z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); - - // translate so (r_j1, z_j1) is the origin of the coordinate system - r_cp -= r_j1; - z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); - - // - // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. - // The triangle is isosceles, because from the origin to J2 is L1, and - // from J2 to CP is L2, and L1 and L2 are the same length. - // - // Bisect the base of that triangle, and call the center point of the - // base "Center". - // - // Draw a line between J2 and Center. This defines two right - // triangles: (J1, J2, Center) and (CP, J2, Center). - // - // The length of the (J1, Center) and (CP, Center) lines are equal, and - // are half the distance from the origin to CP. - // - - distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); - distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); - - // find the angle of the vector from the origin to the CP - angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); - if (z_cp < 0) { - angle_to_cp *= -1; - } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); - - // find the angle (Center, J1, J2) - j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); - - joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); - - // now we can compute the location of J2 - z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); - - joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); - - -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); - - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; - } - - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); - - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); - } - - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); - } -#endif - - // A and B are wrist roll and pitch, handled in hal by external kinematics - joints[3] = pose->a; - joints[4] = pose->b; - - return 0; + return scorbot_inverse_math(&scorbot_params, pose, joints); } @@ -301,10 +263,13 @@ KINEMATICS_TYPE kinematicsType(void) { #include "rtapi_app.h" #include "hal.h" +const char* kinematicsGetName(void) { return "scorbot-kins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); static int comp_id; @@ -322,3 +287,50 @@ void rtapi_app_exit(void) { hal_exit(comp_id); } +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + scorbot_params_t p; + p.l0_horizontal = kp->params.scorbot.l0_horizontal; + p.l0_vertical = kp->params.scorbot.l0_vertical; + p.l1_length = kp->params.scorbot.l1_length; + p.l2_length = kp->params.scorbot.l2_length; + return scorbot_forward_math(&p, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + scorbot_params_t p; + p.l0_horizontal = kp->params.scorbot.l0_horizontal; + p.l0_vertical = kp->params.scorbot.l0_vertical; + p.l1_length = kp->params.scorbot.l1_length; + p.l2_length = kp->params.scorbot.l2_length; + return scorbot_inverse_math(&p, pos, joints); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + (void)params; (void)read_float; (void)read_bit; (void)read_s32; + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); + diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index bfb225125c0..aa1af6f6793 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -232,11 +232,17 @@ RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static char *sparm; RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); +const char* kinematicsGetName(void) +{ + return kp.kinsname; +} + EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 0111d08ada0..c3debd9dbc4 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -72,6 +72,112 @@ #endif #endif +/* Parameters struct for tripod kinematics */ +typedef struct { + double bx; /* X coordinate of point B */ + double cx; /* X coordinate of point C */ + double cy; /* Y coordinate of point C */ +} tripod_params_t; + +#ifndef tripod_sq +#define tripod_sq(x) ((x)*(x)) +#endif + +/* + * Pure forward kinematics - strut lengths to Cartesian position + * + * Takes three strut lengths and computes Dx, Dy, and Dz. + * The forward flag resolves D above/below the xy plane. + * + * Returns 0 on success, -1 on singularity + */ +static int tripod_forward_math(const tripod_params_t *params, + const double *joints, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + double P, Q, R; + double s, t, u; + double AD = joints[0]; + double BD = joints[1]; + double CD = joints[2]; + double Bx_v = params->bx; + double Cx_v = params->cx; + double Cy_v = params->cy; + + P = tripod_sq(AD); + Q = tripod_sq(BD) - tripod_sq(Bx_v); + R = tripod_sq(CD) - tripod_sq(Cx_v) - tripod_sq(Cy_v); + s = -2.0 * Bx_v; + t = -2.0 * Cx_v; + u = -2.0 * Cy_v; + + if (s == 0.0) { + /* points A and B coincident */ + return -1; + } + pos->tran.x = (Q - P) / s; + + if (u == 0.0) { + /* points A B C are colinear */ + return -1; + } + pos->tran.y = (R - Q - (t - s) * pos->tran.x) / u; + pos->tran.z = P - tripod_sq(pos->tran.x) - tripod_sq(pos->tran.y); + if (pos->tran.z < 0.0) { + /* triangle inequality violated */ + return -1; + } + pos->tran.z = sqrt(pos->tran.z); + if (fflags && *fflags) { + pos->tran.z = -pos->tran.z; + } + + pos->a = 0.0; + pos->b = 0.0; + pos->c = 0.0; + pos->u = 0.0; + pos->v = 0.0; + pos->w = 0.0; + + return 0; +} + +/* + * Pure inverse kinematics - Cartesian position to strut lengths + * + * Returns 0 on success + */ +static int tripod_inverse_math(const tripod_params_t *params, + const EmcPose *pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)iflags; + double Dx = pos->tran.x; + double Dy = pos->tran.y; + double Dz = pos->tran.z; + double Bx_v = params->bx; + double Cx_v = params->cx; + double Cy_v = params->cy; + + joints[0] = sqrt(tripod_sq(Dx) + tripod_sq(Dy) + tripod_sq(Dz)); + joints[1] = sqrt(tripod_sq(Dx - Bx_v) + tripod_sq(Dy) + tripod_sq(Dz)); + joints[2] = sqrt(tripod_sq(Dx - Cx_v) + tripod_sq(Dy - Cy_v) + tripod_sq(Dz)); + + if (fflags) { + *fflags = 0; + if (Dz < 0.0) { + *fflags = 1; + } + } + + return 0; +} + #include "hal.h" struct haldata { hal_float_t *bx, *cx, *cy; @@ -81,8 +187,6 @@ struct haldata { #define Cx (*(haldata->cx)) #define Cy (*(haldata->cy)) -#define sq(x) ((x)*(x)) - /* forward kinematics takes three strut lengths and computes Dx, Dy, and Dz pos->tran.x,y,z, respectively. The forward flag is used to resolve @@ -128,56 +232,11 @@ int kinematicsForward(const double * joints, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { - (void)iflags; -#define AD (joints[0]) -#define BD (joints[1]) -#define CD (joints[2]) -#define Dx (pos->tran.x) -#define Dy (pos->tran.y) -#define Dz (pos->tran.z) - double P, Q, R; - double s, t, u; - - P = sq(AD); - Q = sq(BD) - sq(Bx); - R = sq(CD) - sq(Cx) - sq(Cy); - s = -2.0 * Bx; - t = -2.0 * Cx; - u = -2.0 * Cy; - - if (s == 0.0) { - /* points A and B coincident. Fix Bx, #defined up top. */ - return -1; - } - Dx = (Q - P) / s; - - if (u == 0.0) { - /* points A B C are colinear. Fix Cy, #defined up top. */ - return -1; - } - Dy = (R - Q - (t - s) * Dx) / u; - Dz = P - sq(Dx) - sq(Dy); - if (Dz < 0.0) { - /* triangle inequality violated */ - return -1; - } - Dz = sqrt(Dz); - if (*fflags) { - Dz = -Dz; - } - - pos->a = 0.0; - pos->b = 0.0; - pos->c = 0.0; - - return 0; - -#undef AD -#undef BD -#undef CD -#undef Dx -#undef Dy -#undef Dz + tripod_params_t params; + params.bx = Bx; + params.cx = Cx; + params.cy = Cy; + return tripod_forward_math(¶ms, joints, pos, fflags, iflags); } int kinematicsInverse(const EmcPose * pos, @@ -185,31 +244,11 @@ int kinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { - (void)iflags; -#define AD (joints[0]) -#define BD (joints[1]) -#define CD (joints[2]) -#define Dx (pos->tran.x) -#define Dy (pos->tran.y) -#define Dz (pos->tran.z) - - AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); - BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); - CD = sqrt(sq(Dx - Cx) + sq(Dy - Cy) + sq(Dz)); - - *fflags = 0; - if (Dz < 0.0) { - *fflags = 1; - } - - return 0; - -#undef AD -#undef BD -#undef CD -#undef Dx -#undef Dy -#undef Dz + tripod_params_t params; + params.bx = Bx; + params.cx = Cx; + params.cy = Cy; + return tripod_inverse_math(¶ms, pos, joints, iflags, fflags); } KINEMATICS_TYPE kinematicsType() @@ -350,10 +389,13 @@ int main(int argc, char *argv[]) #include "rtapi_app.h" /* RTAPI realtime module decls */ #include "hal.h" +const char* kinematicsGetName(void) { return "tripodkins"; } + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); @@ -383,3 +425,57 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + tripod_params_t p; + p.bx = kp->params.tripod.bx; + p.cx = kp->params.tripod.cx; + p.cy = kp->params.tripod.cy; + return tripod_forward_math(&p, joints, pos, NULL, NULL); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + tripod_params_t p; + p.bx = kp->params.tripod.bx; + p.cx = kp->params.tripod.cx; + p.cy = kp->params.tripod.cy; + return tripod_inverse_math(&p, pos, joints, NULL, NULL); +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_bit; (void)read_s32; + + if (read_float("tripodkins.Bx", &kp->params.tripod.bx) != 0) + return -1; + if (read_float("tripodkins.Cx", &kp->params.tripod.cx) != 0) + return -1; + if (read_float("tripodkins.Cy", &kp->params.tripod.cy) != 0) + return -1; + + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 263d4ab5189..4235df9d3dd 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -52,10 +52,16 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); +const char* kinematicsGetName(void) +{ + return "trivkins"; +} + KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsGetName); MODULE_LICENSE("GPL"); static int comp_id; @@ -85,3 +91,63 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" +#include + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + int i; + double axes[9] = {0}; + + for (i = 0; i < kp->num_joints; i++) { + int axis = kp->joint_to_axis[i]; + if (axis >= 0 && axis < 9) + axes[axis] = joints[i]; + } + pos->tran.x = axes[0]; pos->tran.y = axes[1]; pos->tran.z = axes[2]; + pos->a = axes[3]; pos->b = axes[4]; pos->c = axes[5]; + pos->u = axes[6]; pos->v = axes[7]; pos->w = axes[8]; + return 0; +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + int i; + double axes[9]; + + axes[0] = pos->tran.x; axes[1] = pos->tran.y; axes[2] = pos->tran.z; + axes[3] = pos->a; axes[4] = pos->b; axes[5] = pos->c; + axes[6] = pos->u; axes[7] = pos->v; axes[8] = pos->w; + + for (i = 0; i < kp->num_joints; i++) { + int axis = kp->joint_to_axis[i]; + joints[i] = (axis >= 0 && axis < 9) ? axes[axis] : 0.0; + } + return 0; +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + (void)params; (void)read_float; (void)read_bit; (void)read_s32; + return 0; +} + +int nonrt_is_identity(void) { return 1; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 44375842f5f..e2d661ea9a4 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,8 +25,8 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -37,6 +37,234 @@ #include "rtapi_string.h" #include "rtapi_ctype.h" +/* ======================================================================== + * TRT math types and functions (was in trtfuncs_math.h) + * ======================================================================== */ + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif +#ifndef TO_RAD +#define TO_RAD (M_PI / 180.0) +#endif + +typedef struct { + double x_rot_point; + double y_rot_point; + double z_rot_point; + double x_offset; + double y_offset; + double z_offset; + double tool_offset; + int conventional_directions; +} trt_params_t; + +typedef struct { + int jx, jy, jz; + int ja, jb, jc; + int ju, jv, jw; +} trt_joints_t; + +int xyzac_forward_math(const trt_params_t *params, + const trt_joints_t *jmap, + const double *joints, + EmcPose *pos) +{ + const double x_rot_point = params->x_rot_point; + const double y_rot_point = params->y_rot_point; + const double z_rot_point = params->z_rot_point; + const double dt = params->tool_offset; + const double dy = params->y_offset; + const double dz = params->z_offset + dt; + const double a_rad = joints[jmap->ja] * TO_RAD; + const double c_rad = joints[jmap->jc] * TO_RAD; + + const double con = params->conventional_directions ? 1.0 : -1.0; + + pos->tran.x = + cos(c_rad) * (joints[jmap->jx] - x_rot_point) + - con * sin(c_rad) * cos(a_rad) * (joints[jmap->jy] - dy - y_rot_point) + + sin(c_rad) * sin(a_rad) * (joints[jmap->jz] - dz - z_rot_point) + - con * sin(c_rad) * dy + + x_rot_point; + + pos->tran.y = + con * sin(c_rad) * (joints[jmap->jx] - x_rot_point) + + cos(c_rad) * cos(a_rad) * (joints[jmap->jy] - dy - y_rot_point) + - con * cos(c_rad) * sin(a_rad) * (joints[jmap->jz] - dz - z_rot_point) + + cos(c_rad) * dy + + y_rot_point; + + pos->tran.z = + 0 + + con * sin(a_rad) * (joints[jmap->jy] - dy - y_rot_point) + + cos(a_rad) * (joints[jmap->jz] - dz - z_rot_point) + + dz + + z_rot_point; + + pos->a = joints[jmap->ja]; + pos->c = joints[jmap->jc]; + + pos->b = (jmap->jb >= 0) ? joints[jmap->jb] : 0; + pos->u = (jmap->ju >= 0) ? joints[jmap->ju] : 0; + pos->v = (jmap->jv >= 0) ? joints[jmap->jv] : 0; + pos->w = (jmap->jw >= 0) ? joints[jmap->jw] : 0; + + return 0; +} + +int xyzac_inverse_math(const trt_params_t *params, + const trt_joints_t *jmap, + const EmcPose *pos, + EmcPose *axis_values) +{ + const double x_rot_point = params->x_rot_point; + const double y_rot_point = params->y_rot_point; + const double z_rot_point = params->z_rot_point; + const double dy = params->y_offset; + const double dt = params->tool_offset; + const double dz = params->z_offset + dt; + const double a_rad = pos->a * TO_RAD; + const double c_rad = pos->c * TO_RAD; + + const double con = params->conventional_directions ? 1.0 : -1.0; + + axis_values->tran.x = + cos(c_rad) * (pos->tran.x - x_rot_point) + + con * sin(c_rad) * (pos->tran.y - y_rot_point) + + x_rot_point; + + axis_values->tran.y = - con * sin(c_rad) * cos(a_rad) * (pos->tran.x - x_rot_point) + + cos(c_rad) * cos(a_rad) * (pos->tran.y - y_rot_point) + + con * sin(a_rad) * (pos->tran.z - z_rot_point) + - cos(a_rad) * dy + - con * sin(a_rad) * dz + + dy + + y_rot_point; + + axis_values->tran.z = + sin(c_rad) * sin(a_rad) * (pos->tran.x - x_rot_point) + - con * cos(c_rad) * sin(a_rad) * (pos->tran.y - y_rot_point) + + cos(a_rad) * (pos->tran.z - z_rot_point) + + con * sin(a_rad) * dy + - cos(a_rad) * dz + + dz + + z_rot_point; + + axis_values->a = pos->a; + axis_values->c = pos->c; + + axis_values->b = (jmap->jb >= 0) ? pos->b : 0; + axis_values->u = (jmap->ju >= 0) ? pos->u : 0; + axis_values->v = (jmap->jv >= 0) ? pos->v : 0; + axis_values->w = (jmap->jw >= 0) ? pos->w : 0; + + return 0; +} + +int xyzbc_forward_math(const trt_params_t *params, + const trt_joints_t *jmap, + const double *joints, + EmcPose *pos) +{ + const double x_rot_point = params->x_rot_point; + const double y_rot_point = params->y_rot_point; + const double z_rot_point = params->z_rot_point; + const double dx = params->x_offset; + const double dt = params->tool_offset; + const double dz = params->z_offset + dt; + const double b_rad = joints[jmap->jb] * TO_RAD; + const double c_rad = joints[jmap->jc] * TO_RAD; + + const double con = params->conventional_directions ? 1.0 : -1.0; + + pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[jmap->jx] - dx - x_rot_point) + - con * sin(c_rad) * (joints[jmap->jy] - y_rot_point) + + con * cos(c_rad) * sin(b_rad) * (joints[jmap->jz] - dz - z_rot_point) + + cos(c_rad) * dx + + x_rot_point; + + pos->tran.y = + con * sin(c_rad) * cos(b_rad) * (joints[jmap->jx] - dx - x_rot_point) + + cos(c_rad) * (joints[jmap->jy] - y_rot_point) + + sin(c_rad) * sin(b_rad) * (joints[jmap->jz] - dz - z_rot_point) + + con * sin(c_rad) * dx + + y_rot_point; + + pos->tran.z = - con * sin(b_rad) * (joints[jmap->jx] - dx - x_rot_point) + + cos(b_rad) * (joints[jmap->jz] - dz - z_rot_point) + + dz + + z_rot_point; + + pos->b = joints[jmap->jb]; + pos->c = joints[jmap->jc]; + + pos->a = (jmap->ja >= 0) ? joints[jmap->ja] : 0; + pos->u = (jmap->ju >= 0) ? joints[jmap->ju] : 0; + pos->v = (jmap->jv >= 0) ? joints[jmap->jv] : 0; + pos->w = (jmap->jw >= 0) ? joints[jmap->jw] : 0; + + return 0; +} + +int xyzbc_inverse_math(const trt_params_t *params, + const trt_joints_t *jmap, + const EmcPose *pos, + EmcPose *axis_values) +{ + const double x_rot_point = params->x_rot_point; + const double y_rot_point = params->y_rot_point; + const double z_rot_point = params->z_rot_point; + const double dx = params->x_offset; + const double dt = params->tool_offset; + const double dz = params->z_offset + dt; + const double b_rad = pos->b * TO_RAD; + const double c_rad = pos->c * TO_RAD; + const double dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx; + const double dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz; + + const double con = params->conventional_directions ? 1.0 : -1.0; + + axis_values->tran.x = + cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point) + + con * sin(c_rad) * cos(b_rad) * (pos->tran.y - y_rot_point) + - con * sin(b_rad) * (pos->tran.z - z_rot_point) + + dpx + + x_rot_point; + + axis_values->tran.y = - con * sin(c_rad) * (pos->tran.x - x_rot_point) + + cos(c_rad) * (pos->tran.y - y_rot_point) + + y_rot_point; + + axis_values->tran.z = + con * cos(c_rad) * sin(b_rad) * (pos->tran.x - x_rot_point) + + sin(c_rad) * sin(b_rad) * (pos->tran.y - y_rot_point) + + cos(b_rad) * (pos->tran.z - z_rot_point) + + dpz + + z_rot_point; + + axis_values->b = pos->b; + axis_values->c = pos->c; + + axis_values->a = (jmap->ja >= 0) ? pos->a : 0; + axis_values->u = (jmap->ju >= 0) ? pos->u : 0; + axis_values->v = (jmap->jv >= 0) ? pos->v : 0; + axis_values->w = (jmap->jw >= 0) ? pos->w : 0; + + return 0; +} + +void trt_axis_to_joints(const trt_joints_t *jmap, + const EmcPose *axis_values, + double *joints) +{ + if (jmap->jx >= 0) joints[jmap->jx] = axis_values->tran.x; + if (jmap->jy >= 0) joints[jmap->jy] = axis_values->tran.y; + if (jmap->jz >= 0) joints[jmap->jz] = axis_values->tran.z; + if (jmap->ja >= 0) joints[jmap->ja] = axis_values->a; + if (jmap->jb >= 0) joints[jmap->jb] = axis_values->b; + if (jmap->jc >= 0) joints[jmap->jc] = axis_values->c; + if (jmap->ju >= 0) joints[jmap->ju] = axis_values->u; + if (jmap->jv >= 0) joints[jmap->jv] = axis_values->v; + if (jmap->jw >= 0) joints[jmap->jw] = axis_values->w; +} + +/* ======================================================================== + * RT interface (reads HAL pins) + * ======================================================================== */ + static int trtfuncs_max_joints; // joint number assignments (-1 ==> not assigned) @@ -52,6 +280,9 @@ static int JU = -1; static int JV = -1; static int JW = -1; +// Joint mapping struct for math functions +static trt_joints_t jmap; + struct haldata { hal_float_t *x_rot_point; hal_float_t *y_rot_point; @@ -117,6 +348,11 @@ int trtKinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} } + // Populate joint map struct for math functions + jmap.jx = JX; jmap.jy = JY; jmap.jz = JZ; + jmap.ja = JA; jmap.jb = JB; jmap.jc = JC; + jmap.ju = JU; jmap.jv = JV; jmap.jw = JW; + rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); for (jno=0; jnox_rot_point = *(haldata->x_rot_point); + params->y_rot_point = *(haldata->y_rot_point); + params->z_rot_point = *(haldata->z_rot_point); + params->x_offset = *(haldata->x_offset); + params->y_offset = *(haldata->y_offset); + params->z_offset = *(haldata->z_offset); + params->tool_offset = *(haldata->tool_offset); + params->conventional_directions = *(haldata->conventional_directions); +} + int xyzacKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -158,45 +407,9 @@ int xyzacKinematicsForward(const double *joints, { (void)fflags; (void)iflags; - const double x_rot_point = *(haldata->x_rot_point); - const double y_rot_point = *(haldata->y_rot_point); - const double z_rot_point = *(haldata->z_rot_point); - const double dt = *(haldata->tool_offset); - const double dy = *(haldata->y_offset); - const double dz = *(haldata->z_offset) + dt; - const double a_rad = joints[JA]*TO_RAD; - const double c_rad = joints[JC]*TO_RAD; - - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; - - pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) - + sin(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point) - - con * sin(c_rad) * dy - + x_rot_point; - - pos->tran.y = + con * sin(c_rad) * (joints[JX] - x_rot_point) - + cos(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) - - con * cos(c_rad) * sin(a_rad) * (joints[JZ] - dz - z_rot_point) - + cos(c_rad) * dy - + y_rot_point; - - pos->tran.z = + 0 - + con * sin(a_rad) * (joints[JY] - dy - y_rot_point) - + cos(a_rad) * (joints[JZ] - dz - z_rot_point) - + dz - + z_rot_point; - - pos->a = joints[JA]; - pos->c = joints[JC]; - - // optional letters (specify with coordinates module parameter) - pos->b = (JB != -1)? joints[JB] : 0; - pos->u = (JU != -1)? joints[JU] : 0; - pos->v = (JV != -1)? joints[JV] : 0; - pos->w = (JW != -1)? joints[JW] : 0; - - return 0; + trt_params_t params; + trt_get_params(¶ms); + return xyzac_forward_math(¶ms, &jmap, joints, pos); } // xyzacKinematicsForward() int xyzacKinematicsInverse(const EmcPose * pos, @@ -206,48 +419,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - const double x_rot_point = *(haldata->x_rot_point); - const double y_rot_point = *(haldata->y_rot_point); - const double z_rot_point = *(haldata->z_rot_point); - const double dy = *(haldata->y_offset); - const double dt = *(haldata->tool_offset); - const double dz = *(haldata->z_offset) + dt; - const double a_rad = pos->a*TO_RAD; - const double c_rad = pos->c*TO_RAD; - - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; - - EmcPose P; // computed position - - P.tran.x = + cos(c_rad) * (pos->tran.x - x_rot_point) - + con * sin(c_rad) * (pos->tran.y - y_rot_point) - + x_rot_point; - - P.tran.y = - con * sin(c_rad) * cos(a_rad) * (pos->tran.x - x_rot_point) - + cos(c_rad) * cos(a_rad) * (pos->tran.y - y_rot_point) - + con * sin(a_rad) * (pos->tran.z - z_rot_point) - - cos(a_rad) * dy - - con * sin(a_rad) * dz - + dy - + y_rot_point; - - P.tran.z = + sin(c_rad) * sin(a_rad) * (pos->tran.x - x_rot_point) - - con * cos(c_rad) * sin(a_rad) * (pos->tran.y - y_rot_point) - + cos(a_rad) * (pos->tran.z - z_rot_point) - + con * sin(a_rad) * dy - - cos(a_rad) * dz - + dz - + z_rot_point; - - - P.a = pos->a; - P.c = pos->c; - - // optional letters (specify with coordinates module parameter) - P.b = (JB != -1)? pos->b : 0; - P.u = (JU != -1)? pos->u : 0; - P.v = (JV != -1)? pos->v : 0; - P.w = (JW != -1)? pos->w : 0; + trt_params_t params; + trt_get_params(¶ms); + + // Compute axis values using pure math function + EmcPose P; + xyzac_inverse_math(¶ms, &jmap, pos, &P); // update joints with support for // multiple-joints per-coordinate letter: @@ -266,45 +443,9 @@ int xyzbcKinematicsForward(const double *joints, { (void)fflags; (void)iflags; - // Note: 'principal' joints are used - const double x_rot_point = *(haldata->x_rot_point); - const double y_rot_point = *(haldata->y_rot_point); - const double z_rot_point = *(haldata->z_rot_point); - const double dx = *(haldata->x_offset); - const double dt = *(haldata->tool_offset); - const double dz = *(haldata->z_offset) + dt; - const double b_rad = joints[JB]*TO_RAD; - const double c_rad = joints[JC]*TO_RAD; - - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; - - pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - - con * sin(c_rad) * (joints[JY] - y_rot_point) - + con * cos(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point) - + cos(c_rad) * dx - + x_rot_point; - - pos->tran.y = + con * sin(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - + cos(c_rad) * (joints[JY] - y_rot_point) - + sin(c_rad) * sin(b_rad) * (joints[JZ] - dz - z_rot_point) - + con * sin(c_rad) * dx - + y_rot_point; - - pos->tran.z = - con * sin(b_rad) * (joints[JX] - dx - x_rot_point) - + cos(b_rad) * (joints[JZ] - dz - z_rot_point) - + dz - + z_rot_point; - - pos->b = joints[JB]; - pos->c = joints[JC]; - - // optional letters (specify with coordinates module parameter) - pos->a = (JA != -1)? joints[JA] : 0; - pos->u = (JU != -1)? joints[JU] : 0; - pos->v = (JV != -1)? joints[JV] : 0; - pos->w = (JW != -1)? joints[JW] : 0; - - return 0; + trt_params_t params; + trt_get_params(¶ms); + return xyzbc_forward_math(¶ms, &jmap, joints, pos); } // xyzbcKinematicsForward() int xyzbcKinematicsInverse(const EmcPose * pos, @@ -314,45 +455,12 @@ int xyzbcKinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - const double x_rot_point = *(haldata->x_rot_point); - const double y_rot_point = *(haldata->y_rot_point); - const double z_rot_point = *(haldata->z_rot_point); - const double dx = *(haldata->x_offset); - const double dt = *(haldata->tool_offset); - const double dz = *(haldata->z_offset) + dt; - const double b_rad = pos->b*TO_RAD; - const double c_rad = pos->c*TO_RAD; - const double dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx; - const double dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz; - - const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0; - - EmcPose P; // computed position - - P.tran.x = + cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point) - + con * sin(c_rad) * cos(b_rad) * (pos->tran.y - y_rot_point) - - con * sin(b_rad) * (pos->tran.z - z_rot_point) - + dpx - + x_rot_point; - - P.tran.y = - con * sin(c_rad) * (pos->tran.x - x_rot_point) - + cos(c_rad) * (pos->tran.y - y_rot_point) - + y_rot_point; - - P.tran.z = + con * cos(c_rad) * sin(b_rad) * (pos->tran.x - x_rot_point) - + sin(c_rad) * sin(b_rad) * (pos->tran.y - y_rot_point) - + cos(b_rad) * (pos->tran.z - z_rot_point) - + dpz - + z_rot_point; - - P.b = pos->b; - P.c = pos->c; - - // optional letters (specify with coordinates module parameter) - P.a = (JA != -1)? pos->a : 0; - P.u = (JU != -1)? pos->u : 0; - P.v = (JV != -1)? pos->v : 0; - P.w = (JW != -1)? pos->w : 0; + trt_params_t params; + trt_get_params(¶ms); + + // Compute axis values using pure math function + EmcPose P; + xyzbc_inverse_math(¶ms, &jmap, pos, &P); // update joints with support for // multiple-joints per-coordinate letter: diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b11c5352e0e..e44e3809ddb 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -7,7 +7,7 @@ * 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 * 3) the 0th switchkins_type is the startup default * 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 5) The directions of the rotational axes are the opposite of the * conventional axis directions. */ @@ -54,3 +54,97 @@ int switchkinsSetup(kparms* kp, return 0; } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +/* TRT math types and functions (defined in trtfuncs.c, linked into this .so) */ +typedef struct { + double x_rot_point, y_rot_point, z_rot_point; + double x_offset, y_offset, z_offset, tool_offset; + int conventional_directions; +} trt_params_t; + +typedef struct { + int jx, jy, jz, ja, jb, jc, ju, jv, jw; +} trt_joints_t; + +extern int xyzac_forward_math(const trt_params_t *, const trt_joints_t *, + const double *, EmcPose *); +extern int xyzac_inverse_math(const trt_params_t *, const trt_joints_t *, + const EmcPose *, EmcPose *); +extern void trt_axis_to_joints(const trt_joints_t *, const EmcPose *, double *); + +static void nonrt_build_trt(const kinematics_params_t *kp, + trt_params_t *p, trt_joints_t *jm) +{ + p->x_rot_point = kp->params.trt.x_rot_point; + p->y_rot_point = kp->params.trt.y_rot_point; + p->z_rot_point = kp->params.trt.z_rot_point; + p->x_offset = kp->params.trt.x_offset; + p->y_offset = kp->params.trt.y_offset; + p->z_offset = kp->params.trt.z_offset; + p->tool_offset = kp->params.trt.tool_offset; + p->conventional_directions = kp->params.trt.conventional_directions; + + jm->jx = kp->axis_to_joint[0]; jm->jy = kp->axis_to_joint[1]; + jm->jz = kp->axis_to_joint[2]; jm->ja = kp->axis_to_joint[3]; + jm->jb = kp->axis_to_joint[4]; jm->jc = kp->axis_to_joint[5]; + jm->ju = kp->axis_to_joint[6]; jm->jv = kp->axis_to_joint[7]; + jm->jw = kp->axis_to_joint[8]; +} + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + trt_params_t p; + trt_joints_t jm; + nonrt_build_trt(kp, &p, &jm); + return xyzac_forward_math(&p, &jm, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + trt_params_t p; + trt_joints_t jm; + EmcPose axis_values; + + nonrt_build_trt(kp, &p, &jm); + xyzac_inverse_math(&p, &jm, pos, &axis_values); + trt_axis_to_joints(&jm, &axis_values, joints); + return 0; +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_s32; + + read_float("xyzac-trt-kins.x-rot-point", &kp->params.trt.x_rot_point); + read_float("xyzac-trt-kins.y-rot-point", &kp->params.trt.y_rot_point); + read_float("xyzac-trt-kins.z-rot-point", &kp->params.trt.z_rot_point); + read_float("xyzac-trt-kins.x-offset", &kp->params.trt.x_offset); + read_float("xyzac-trt-kins.y-offset", &kp->params.trt.y_offset); + read_float("xyzac-trt-kins.z-offset", &kp->params.trt.z_offset); + read_float("xyzac-trt-kins.tool-offset", &kp->params.trt.tool_offset); + read_bit("xyzac-trt-kins.conventional-directions", + &kp->params.trt.conventional_directions); + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 4f499ccf6b2..d8302b71e6e 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -7,7 +7,7 @@ * 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 * 3) the 0th switchkins_type is the startup default * 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 5) The directions of the rotational axes are the opposite of the * conventional axis directions. */ @@ -54,3 +54,97 @@ int switchkinsSetup(kparms* kp, return 0; } + +/* ======================================================================== + * Non-RT interface for userspace trajectory planner + * ======================================================================== */ +#include "kinematics_params.h" + +/* TRT math types and functions (defined in trtfuncs.c, linked into this .so) */ +typedef struct { + double x_rot_point, y_rot_point, z_rot_point; + double x_offset, y_offset, z_offset, tool_offset; + int conventional_directions; +} trt_params_t; + +typedef struct { + int jx, jy, jz, ja, jb, jc, ju, jv, jw; +} trt_joints_t; + +extern int xyzbc_forward_math(const trt_params_t *, const trt_joints_t *, + const double *, EmcPose *); +extern int xyzbc_inverse_math(const trt_params_t *, const trt_joints_t *, + const EmcPose *, EmcPose *); +extern void trt_axis_to_joints(const trt_joints_t *, const EmcPose *, double *); + +static void nonrt_build_trt(const kinematics_params_t *kp, + trt_params_t *p, trt_joints_t *jm) +{ + p->x_rot_point = kp->params.trt.x_rot_point; + p->y_rot_point = kp->params.trt.y_rot_point; + p->z_rot_point = kp->params.trt.z_rot_point; + p->x_offset = kp->params.trt.x_offset; + p->y_offset = kp->params.trt.y_offset; + p->z_offset = kp->params.trt.z_offset; + p->tool_offset = kp->params.trt.tool_offset; + p->conventional_directions = kp->params.trt.conventional_directions; + + jm->jx = kp->axis_to_joint[0]; jm->jy = kp->axis_to_joint[1]; + jm->jz = kp->axis_to_joint[2]; jm->ja = kp->axis_to_joint[3]; + jm->jb = kp->axis_to_joint[4]; jm->jc = kp->axis_to_joint[5]; + jm->ju = kp->axis_to_joint[6]; jm->jv = kp->axis_to_joint[7]; + jm->jw = kp->axis_to_joint[8]; +} + +int nonrt_kinematicsForward(const void *params, + const double *joints, + EmcPose *pos) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + trt_params_t p; + trt_joints_t jm; + nonrt_build_trt(kp, &p, &jm); + return xyzbc_forward_math(&p, &jm, joints, pos); +} + +int nonrt_kinematicsInverse(const void *params, + const EmcPose *pos, + double *joints) +{ + const kinematics_params_t *kp = (const kinematics_params_t *)params; + trt_params_t p; + trt_joints_t jm; + EmcPose axis_values; + + nonrt_build_trt(kp, &p, &jm); + xyzbc_inverse_math(&p, &jm, pos, &axis_values); + trt_axis_to_joints(&jm, &axis_values, joints); + return 0; +} + +int nonrt_refresh(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)) +{ + kinematics_params_t *kp = (kinematics_params_t *)params; + (void)read_s32; + + read_float("xyzbc-trt-kins.x-rot-point", &kp->params.trt.x_rot_point); + read_float("xyzbc-trt-kins.y-rot-point", &kp->params.trt.y_rot_point); + read_float("xyzbc-trt-kins.z-rot-point", &kp->params.trt.z_rot_point); + read_float("xyzbc-trt-kins.x-offset", &kp->params.trt.x_offset); + read_float("xyzbc-trt-kins.y-offset", &kp->params.trt.y_offset); + read_float("xyzbc-trt-kins.z-offset", &kp->params.trt.z_offset); + read_float("xyzbc-trt-kins.tool-offset", &kp->params.trt.tool_offset); + read_bit("xyzbc-trt-kins.conventional-directions", + &kp->params.trt.conventional_directions); + return 0; +} + +int nonrt_is_identity(void) { return 0; } + +EXPORT_SYMBOL(nonrt_kinematicsForward); +EXPORT_SYMBOL(nonrt_kinematicsInverse); +EXPORT_SYMBOL(nonrt_refresh); +EXPORT_SYMBOL(nonrt_is_identity); diff --git a/src/emc/kinematics_userspace/hal_pin_reader.c b/src/emc/kinematics_userspace/hal_pin_reader.c new file mode 100644 index 00000000000..762c9020f29 --- /dev/null +++ b/src/emc/kinematics_userspace/hal_pin_reader.c @@ -0,0 +1,200 @@ +/******************************************************************** + * Description: hal_pin_reader.c + * Simple HAL pin reader for userspace kinematics parameter refresh. + * Attaches to HAL shared memory and reads pin values by name. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif +#include +#include +#include +#include + +#include "rtapi.h" +#include "hal.h" +#include "hal_priv.h" +#include "hal_pin_reader.h" + +/* Static state for HAL attachment */ +static struct { + int attached; + int rtapi_module_id; + int rtapi_shmem_id; + char *hal_shmem_base; + hal_data_t *hal_data; +} hal_reader_ctx; + +/* + * Attach to HAL shared memory + */ +static int hal_reader_attach(void) +{ + void *mem; + int retval; + + if (hal_reader_ctx.attached) { + return 0; + } + + hal_reader_ctx.rtapi_module_id = rtapi_init("hal_pin_reader"); + if (hal_reader_ctx.rtapi_module_id < 0) { + return -1; + } + + hal_reader_ctx.rtapi_shmem_id = rtapi_shmem_new(HAL_KEY, + hal_reader_ctx.rtapi_module_id, + HAL_SIZE); + if (hal_reader_ctx.rtapi_shmem_id < 0) { + rtapi_exit(hal_reader_ctx.rtapi_module_id); + return -1; + } + + retval = rtapi_shmem_getptr(hal_reader_ctx.rtapi_shmem_id, &mem); + if (retval < 0) { + rtapi_shmem_delete(hal_reader_ctx.rtapi_shmem_id, + hal_reader_ctx.rtapi_module_id); + rtapi_exit(hal_reader_ctx.rtapi_module_id); + return -1; + } + + hal_reader_ctx.hal_shmem_base = (char *)mem; + hal_reader_ctx.hal_data = (hal_data_t *)mem; + + if (hal_reader_ctx.hal_data->version != HAL_VER) { + rtapi_shmem_delete(hal_reader_ctx.rtapi_shmem_id, + hal_reader_ctx.rtapi_module_id); + rtapi_exit(hal_reader_ctx.rtapi_module_id); + hal_reader_ctx.hal_shmem_base = NULL; + hal_reader_ctx.hal_data = NULL; + return -1; + } + + hal_reader_ctx.attached = 1; + return 0; +} + +/* + * Find a pin by name + */ +static hal_pin_t *find_pin_by_name(const char *name) +{ + int next; + hal_pin_t *pin; + hal_oldname_t *oldname; + hal_data_t *hd = hal_reader_ctx.hal_data; + char *base = hal_reader_ctx.hal_shmem_base; + + if (!hd || !base) return NULL; + + next = hd->pin_list_ptr; + while (next != 0) { + pin = (hal_pin_t *)(base + next); + + if (strcmp(pin->name, name) == 0) { + return pin; + } + + if (pin->oldname != 0) { + oldname = (hal_oldname_t *)(base + pin->oldname); + if (strcmp(oldname->name, name) == 0) { + return pin; + } + } + + next = pin->next_ptr; + } + + return NULL; +} + +/* + * Get pointer to pin data + */ +static void *get_pin_data_ptr(hal_pin_t *pin) +{ + hal_sig_t *sig; + hal_data_u *data; + char *base = hal_reader_ctx.hal_shmem_base; + + if (pin->signal != 0) { + sig = (hal_sig_t *)(base + pin->signal); + data = (hal_data_u *)(base + sig->data_ptr); + } else { + data = &pin->dummysig; + } + + switch (pin->type) { + case HAL_BIT: return (void *)&data->b; + case HAL_FLOAT: return (void *)&data->f; + case HAL_S32: return (void *)&data->s; + case HAL_U32: return (void *)&data->u; + case HAL_S64: return (void *)&data->ls; + case HAL_U64: return (void *)&data->lu; + default: return (void *)data; + } +} + +/* + * Public API: Read float pin + */ +int hal_pin_reader_read_float(const char *name, double *value) +{ + hal_pin_t *pin; + void *data_ptr; + + if (!name || !value) return -1; + if (hal_reader_attach() != 0) return -1; + + pin = find_pin_by_name(name); + if (!pin || pin->type != HAL_FLOAT) return -1; + + data_ptr = get_pin_data_ptr(pin); + *value = *((volatile hal_float_t *)data_ptr); + return 0; +} + +/* + * Public API: Read bit pin + */ +int hal_pin_reader_read_bit(const char *name, int *value) +{ + hal_pin_t *pin; + void *data_ptr; + + if (!name || !value) return -1; + if (hal_reader_attach() != 0) return -1; + + pin = find_pin_by_name(name); + if (!pin || pin->type != HAL_BIT) return -1; + + data_ptr = get_pin_data_ptr(pin); + *value = *((volatile hal_bit_t *)data_ptr) ? 1 : 0; + return 0; +} + +/* + * Public API: Read s32 pin + */ +int hal_pin_reader_read_s32(const char *name, int *value) +{ + hal_pin_t *pin; + void *data_ptr; + + if (!name || !value) return -1; + if (hal_reader_attach() != 0) return -1; + + pin = find_pin_by_name(name); + if (!pin || pin->type != HAL_S32) return -1; + + data_ptr = get_pin_data_ptr(pin); + *value = *((volatile hal_s32_t *)data_ptr); + return 0; +} diff --git a/src/emc/kinematics_userspace/hal_pin_reader.h b/src/emc/kinematics_userspace/hal_pin_reader.h new file mode 100644 index 00000000000..625ac1f96b8 --- /dev/null +++ b/src/emc/kinematics_userspace/hal_pin_reader.h @@ -0,0 +1,50 @@ +/******************************************************************** + * Description: hal_pin_reader.h + * Simple HAL pin reader for userspace kinematics parameter refresh. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef HAL_PIN_READER_H +#define HAL_PIN_READER_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Read a HAL float pin by name. + * + * @param name Full pin name (e.g., "5axiskins.pivot-length") + * @param value Pointer to store the pin value + * @return 0 on success, -1 if pin not found or wrong type + */ +int hal_pin_reader_read_float(const char *name, double *value); + +/** + * Read a HAL bit pin by name. + * + * @param name Full pin name (e.g., "maxkins.conventional-directions") + * @param value Pointer to store the pin value (0 or 1) + * @return 0 on success, -1 if pin not found or wrong type + */ +int hal_pin_reader_read_bit(const char *name, int *value); + +/** + * Read a HAL s32 pin by name. + * + * @param name Full pin name + * @param value Pointer to store the pin value + * @return 0 on success, -1 if pin not found or wrong type + */ +int hal_pin_reader_read_s32(const char *name, int *value); + +#ifdef __cplusplus +} +#endif + +#endif /* HAL_PIN_READER_H */ diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..ba3abca20af --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,296 @@ +/******************************************************************** + * Description: kinematics_user.c + * Userspace kinematics loader for trajectory planning + * + * Loads the RT kinematics .so via dlopen and resolves nonrt_* + * functions exported by each kinematics module. This allows the + * userspace planner to call kinematics without RT dependencies. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "kinematics_user.h" +#include "hal_pin_reader.h" +#include "kinematics_params.h" +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +/* ======================================================================== + * Internal context definition + * ======================================================================== */ + +typedef int (*nonrt_forward_fn)(const void *params, const double *joints, EmcPose *pos); +typedef int (*nonrt_inverse_fn)(const void *params, const EmcPose *pos, double *joints); +typedef int (*nonrt_refresh_fn)(void *params, + int (*read_float)(const char *, double *), + int (*read_bit)(const char *, int *), + int (*read_s32)(const char *, int *)); +typedef int (*nonrt_is_identity_fn)(void); + +struct KinematicsUserContext { + kinematics_params_t params; + int initialized; + int is_identity; + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle to RT .so */ + nonrt_forward_fn forward; + nonrt_inverse_fn inverse; + nonrt_refresh_fn refresh; +}; + +/* ======================================================================== + * Helper functions + * ======================================================================== */ + +/* + * Build joint mapping from coordinates string + */ +static int build_joint_mapping(kinematics_params_t *kp, const char *coordinates) +{ + int joint = 0; + const char *c; + int i; + + if (!kp) return -1; + + /* Initialize all mappings to -1 */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + kp->joint_to_axis[i] = -1; + } + for (i = 0; i < 9; i++) { + kp->axis_to_joint[i] = -1; + } + + /* Use default coordinates if none provided */ + if (!coordinates || !*coordinates) { + coordinates = "XYZABCUVW"; + } + + /* Parse coordinates string */ + for (c = coordinates; *c && joint < EMCMOT_MAX_JOINTS && joint < kp->num_joints; c++) { + int axis = -1; + switch (toupper((unsigned char)*c)) { + case 'X': axis = 0; break; + case 'Y': axis = 1; break; + case 'Z': axis = 2; break; + case 'A': axis = 3; break; + case 'B': axis = 4; break; + case 'C': axis = 5; break; + case 'U': axis = 6; break; + case 'V': axis = 7; break; + case 'W': axis = 8; break; + default: continue; /* Skip invalid characters */ + } + kp->joint_to_axis[joint] = axis; + /* Build axis-to-joint mapping (first joint for each axis) */ + if (kp->axis_to_joint[axis] == -1) { + kp->axis_to_joint[axis] = joint; + } + joint++; + } + + return joint; +} + +/* ======================================================================== + * RT module loading via dlopen + * ======================================================================== */ + +/* + * Load an RT kinematics .so and resolve nonrt_* symbols. + * Returns 0 on success, -1 on failure. + */ +static int load_rt_module(KinematicsUserContext *ctx, const char *module_name) +{ + char module_path[512]; + void *handle; + nonrt_is_identity_fn is_identity_fn; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + + ctx->rt_handle = handle; + + /* Resolve nonrt function symbols */ + ctx->forward = (nonrt_forward_fn)dlsym(handle, "nonrt_kinematicsForward"); + ctx->inverse = (nonrt_inverse_fn)dlsym(handle, "nonrt_kinematicsInverse"); + ctx->refresh = (nonrt_refresh_fn)dlsym(handle, "nonrt_refresh"); + is_identity_fn = (nonrt_is_identity_fn)dlsym(handle, "nonrt_is_identity"); + + if (!ctx->forward || !ctx->inverse || !ctx->refresh || !is_identity_fn) { + fprintf(stderr, "kinematicsUserInit: missing nonrt symbols in '%s'\n", + module_path); + if (!ctx->forward) fprintf(stderr, " missing: nonrt_kinematicsForward\n"); + if (!ctx->inverse) fprintf(stderr, " missing: nonrt_kinematicsInverse\n"); + if (!ctx->refresh) fprintf(stderr, " missing: nonrt_refresh\n"); + if (!is_identity_fn) fprintf(stderr, " missing: nonrt_is_identity\n"); + dlclose(handle); + ctx->rt_handle = NULL; + return -1; + } + + ctx->is_identity = is_identity_fn(); + ctx->kins_type = ctx->is_identity ? KINEMATICS_IDENTITY : KINEMATICS_BOTH; + + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates) +{ + KinematicsUserContext *ctx; + kinematics_params_t *kp; + + /* Debug: print incoming parameters */ + fprintf(stderr, "kinematicsUserInit ENTRY: kins_type='%s', num_joints=%d, coordinates='%s'\n", + kins_type ? kins_type : "(null)", + num_joints, + coordinates ? coordinates : "(null)"); + + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + fprintf(stderr, "kinematicsUserInit: invalid num_joints %d\n", num_joints); + return NULL; + } + + /* Allocate context */ + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) { + fprintf(stderr, "kinematicsUserInit: memory allocation failed\n"); + return NULL; + } + + kp = &ctx->params; + kp->num_joints = num_joints; + + /* Copy module name and coordinates */ + if (kins_type) { + strncpy(kp->module_name, kins_type, sizeof(kp->module_name) - 1); + kp->module_name[sizeof(kp->module_name) - 1] = '\0'; + } + if (coordinates) { + strncpy(kp->coordinates, coordinates, sizeof(kp->coordinates) - 1); + kp->coordinates[sizeof(kp->coordinates) - 1] = '\0'; + } + + /* Build joint-axis mapping */ + build_joint_mapping(kp, coordinates); + + /* Load RT module via dlopen */ + if (load_rt_module(ctx, kins_type) != 0) { + fprintf(stderr, "kinematicsUserInit: failed to load RT module for '%s'\n", + kins_type ? kins_type : "(null)"); + fprintf(stderr, " searched: %s/rtlib/%s.so\n", + EMC2_HOME, kins_type ? kins_type : "(null)"); + free(ctx); + return NULL; + } + + kp->valid = 1; + ctx->initialized = 1; + + /* Read initial HAL pin values */ + if (ctx->refresh(kp, hal_pin_reader_read_float, + hal_pin_reader_read_bit, + hal_pin_reader_read_s32) != 0) { + fprintf(stderr, "kinematicsUserInit: warning - could not read initial HAL pins\n"); + /* Don't fail - HAL pins may not exist yet */ + } + + fprintf(stderr, "kinematicsUserInit: loaded RT module for '%s' with %d joints, coords='%s'\n", + kp->module_name, kp->num_joints, kp->coordinates); + + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) { + return -1; + } + return ctx->inverse(&ctx->params, world, joints); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) { + return -1; + } + return ctx->forward(&ctx->params, joints, world); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) { + return 0; + } + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) { + return 0; + } + return ctx->params.num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) { + return KINEMATICS_IDENTITY; + } + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) { + return "unknown"; + } + return ctx->params.module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) { + return -1; + } + return ctx->refresh(&ctx->params, hal_pin_reader_read_float, + hal_pin_reader_read_bit, + hal_pin_reader_read_s32); +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + if (ctx) { + if (ctx->rt_handle) { + dlclose(ctx->rt_handle); + } + free(ctx); + } +} diff --git a/src/emc/kinematics_userspace/kinematics_user.h b/src/emc/kinematics_userspace/kinematics_user.h new file mode 100644 index 00000000000..db6556741b6 --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.h @@ -0,0 +1,174 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * Parameters like pivot_length are read from HAL pins via hal_pin_reader. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include "emcpos.h" /* EmcPose */ +#include "kinematics.h" /* KINEMATICS_TYPE, flags */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +/* Axis coordinate indices for EmcPose */ +typedef enum { + AXIS_X = 0, AXIS_Y = 1, AXIS_Z = 2, + AXIS_A = 3, AXIS_B = 4, AXIS_C = 5, + AXIS_U = 6, AXIS_V = 7, AXIS_W = 8, + AXIS_COUNT = 9 +} AxisIndex; + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters from HAL pins + * + * Call this before planning to ensure parameters like pivot_length + * are current. Reads HAL pins directly based on the kinematics type. + * + * @param ctx Kinematics context + * @return 0 on success, -1 on failure + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/src/emc/motion/atomic_9d.h b/src/emc/motion/atomic_9d.h new file mode 100644 index 00000000000..0d70f2c2f66 --- /dev/null +++ b/src/emc/motion/atomic_9d.h @@ -0,0 +1,117 @@ +/******************************************************************** +* Description: atomic_9d.h +* Atomic operations for 9D planner lock-free queue +* +* Ported from Tormach LinuxCNC implementation. +* Uses SPSC (single-producer/single-consumer) model: +* - Producer (userspace) ONLY writes to 'end' index +* - Consumer (RT) ONLY writes to 'start' index +* This eliminates race conditions without needing CAS loops. +* +* Author: Tormach (original), Port by LinuxCNC community +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +********************************************************************/ +#ifndef ATOMIC_9D_H +#define ATOMIC_9D_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Atomically increment queue index with wraparound + * + * For SPSC (single-producer/single-consumer) model: + * Each index is only written by ONE thread, so no CAS needed. + * Just load, compute, and exchange atomically. + * + * @param idx Pointer to index to increment + * @param queue_size Size of circular buffer + * @return New index value after increment + * + * MEMORY ORDERING: + * - ACQUIRE on load: ensures we see all prior writes + * - ACQ_REL on exchange: publishes new value with full barrier + */ +static inline int atomicIncrementIndex(int *idx, int queue_size) +{ + int new_idx = ((__atomic_load_n(idx, __ATOMIC_ACQUIRE)) + 1) % queue_size; + __atomic_exchange_n(idx, new_idx, __ATOMIC_ACQ_REL); + return new_idx; +} + +/** + * @brief Atomically decrement queue index with wraparound + * + * For SPSC model - same logic as increment but subtracts. + * + * @param idx Pointer to index to decrement + * @param queue_size Size of circular buffer + * @return New index value after decrement + */ +static inline int atomicDecrementIndex(int *idx, int queue_size) +{ + int new_idx = ((__atomic_load_n(idx, __ATOMIC_ACQUIRE)) - 1 + queue_size) % queue_size; + __atomic_exchange_n(idx, new_idx, __ATOMIC_ACQ_REL); + return new_idx; +} + +/** + * @brief Atomically load an integer value + * + * @param ptr Pointer to value to load + * @return Loaded value + */ +static inline int atomicLoadInt(const int *ptr) +{ + return __atomic_load_n(ptr, __ATOMIC_ACQUIRE); +} + +/** + * @brief Atomically store an integer value + * + * @param ptr Pointer to location to store + * @param value Value to store + */ +static inline void atomicStoreInt(int *ptr, int value) +{ + __atomic_store_n(ptr, value, __ATOMIC_RELEASE); +} + +/** + * @brief Atomically load a double value + * + * Note: On most platforms, double loads/stores are not atomic. + * This function provides atomic semantics using compiler built-ins. + * + * @param ptr Pointer to value to load + * @return Loaded value + */ +static inline double atomicLoadDouble(const double *ptr) +{ + // Use memcpy to avoid strict aliasing issues + double value; + __atomic_load(ptr, &value, __ATOMIC_ACQUIRE); + return value; +} + +/** + * @brief Atomically store a double value + * + * @param ptr Pointer to location to store + * @param value Value to store + */ +static inline void atomicStoreDouble(double *ptr, double value) +{ + __atomic_store(ptr, &value, __ATOMIC_RELEASE); +} + +#ifdef __cplusplus +} +#endif + +#endif // ATOMIC_9D_H diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 2d50b6db80f..e85245de5fd 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -578,6 +578,18 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) controller cycle */ rtapi_print_msg(RTAPI_MSG_DBG, "COORD"); + + /* 9D planner: Sync goalPos BEFORE setting coordinating flag. + * This ensures goalPos is valid before userspace adds any segments. + * Critical for program re-runs where we're already in COORD mode + * and the control.c tpSetPos() path won't be taken. + * (Following Tormach's tpResetAtModeChange pattern) + */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + tpSyncGoalPos_9D(&emcmotInternal->coord_tp, + &emcmotStatus->carte_pos_cmd); + } + emcmotInternal->coordinating = 1; emcmotInternal->teleoperating = 0; if (emcmotConfig->kinType != KINEMATICS_IDENTITY) { @@ -588,6 +600,14 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; } } + + /* 9D planner: Clean up any abort state after mode entry. + * This ensures spurious abort commands from userspace during + * mode transitions don't wipe fresh segments. + */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + tpCleanupAfterAbort_9D(&emcmotInternal->coord_tp); + } break; case EMCMOT_TELEOP: @@ -998,6 +1018,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) /* emcmotInternal->coord_tp up a linear move */ /* requires motion enabled, coordinated mode, not on limits */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_LINE"); + /* Check if userspace already queued this move (dual-layer architecture) */ + if (emcmotCommand->userspace_already_queued) { + rtapi_print_msg(RTAPI_MSG_DBG, "SET_LINE: userspace already queued, skipping tpAddLine"); + emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; + break; + } if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { reportError(_("need to be enabled, in coord mode for linear move")); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; @@ -1067,6 +1093,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) /* emcmotInternal->coord_tp up a circular move */ /* requires coordinated mode, enable on, not on limits */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_CIRCLE"); + /* Check if userspace already queued this move (dual-layer architecture) */ + if (emcmotCommand->userspace_already_queued) { + rtapi_print_msg(RTAPI_MSG_DBG, "SET_CIRCLE: userspace already queued, skipping tpAddCircle"); + emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; + break; + } if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { reportError(_("need to be enabled, in coord mode for circular move")); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; @@ -1191,14 +1223,21 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) break; case EMCMOT_SET_PLANNER_TYPE: - /* set the type of planner: 0 = trapezoidal, 1 = S-curve */ + /* set the type of planner: 0 = trapezoidal, 1 = S-curve, 2 = 9D (EXPERIMENTAL) */ /* can do it at any time */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_PLANNER_TYPE, type(%d)", emcmotCommand->planner_type); - // Only 0 and 1 are supported, set to 0 if invalid - if (emcmotCommand->planner_type != 0 && emcmotCommand->planner_type != 1) { - emcmotStatus->planner_type = 0; - } else { + // Support types 0, 1, and 2 (9D experimental) + if (emcmotCommand->planner_type >= 0 && emcmotCommand->planner_type <= 2) { emcmotStatus->planner_type = emcmotCommand->planner_type; + if (emcmotCommand->planner_type == 2) { + rtapi_print_msg(RTAPI_MSG_WARN, + "PLANNER_TYPE 2 (9D) is EXPERIMENTAL. Use with caution.\n"); + } + } else { + // Invalid planner type, default to 0 + emcmotStatus->planner_type = 0; + rtapi_print_msg(RTAPI_MSG_ERR, + "Invalid PLANNER_TYPE %d, defaulting to 0\n", emcmotCommand->planner_type); } break; @@ -1519,6 +1558,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) /* emcmotInternal->coord_tp up a linear move */ /* requires coordinated mode, enable off, not on limits */ rtapi_print_msg(RTAPI_MSG_DBG, "RIGID_TAP"); + /* Check if userspace already queued this move (dual-layer architecture) */ + if (emcmotCommand->userspace_already_queued) { + rtapi_print_msg(RTAPI_MSG_DBG, "RIGID_TAP: userspace already queued, skipping tpAddRigidTap"); + emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; + break; + } if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) { reportError(_("need to be enabled, in coord mode for rigid tap move")); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; @@ -1955,6 +2000,18 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotConfig->inhibit_probe_home_error = emcmotCommand->probe_home_err_inhibit; break; + case EMCMOT_SET_EMULATE_LEGACY_MOVE_COMMANDS: + /* Set dual-layer architecture mode: 1 = send NML after userspace planning */ + rtapi_print_msg(RTAPI_MSG_DBG, "SET_EMULATE_LEGACY_MOVE_COMMANDS(%d)", + emcmotCommand->emulate_legacy_move_commands); + emcmotConfig->emulate_legacy_move_commands = emcmotCommand->emulate_legacy_move_commands; + break; + + case EMCMOT_UPDATE_KINS_PARAMS: + /* No longer needed - userspace reads HAL pins directly */ + rtapi_print_msg(RTAPI_MSG_DBG, "UPDATE_KINS_PARAMS (no-op, userspace reads HAL pins)"); + break; + } /* end of: command switch */ if (emcmotStatus->commandStatus != EMCMOT_COMMAND_OK) { rtapi_print_msg(RTAPI_MSG_DBG, "ERROR: %d", diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 0f5d029d822..b02017e3ef8 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1338,81 +1338,135 @@ static void get_pos_cmds(long period) case EMCMOT_MOTION_COORD: axis_jog_abort_all(1); - /* check joint 0 to see if the interpolators are empty */ coord_cubic_active = 1; - while (cubicNeedNextPoint(&(joints[0].cubic))) { - /* they're empty, pull next point(s) off Cartesian planner */ - /* run coordinated trajectory planning cycle */ - tpRunCycle(&emcmotInternal->coord_tp, period); - /* get new commanded traj pos */ - tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); + /* CRITICAL FIX: ALWAYS progress trajectory segments every RT cycle. + * Previously, tpRunCycle was only called when cubic buffer needed points, + * creating long gaps (15-18 cycles) where segments in queue didn't progress. + * This caused premature "done" detection and motion stops at ~92% complete. + */ + tpRunCycle(&emcmotInternal->coord_tp, period); + /* get new commanded traj pos */ + tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); - if (axis_update_coord_with_bound(pcmd_p, servo_period)) { - ext_offset_coord_limit = 1; - } else { - ext_offset_coord_limit = 0; - } + if (axis_update_coord_with_bound(pcmd_p, servo_period)) { + ext_offset_coord_limit = 1; + } else { + ext_offset_coord_limit = 0; + } + + if (emcmotStatus->planner_type == 2) { + /* Planner type 2 (Ruckig): bypass cubic interpolator. */ + EmcPose current_pos = emcmotStatus->carte_pos_cmd; + + result = kinematicsInverse(¤t_pos, positions, &iflags, &fflags); + if (result == 0) { + double path_vel = emcmotStatus->current_vel; + double path_acc = emcmotStatus->current_acc; + double path_jerk = emcmotStatus->current_jerk; + PmCartesian dir = emcmotStatus->current_dir; - /* OUTPUT KINEMATICS - convert to joints in local array */ - result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, - &iflags, &fflags); - if(result == 0) - { - /* copy to joint structures and spline them up */ for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { - if(!isfinite(positions[joint_num])) - { - reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), - joint_num); - SET_MOTION_ERROR_FLAG(1); - emcmotInternal->enabling = 0; - break; + if (!isfinite(positions[joint_num])) { + reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), + joint_num); + SET_MOTION_ERROR_FLAG(1); + emcmotInternal->enabling = 0; + break; } - /* point to joint struct */ joint = &joints[joint_num]; joint->coarse_pos = positions[joint_num]; - /* spline joints up-- note that we may be adding points - that fail soft limits, but we'll abort at the end of - this cycle so it doesn't really matter */ - cubicAddPoint(&(joint->cubic), joint->coarse_pos); + joint->pos_cmd = positions[joint_num]; + + /* Decompose path vel/acc/jerk into per-axis using direction vector. + * For Cartesian machines joint 0,1,2 = X,Y,Z */ + double d = 0.0; + if (joint_num == 0) d = dir.x; + else if (joint_num == 1) d = dir.y; + else if (joint_num == 2) d = dir.z; + + joint->vel_cmd = path_vel * d; + joint->acc_cmd = path_acc * d; + joint->jerk_cmd = path_jerk * d; } + /* Still feed the cubic so it stays primed if we switch planner types */ + while (cubicNeedNextPoint(&(joints[0].cubic))) { + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + cubicAddPoint(&(joints[joint_num].cubic), positions[joint_num]); + } + } + } else { + reportError(_("kinematicsInverse failed")); + SET_MOTION_ERROR_FLAG(1); + emcmotInternal->enabling = 0; } - else - { - reportError(_("kinematicsInverse failed")); - SET_MOTION_ERROR_FLAG(1); - emcmotInternal->enabling = 0; - break; - } + } else { + /* Planner type 0 and 1: use cubic interpolator as before */ - /* END OF OUTPUT KINS */ - } // while - /* there is data in the interpolators */ - /* run interpolation */ - for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { - /* point to joint struct */ - joint = &joints[joint_num]; - /* interpolate to get new position and velocity */ + /* Fill cubic buffer if needed (may need multiple points per cycle) */ + while (cubicNeedNextPoint(&(joints[0].cubic))) { + /* Use current trajectory position (already updated above) */ + EmcPose current_pos = emcmotStatus->carte_pos_cmd; + + /* OUTPUT KINEMATICS - convert to joints in local array */ + result = kinematicsInverse(¤t_pos, positions, &iflags, &fflags); + if(result == 0) + { + /* copy to joint structures and spline them up */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + if(!isfinite(positions[joint_num])) + { + reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), + joint_num); + SET_MOTION_ERROR_FLAG(1); + emcmotInternal->enabling = 0; + break; + } + /* point to joint struct */ + joint = &joints[joint_num]; + joint->coarse_pos = positions[joint_num]; + /* spline joints up-- note that we may be adding points + that fail soft limits, but we'll abort at the end of + this cycle so it doesn't really matter */ + cubicAddPoint(&(joint->cubic), joint->coarse_pos); + } + } + else + { + reportError(_("kinematicsInverse failed")); + SET_MOTION_ERROR_FLAG(1); + emcmotInternal->enabling = 0; + break; + } + + /* If cubic STILL needs more points, get next trajectory point */ + if (cubicNeedNextPoint(&(joints[0].cubic))) { + tpRunCycle(&emcmotInternal->coord_tp, period); + tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); + } else { + break; // Buffer full, exit loop + } + + /* END OF OUTPUT KINS */ + } // while + /* there is data in the interpolators */ + /* run interpolation (consumes one point per RT cycle) */ + for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { + /* point to joint struct */ + joint = &joints[joint_num]; + /* interpolate to get new position and velocity */ joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd), &(joint->jerk_cmd)); - } + } - /* Use accurate jerk values from TP output (for Cartesian machines only) - * For standard XYZ machines, joint[0-2] correspond to X, Y, Z axes - * TP outputs: current_jerk (path jerk) and current_dir (direction unit vector) - * Per-axis jerk = path_jerk * direction_component - */ - if (emcmotStatus->planner_type == 1) { - // S-curve mode: use accurate jerk values - double path_jerk = emcmotStatus->current_jerk; - PmCartesian dir = emcmotStatus->current_dir; - - // For the first 3 joints (assuming X, Y, Z), use accurate jerk - if (NO_OF_KINS_JOINTS >= 1) joints[0].jerk_cmd = path_jerk * dir.x; - if (NO_OF_KINS_JOINTS >= 2) joints[1].jerk_cmd = path_jerk * dir.y; - if (NO_OF_KINS_JOINTS >= 3) joints[2].jerk_cmd = path_jerk * dir.z; - // Rotary axes (A, B, C) keep the cubic interpolator values for now - } + /* Use accurate jerk values from TP output (for Cartesian machines only) */ + if (emcmotStatus->planner_type == 1) { + double path_jerk = emcmotStatus->current_jerk; + PmCartesian dir = emcmotStatus->current_dir; + if (NO_OF_KINS_JOINTS >= 1) joints[0].jerk_cmd = path_jerk * dir.x; + if (NO_OF_KINS_JOINTS >= 2) joints[1].jerk_cmd = path_jerk * dir.y; + if (NO_OF_KINS_JOINTS >= 3) joints[2].jerk_cmd = path_jerk * dir.z; + } + } /* end planner type dispatch */ /* report motion status */ SET_MOTION_INPOS_FLAG(0); diff --git a/src/emc/motion/kinematics_params.h b/src/emc/motion/kinematics_params.h new file mode 100644 index 00000000000..1aab5ffdff0 --- /dev/null +++ b/src/emc/motion/kinematics_params.h @@ -0,0 +1,195 @@ +/******************************************************************** + * Description: kinematics_params.h + * Shared memory structure for kinematics parameters + * Allows userspace to read kinematics module parameters from RT + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef KINEMATICS_PARAMS_H +#define KINEMATICS_PARAMS_H + +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS */ + +/* Parameters for 5axiskins */ +typedef struct { + double pivot_length; +} kins_5axis_params_t; + +/* Parameters for xyzac-trt-kins and xyzbc-trt-kins */ +typedef struct { + double x_rot_point; + double y_rot_point; + double z_rot_point; + double x_offset; + double y_offset; + double z_offset; + double tool_offset; + int conventional_directions; +} kins_trt_params_t; + +/* Parameters for maxkins */ +typedef struct { + double pivot_length; + int conventional_directions; +} kins_max_params_t; + +/* Parameters for pumakins */ +typedef struct { + double a2; + double a3; + double d3; + double d4; + double d6; +} kins_puma_params_t; + +/* Parameters for tripodkins */ +typedef struct { + double bx; + double cx; + double cy; +} kins_tripod_params_t; + +/* Number of struts for hexapod/pentapod */ +#define KINS_GENHEX_NUM_STRUTS 6 +#define KINS_PENTA_NUM_STRUTS 5 + +/* Parameters for genhexkins */ +typedef struct { + double basex[KINS_GENHEX_NUM_STRUTS]; + double basey[KINS_GENHEX_NUM_STRUTS]; + double basez[KINS_GENHEX_NUM_STRUTS]; + double platformx[KINS_GENHEX_NUM_STRUTS]; + double platformy[KINS_GENHEX_NUM_STRUTS]; + double platformz[KINS_GENHEX_NUM_STRUTS]; + double basenx[KINS_GENHEX_NUM_STRUTS]; + double baseny[KINS_GENHEX_NUM_STRUTS]; + double basenz[KINS_GENHEX_NUM_STRUTS]; + double platformnx[KINS_GENHEX_NUM_STRUTS]; + double platformny[KINS_GENHEX_NUM_STRUTS]; + double platformnz[KINS_GENHEX_NUM_STRUTS]; + unsigned int iter_limit; + unsigned int max_iter; + double max_error; + double conv_criterion; + double tool_offset; + double spindle_offset; + double screw_lead; +} kins_genhex_params_t; + +/* Max joints for genser DH parameters */ +#define KINS_GENSER_MAX_JOINTS 9 + +/* Parameters for genserfuncs (generic serial robot) */ +typedef struct { + int link_num; /* Number of active links (up to KINS_GENSER_MAX_JOINTS) */ + unsigned int max_iterations; + unsigned int last_iterations; /* Iterations used in last inverse solve */ + double a[KINS_GENSER_MAX_JOINTS]; + double alpha[KINS_GENSER_MAX_JOINTS]; + double d[KINS_GENSER_MAX_JOINTS]; + int unrotate[KINS_GENSER_MAX_JOINTS]; +} kins_genser_params_t; + +/* Parameters for pentakins */ +typedef struct { + double basex[KINS_PENTA_NUM_STRUTS]; + double basey[KINS_PENTA_NUM_STRUTS]; + double basez[KINS_PENTA_NUM_STRUTS]; + double effectorr[KINS_PENTA_NUM_STRUTS]; + double effectorz[KINS_PENTA_NUM_STRUTS]; + unsigned int iter_limit; + double conv_criterion; +} kins_penta_params_t; + +/* Parameters for lineardeltakins */ +typedef struct { + double radius; + double jointradius; +} kins_lineardelta_params_t; + +/* Parameters for rotarydeltakins */ +typedef struct { + double platformradius; /* distance from origin to a hip joint */ + double thighlength; /* thigh connects the hip to the knee */ + double shinlength; /* shin (parallelogram) connects the knee to the foot */ + double footradius; /* distance from center of foot to an ankle joint */ +} kins_rotarydelta_params_t; + +/* Parameters for rosekins */ +typedef struct { + double revolutions; + double theta_degrees; + double bigtheta_degrees; +} kins_rose_params_t; + +/* Parameters for scarakins */ +typedef struct { + double d1; /* Vertical distance from ground to inner arm center */ + double d2; /* Length of inner arm */ + double d3; /* Vertical offset between inner and outer arm */ + double d4; /* Length of outer arm */ + double d5; /* Vertical distance from end effector to tooltip */ + double d6; /* Horizontal offset from end effector axis to tooltip */ +} kins_scara_params_t; + +/* Parameters for scorbotkins */ +typedef struct { + double l0_horizontal; /* Horizontal distance from J0 to J1 */ + double l0_vertical; /* Vertical distance from ground to J1 */ + double l1_length; /* Link 1: J1 (shoulder) to J2 (elbow) */ + double l2_length; /* Link 2: J2 (elbow) to wrist */ +} kins_scorbot_params_t; + +/* + * Main kinematics parameters structure for shared memory + * + * Thread safety: Uses split-read pattern (head/tail) like emcmot_status_t + * RT increments head before update, sets tail=head after + * Userspace copies struct, checks head==tail, retries if mismatch + */ +typedef struct kinematics_params_t { + /* Split-read consistency markers */ + unsigned char head; + unsigned char tail; + + /* Kinematics identification */ + char module_name[32]; /* e.g., "5axiskins", "xyzac-trt-kins" */ + char coordinates[16]; /* e.g., "XYZBCW", "XYZAC" */ + + /* Joint configuration */ + int num_joints; + int joint_to_axis[EMCMOT_MAX_JOINTS]; /* joint -> axis index (-1 = unmapped) */ + int axis_to_joint[9]; /* axis (XYZABCUVW) -> principal joint (-1 = unmapped) */ + + /* Kinematics-specific parameters (union) */ + union { + kins_5axis_params_t fiveaxis; + kins_trt_params_t trt; + kins_max_params_t maxkins; + kins_puma_params_t puma; + kins_tripod_params_t tripod; + kins_genhex_params_t genhex; + kins_genser_params_t genser; + kins_penta_params_t penta; + kins_lineardelta_params_t lineardelta; + kins_rotarydelta_params_t rotarydelta; + kins_rose_params_t rose; + kins_scara_params_t scara; + kins_scorbot_params_t scorbot; + double raw[128]; /* Reserve space for largest struct (genhex ~= 78 doubles) */ + } params; + + /* Validity flag - set to 1 when params are initialized */ + int valid; + + /* Sequence number - incremented on any parameter change */ + unsigned int seq_num; + +} kinematics_params_t; + +#endif /* KINEMATICS_PARAMS_H */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index d27b42d4f60..8d4d92dc08c 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -245,6 +245,8 @@ static int module_intfc() { tpMotData(emcmotStatus ,emcmotConfig + ,emcmotStruct + ,emcmotInternal ); return 0; } @@ -882,6 +884,7 @@ static int init_comm_buffers(void) ZERO_EMC_POSE(emcmotStatus->carte_pos_fb); emcmotStatus->vel = 0.0; emcmotConfig->limitVel = 0.0; + emcmotConfig->emulate_legacy_move_commands = 1; /* Default: send NML for backward compat */ emcmotStatus->acc = 0.0; emcmotStatus->feed_scale = 1.0; emcmotStatus->rapid_scale = 1.0; @@ -899,6 +902,20 @@ static int init_comm_buffers(void) SET_MOTION_ENABLE_FLAG(0); /* record the kinematics type of the machine */ emcmotConfig->kinType = kinematicsType(); + + /* record the kinematics module name for userspace access */ + { + const char *kins_name = kinematicsGetName(); + if (kins_name) { + rtapi_strxcpy(emcmotConfig->kins_module_name, kins_name); + } else { + emcmotConfig->kins_module_name[0] = '\0'; + } + rtapi_print_msg(RTAPI_MSG_INFO, + "MOTION: kinematics module='%s'\n", + emcmotConfig->kins_module_name); + } + emcmot_config_change(); for (spindle_num = 0; spindle_num < EMCMOT_MAX_SPINDLES; spindle_num++){ diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 3e28a403875..68f5e86210f 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -123,7 +123,7 @@ extern "C" { EMCMOT_SET_VEL_LIMIT, /* set the max vel for all moves (tooltip) */ EMCMOT_SET_ACC, /* set the max accel for moves (tooltip) */ EMCMOT_SET_JERK, /* set the max jerk for moves (tooltip) */ - EMCMOT_SET_PLANNER_TYPE, /* set planner type (0=trapezoidal, 1=S-curve) */ + EMCMOT_SET_PLANNER_TYPE, /* set planner type (0=trapezoidal, 1=S-curve, 2=9D EXPERIMENTAL) */ EMCMOT_SET_TERM_COND, /* set termination condition (stop, blend) */ EMCMOT_SET_NUM_JOINTS, /* set the number of joints */ EMCMOT_SET_NUM_SPINDLES, /* set the number of spindles */ @@ -145,6 +145,7 @@ extern "C" { EMCMOT_SETUP_ARC_BLENDS, EMCMOT_SET_PROBE_ERR_INHIBIT, + EMCMOT_SET_EMULATE_LEGACY_MOVE_COMMANDS, /* set dual-layer architecture mode */ EMCMOT_ENABLE_WATCHDOG, /* enable watchdog sound, parport */ EMCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */ EMCMOT_JOG_CONT, /* continuous jog */ @@ -176,6 +177,8 @@ extern "C" { EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ + EMCMOT_UPDATE_KINS_PARAMS, /* Request RT to re-export kins params to shared memory */ + } cmd_code_t; /* this enum lists the possible results of a command */ @@ -220,7 +223,17 @@ extern "C" { double acc; /* max acceleration */ double jerk; /* jerk for traj */ double ini_maxjerk; - int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + /* planner_type: Trajectory planning algorithm + * 0 = Trapezoidal velocity profile (default, most tested) + * 1 = S-curve (jerk-limited) velocity profile + * 2 = Tormach 9D dual-layer lookahead planner (EXPERIMENTAL) + * - Userspace planning with RT execution, lock-free queue + * - Backward velocity pass optimization ("rising tide") + * - Peak smoothing for reduced jerk + * - Requires [TRAJ] OPTIMIZATION_DEPTH, RAMP_FREQUENCY, etc. in INI + * - See src/emc/motion_planning/motion_planning_9d.hh for details + */ + int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve, 2 = 9D (EXPERIMENTAL) */ double backlash; /* amount of backlash */ int id; /* id for motion */ int termCond; /* termination condition */ @@ -251,6 +264,7 @@ extern "C" { |2 = move until probe clears */ int probe_jog_err_inhibit; // setting to inhibit probe tripped while jogging error. int probe_home_err_inhibit; // setting to inhibit probe tripped while homeing error. + int emulate_legacy_move_commands; // 1 = send NML after userspace planning (dual-layer mode) EmcPose tool_offset; /* TLO */ double orientation; /* angle for spindle orient */ int state; /*spindle state seems to just be 0 for off and 1 for on andypugh 2025-04-03*/ @@ -267,6 +281,7 @@ extern "C" { double ext_offset_vel; /* velocity for an external axis offset */ double ext_offset_acc; /* acceleration for an external axis offset */ struct state_tag_t tag; + int userspace_already_queued; /* 1 if userspace called tpAddLine/Circle/RigidTap directly */ } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -640,7 +655,7 @@ Suggestion: Split this in to an Error and a Status flag register.. double vel; /* scalar max vel */ double acc; /* scalar max accel */ double jerk; /* jerk for traj */ - int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve, 2 = 9D (EXPERIMENTAL) */ int motionType; double distance_to_go; /* in this move */ @@ -703,6 +718,9 @@ Suggestion: Split this in to an Error and a Status flag register.. KINEMATICS_TYPE kinType; + /* Kinematics module identification for userspace access */ + char kins_module_name[32]; /* e.g., "trivkins", "5axiskins" */ + int numDIO; /* userdefined number of digital IO. default is 4. (EMCMOT_MAX_DIO=64), but can be altered at motmod insmod time */ @@ -734,6 +752,7 @@ Suggestion: Split this in to an Error and a Status flag register.. double maxFeedScale; int inhibit_probe_jog_error; int inhibit_probe_home_error; + int emulate_legacy_move_commands; /* 1 = send NML after userspace planning (backward compat) */ } emcmot_config_t; /* error structure - A ring buffer used to pass formatted printf strings to usr space */ diff --git a/src/emc/motion/usrmotintf.cc b/src/emc/motion/usrmotintf.cc index 80ef69a14a2..e6564e18a6b 100644 --- a/src/emc/motion/usrmotintf.cc +++ b/src/emc/motion/usrmotintf.cc @@ -18,6 +18,7 @@ #include #include /* memcpy() */ #include /* DBL_MIN */ +#include /* offsetof() */ #include "motion.h" /* emcmot_status_t,CMD */ #include "motion_struct.h" /* emcmot_struct_t */ #include "emcmotcfg.h" /* EMCMOT_ERROR_NUM,LEN */ @@ -25,6 +26,7 @@ #include "usrmotintf.h" /* these decls */ #include "_timer.h" #include "rcs_print.hh" +#include "tp.h" /* tpIsDone() */ #include "inifile.hh" @@ -38,12 +40,13 @@ static int inited = 0; /* flag if inited */ -static emcmot_command_t *emcmotCommand = 0; -static emcmot_status_t *emcmotStatus = 0; -static emcmot_config_t *emcmotConfig = 0; -static emcmot_internal_t *emcmotInternal = 0; -static emcmot_error_t *emcmotError = 0; -static emcmot_struct_t *emcmotStruct = 0; +/* Export these for libmotion_planning_9d.so (removed static) */ +emcmot_command_t *emcmotCommand = 0; +emcmot_status_t *emcmotStatus = 0; +emcmot_config_t *emcmotConfig = 0; +emcmot_internal_t *emcmotInternal = 0; +emcmot_error_t *emcmotError = 0; +emcmot_struct_t *emcmotStruct = 0; /* usrmotIniLoad() loads params (SHMEM_KEY, COMM_TIMEOUT) from named INI file */ @@ -265,7 +268,7 @@ void printTPstruct(TP_STRUCT * tp) printf("goalPos :"); printEmcPose(&tp->goalPos); printf("\n"); - printf("done=%d\n", tp->done); + printf("done=%d (computed via tpIsDone())\n", tpIsDone(tp)); printf("depth=%d\n", tp->depth); printf("activeDepth=%d\n", tp->activeDepth); printf("aborting=%d\n", tp->aborting); @@ -620,6 +623,38 @@ int usrmotExit(void) return 0; } +/* Returns direct pointer to internal structure for 9D planner optimization */ +emcmot_internal_t *usrmotGetEmcmotInternal(void) +{ + return emcmotInternal; +} + +/* Returns direct pointer to status structure */ +emcmot_status_t *usrmotGetEmcmotStatus(void) +{ + return emcmotStatus; +} + +/* Returns direct pointer to TP structure for dual-layer architecture. + This allows userspace to call tpAddLine(), tpAddCircle(), etc. directly + on the shared memory TP structure, bypassing NML messaging. + WARNING: This is the actual TP struct shared with the RT process. + Use carefully to avoid corruption and data races. */ +TP_STRUCT *usrmotGetTPDataPtr(void) +{ + if (!emcmotInternal) { + return NULL; + } + TP_STRUCT *tp = &emcmotInternal->coord_tp; + return tp; +} + +/* Returns direct pointer to config structure for TP global initialization */ +emcmot_config_t *usrmotGetEmcmotConfig(void) +{ + return emcmotConfig; +} + /* Loads pairs of comp from the compensation file. The default way is to specify nominal, forward & reverse triplets in the file However if type != 0, it expects nominal, forward_trim & reverse_trim diff --git a/src/emc/motion/usrmotintf.h b/src/emc/motion/usrmotintf.h index a09d4ea8d7c..59465f42eaf 100644 --- a/src/emc/motion/usrmotintf.h +++ b/src/emc/motion/usrmotintf.h @@ -20,6 +20,7 @@ struct emcmot_command_t; struct emcmot_config_t; struct emcmot_internal_t; struct emcmot_error_t; +struct TP_STRUCT; #ifdef __cplusplus extern "C" { @@ -41,6 +42,28 @@ extern "C" { the emcmot controller and puts it in arg */ extern int usrmotReadEmcmotInternal(emcmot_internal_t * s); +/* usrmotGetEmcmotInternal() returns a direct pointer to the internal + structure in shared memory (for 9D planner optimization). + Returns NULL if shared memory is not initialized. */ + extern emcmot_internal_t *usrmotGetEmcmotInternal(void); + +/* usrmotGetEmcmotStatus() returns a direct pointer to the status + structure in shared memory. + Returns NULL if shared memory is not initialized. */ + extern emcmot_status_t *usrmotGetEmcmotStatus(void); + +/* usrmotGetTPDataPtr() returns a direct pointer to the trajectory + planner structure (coord_tp) in shared memory. This enables + dual-layer architecture where userspace calls TP functions directly. + Returns NULL if shared memory is not initialized. + WARNING: This is the actual TP struct shared with RT - use carefully! */ + extern TP_STRUCT *usrmotGetTPDataPtr(void); + +/* usrmotGetEmcmotConfig() returns a direct pointer to the config + structure in shared memory (for TP global initialization). + Returns NULL if shared memory is not initialized. */ + extern emcmot_config_t *usrmotGetEmcmotConfig(void); + /* usrmotReadEmcmotError() gets the earliest queued error string out of the emcmot controller and puts it in arg */ extern int usrmotReadEmcmotError(char *e); diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..183daba4745 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,87 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace +INCLUDES += emc/tp/ruckig/include + +LIBMOTION_PLANNING_9D_SRCS := $(addprefix emc/motion_planning/, \ + motion_planning_9d.cc \ + motion_planning_9d_userspace.cc \ + motion_planning_9d_stubs.c \ + path_sampler.cc \ + jacobian.cc \ + joint_limits.cc \ + userspace_kinematics.cc \ + bezier9.cc \ + blend_sizing.cc \ + ) + +# Userspace kinematics sources (with HAL pin reader for runtime parameters) +LIBMOTION_PLANNING_9D_KINS_SRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + hal_pin_reader.c \ + ) + +# Add TP and NML sources needed by userspace planning (dual-layer architecture) +LIBMOTION_PLANNING_9D_TP_SRCS := $(addprefix emc/tp/, \ + tp.c \ + tc.c \ + tcq.c \ + blendmath.c \ + spherical_arc.c \ + sp_scurve.c \ + ruckig_wrapper.cc \ + ) + +LIBMOTION_PLANNING_9D_NML_SRCS := $(addprefix emc/nml_intf/, \ + emcpose.c \ + ) + +# Ruckig trajectory generation library (statically linked) +LIBRUCKIG_SRCS := $(addprefix emc/tp/ruckig/src/ruckig/, \ + brake.cpp \ + position_first_step1.cpp \ + position_first_step2.cpp \ + position_second_step1.cpp \ + position_second_step2.cpp \ + position_third_step1.cpp \ + position_third_step2.cpp \ + velocity_second_step1.cpp \ + velocity_second_step2.cpp \ + velocity_third_step1.cpp \ + velocity_third_step2.cpp \ + ) + +USERSRCS += $(LIBRUCKIG_SRCS) +USERSRCS += $(LIBMOTION_PLANNING_9D_SRCS) +USERSRCS += $(LIBMOTION_PLANNING_9D_TP_SRCS) +USERSRCS += $(LIBMOTION_PLANNING_9D_NML_SRCS) +USERSRCS += $(LIBMOTION_PLANNING_9D_KINS_SRCS) + +$(call TOOBJSDEPS, $(filter %.cc,$(LIBMOTION_PLANNING_9D_SRCS))) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD -std=c++20 +$(call TOOBJSDEPS, $(filter %.c,$(LIBMOTION_PLANNING_9D_SRCS))) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD +$(call TOOBJSDEPS, $(LIBRUCKIG_SRCS)) : EXTRAFLAGS=-fPIC -std=c++20 +$(call TOOBJSDEPS, $(filter %.c,$(LIBMOTION_PLANNING_9D_TP_SRCS))) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD +$(call TOOBJSDEPS, $(filter %.cc,$(LIBMOTION_PLANNING_9D_TP_SRCS))) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD -std=c++20 +$(call TOOBJSDEPS, $(LIBMOTION_PLANNING_9D_NML_SRCS)) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD +$(call TOOBJSDEPS, $(LIBMOTION_PLANNING_9D_KINS_SRCS)) : EXTRAFLAGS=-fPIC -DUSERSPACE_LIB_BUILD -D_GNU_SOURCE + +../lib/libmotion_planning_9d.so.0: $(patsubst %.cc,objects/%.o,$(filter %.cc,$(LIBMOTION_PLANNING_9D_SRCS))) \ + $(patsubst %.c,objects/%.o,$(filter %.c,$(LIBMOTION_PLANNING_9D_SRCS))) \ + $(patsubst %.c,objects/%.o,$(filter %.c,$(LIBMOTION_PLANNING_9D_TP_SRCS))) \ + $(patsubst %.cc,objects/%.o,$(filter %.cc,$(LIBMOTION_PLANNING_9D_TP_SRCS))) \ + $(patsubst %.c,objects/%.o,$(LIBMOTION_PLANNING_9D_NML_SRCS)) \ + $(patsubst %.c,objects/%.o,$(LIBMOTION_PLANNING_9D_KINS_SRCS)) \ + $(patsubst %.cpp,objects/%.o,$(LIBRUCKIG_SRCS)) \ + ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) -g $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ -lstdc++ -ldl + +../lib/libmotion_planning_9d.so: ../lib/libmotion_planning_9d.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libmotion_planning_9d.so ../lib/libmotion_planning_9d.so.0 + +$(patsubst ./emc/motion_planning/%,../include/%,$(wildcard ./emc/motion_planning/*.h)): ../include/%.h: ./emc/motion_planning/%.h + cp $^ $@ +$(patsubst ./emc/motion_planning/%,../include/%,$(wildcard ./emc/motion_planning/*.hh)): ../include/%.hh: ./emc/motion_planning/%.hh + cp $^ $@ diff --git a/src/emc/motion_planning/bezier9.cc b/src/emc/motion_planning/bezier9.cc new file mode 100644 index 00000000000..e4e617411cc --- /dev/null +++ b/src/emc/motion_planning/bezier9.cc @@ -0,0 +1,616 @@ +#include "bezier9.h" +#include "tp_types.h" +#include "tp_debug.h" +#include "posemath.h" +#include "blendmath.h" +#include "rtapi_math.h" +#include + +// Gauss-Legendre quadrature nodes and weights for 16-point integration +// Nodes are symmetric around 0, weights sum to 2 +static const double GL16_NODES[16] = { + -0.989400934991649932596, -0.944575023073232576078, + -0.865631202387831743880, -0.755404408355003033895, + -0.617876244402643748447, -0.458016777657227386342, + -0.281603550779258913230, -0.095012509837637440185, + 0.095012509837637440185, 0.281603550779258913230, + 0.458016777657227386342, 0.617876244402643748447, + 0.755404408355003033895, 0.865631202387831743880, + 0.944575023073232576078, 0.989400934991649932596 +}; + +static const double GL16_WEIGHTS[16] = { + 0.027152459411754094852, 0.062253523938647892863, + 0.095158511682492784810, 0.124628971255533872052, + 0.149595988816576732081, 0.169156519395002538189, + 0.182603415044923588867, 0.189450610455068496285, + 0.189450610455068496285, 0.182603415044923588867, + 0.169156519395002538189, 0.149595988816576732081, + 0.124628971255533872052, 0.095158511682492784810, + 0.062253523938647892863, 0.027152459411754094852 +}; + +/** + * bezier5_eval - Evaluate quintic Bezier curve at parameter t + * + * B(t) = (1-t)^5 P0 + 5(1-t)^4 t P1 + 10(1-t)^3 t^2 P2 + * + 10(1-t)^2 t^3 P3 + 5(1-t) t^4 P4 + t^5 P5 + */ +static void bezier5_eval(PmCartesian const * const P, + double t, + PmCartesian * const out) +{ + double s = 1.0 - t; + double s2 = s * s; + double s3 = s2 * s; + double s4 = s3 * s; + double s5 = s4 * s; + double t2 = t * t; + double t3 = t2 * t; + double t4 = t3 * t; + double t5 = t4 * t; + + double b0 = s5; + double b1 = 5.0 * s4 * t; + double b2 = 10.0 * s3 * t2; + double b3 = 10.0 * s2 * t3; + double b4 = 5.0 * s * t4; + double b5 = t5; + + out->x = b0*P[0].x + b1*P[1].x + b2*P[2].x + b3*P[3].x + b4*P[4].x + b5*P[5].x; + out->y = b0*P[0].y + b1*P[1].y + b2*P[2].y + b3*P[3].y + b4*P[4].y + b5*P[5].y; + out->z = b0*P[0].z + b1*P[1].z + b2*P[2].z + b3*P[3].z + b4*P[4].z + b5*P[5].z; +} + +/** + * bezier5_deriv - Evaluate first derivative of quintic Bezier at t + * + * B'(t) = 5 * sum_{i=0}^{4} C(4,i) (1-t)^(4-i) t^i (P_{i+1} - P_i) + * + * This is a degree-4 Bezier on the forward-difference control points. + */ +static void bezier5_deriv(PmCartesian const * const P, + double t, + PmCartesian * const out) +{ + double s = 1.0 - t; + double s2 = s * s; + double s3 = s2 * s; + double s4 = s3 * s; + double t2 = t * t; + double t3 = t2 * t; + double t4 = t3 * t; + + // Coefficients: 5 * C(4,i) * s^(4-i) * t^i + double c0 = 5.0 * s4; + double c1 = 5.0 * 4.0 * s3 * t; + double c2 = 5.0 * 6.0 * s2 * t2; + double c3 = 5.0 * 4.0 * s * t3; + double c4 = 5.0 * t4; + + // Forward differences D_i = P_{i+1} - P_i + PmCartesian D[5]; + for (int i = 0; i < 5; i++) { + D[i].x = P[i+1].x - P[i].x; + D[i].y = P[i+1].y - P[i].y; + D[i].z = P[i+1].z - P[i].z; + } + + out->x = c0*D[0].x + c1*D[1].x + c2*D[2].x + c3*D[3].x + c4*D[4].x; + out->y = c0*D[0].y + c1*D[1].y + c2*D[2].y + c3*D[3].y + c4*D[4].y; + out->z = c0*D[0].z + c1*D[1].z + c2*D[2].z + c3*D[3].z + c4*D[4].z; +} + +/** + * bezier5_deriv2 - Evaluate second derivative of quintic Bezier at t + * + * B''(t) = 20 * sum_{i=0}^{3} C(3,i) (1-t)^(3-i) t^i E_i + * + * where E_i = P_{i+2} - 2 P_{i+1} + P_i (second forward differences). + * This is a degree-3 Bezier on E_i. + */ +static void bezier5_deriv2(PmCartesian const * const P, + double t, + PmCartesian * const out) +{ + double s = 1.0 - t; + double s2 = s * s; + double s3 = s2 * s; + double t2 = t * t; + double t3 = t2 * t; + + // Coefficients: 20 * C(3,i) * s^(3-i) * t^i + double c0 = 20.0 * s3; + double c1 = 20.0 * 3.0 * s2 * t; + double c2 = 20.0 * 3.0 * s * t2; + double c3 = 20.0 * t3; + + // Second forward differences E_i = P_{i+2} - 2*P_{i+1} + P_i + PmCartesian E[4]; + for (int i = 0; i < 4; i++) { + E[i].x = P[i+2].x - 2.0*P[i+1].x + P[i].x; + E[i].y = P[i+2].y - 2.0*P[i+1].y + P[i].y; + E[i].z = P[i+2].z - 2.0*P[i+1].z + P[i].z; + } + + out->x = c0*E[0].x + c1*E[1].x + c2*E[2].x + c3*E[3].x; + out->y = c0*E[0].y + c1*E[1].y + c2*E[2].y + c3*E[3].y; + out->z = c0*E[0].z + c1*E[1].z + c2*E[2].z + c3*E[3].z; +} + +/** + * bezier9_deriv_mag - Compute 9D magnitude of derivative + * + * |B'(t)| = sqrt(|B'_xyz|² + |B'_abc|² + |B'_uvw|²) + */ +static double bezier9_deriv_mag(Bezier9 const * const b, double t) +{ + PmCartesian dP, dA, dU; + bezier5_deriv(b->P, t, &dP); + bezier5_deriv(b->A, t, &dA); + bezier5_deriv(b->U, t, &dU); + + double mag_xyz2 = dP.x * dP.x + dP.y * dP.y + dP.z * dP.z; + double mag_abc2 = dA.x * dA.x + dA.y * dA.y + dA.z * dA.z; + double mag_uvw2 = dU.x * dU.x + dU.y * dU.y + dU.z * dU.z; + + return sqrt(mag_xyz2 + mag_abc2 + mag_uvw2); +} + +/** + * gauss_legendre_integrate - Integrate function over [t_start, t_end] + * + * Transforms 16-point Gauss-Legendre rule from [-1, 1] to [t_start, t_end]: + * ∫ f(t) dt ≈ half_range * Σᵢ wᵢ f(mid + half_range * xᵢ) + */ +static double gauss_legendre_integrate(Bezier9 const * const b, + double t_start, + double t_end) +{ + double mid = 0.5 * (t_end + t_start); + double half_range = 0.5 * (t_end - t_start); + double sum = 0.0; + + for (int i = 0; i < 16; i++) { + double t = mid + half_range * GL16_NODES[i]; + sum += GL16_WEIGHTS[i] * bezier9_deriv_mag(b, t); + } + + return half_range * sum; +} + +/** + * build_arc_length_table - Precompute arc-length parameterization + * + * Builds lookup tables mapping Bezier parameter t to arc length s. + * Uses Gauss-Legendre quadrature to compute cumulative arc length. + */ +static void build_arc_length_table(Bezier9 * const b) +{ + b->t_table[0] = 0.0; + b->s_table[0] = 0.0; + + for (int i = 1; i <= BEZIER9_ARC_LENGTH_SAMPLES; i++) { + double t_prev = (double)(i - 1) / BEZIER9_ARC_LENGTH_SAMPLES; + double t_curr = (double)i / BEZIER9_ARC_LENGTH_SAMPLES; + + double ds = gauss_legendre_integrate(b, t_prev, t_curr); + + b->t_table[i] = t_curr; + b->s_table[i] = b->s_table[i - 1] + ds; + } + + b->total_length = b->s_table[BEZIER9_ARC_LENGTH_SAMPLES]; +} + +/** + * arc_length_to_t - Map arc length s to Bezier parameter t + * + * Uses binary search on precomputed lookup table, then linear interpolation. + */ +static double arc_length_to_t(Bezier9 const * const b, double s) +{ + // Clamp to valid range + if (s <= 0.0) return 0.0; + if (s >= b->total_length) return 1.0; + + // Binary search for interval containing s + int lo = 0; + int hi = BEZIER9_ARC_LENGTH_SAMPLES; + + while (hi - lo > 1) { + int mid = (lo + hi) / 2; + if (b->s_table[mid] < s) { + lo = mid; + } else { + hi = mid; + } + } + + // Linear interpolation within interval + double s0 = b->s_table[lo]; + double s1 = b->s_table[hi]; + double t0 = b->t_table[lo]; + double t1 = b->t_table[hi]; + + double ds = s1 - s0; + if (ds < BEZIER9_MIN_LENGTH) { + return t0; + } + + double alpha = (s - s0) / ds; + return t0 + alpha * (t1 - t0); +} + +/** + * Set 6 control points for one 3D subspace with G2 continuity. + * + * Q[0] = start + * Q[1] = start + alpha * u_start + * Q[2] = start + 2*alpha * u_start (collinear → B''(0) = 0) + * Q[3] = end - 2*alpha * u_end (collinear → B''(1) = 0) + * Q[4] = end - alpha * u_end + * Q[5] = end + */ +static void set_quintic_control_points(PmCartesian * Q, + PmCartesian const * start, + PmCartesian const * end, + PmCartesian const * u_start, + PmCartesian const * u_end, + double alpha) +{ + double a2 = 2.0 * alpha; + + Q[0] = *start; + + Q[1].x = start->x + alpha * u_start->x; + Q[1].y = start->y + alpha * u_start->y; + Q[1].z = start->z + alpha * u_start->z; + + Q[2].x = start->x + a2 * u_start->x; + Q[2].y = start->y + a2 * u_start->y; + Q[2].z = start->z + a2 * u_start->z; + + Q[3].x = end->x - a2 * u_end->x; + Q[3].y = end->y - a2 * u_end->y; + Q[3].z = end->z - a2 * u_end->z; + + Q[4].x = end->x - alpha * u_end->x; + Q[4].y = end->y - alpha * u_end->y; + Q[4].z = end->z - alpha * u_end->z; + + Q[5] = *end; +} + +int bezier9Init(Bezier9 * const b, + EmcPose const * const start, + EmcPose const * const end, + PmCartesian const * const u_start_xyz, + PmCartesian const * const u_end_xyz, + PmCartesian const * const u_start_abc, + PmCartesian const * const u_end_abc, + PmCartesian const * const u_start_uvw, + PmCartesian const * const u_end_uvw, + double alpha) +{ + // Validate inputs + if (!b || !start || !end) { + return TP_ERR_MISSING_INPUT; + } + if (!u_start_xyz || !u_end_xyz || !u_start_abc || !u_end_abc || + !u_start_uvw || !u_end_uvw) { + return TP_ERR_MISSING_INPUT; + } + if (alpha <= 0.0) { + tp_debug_print("bezier9Init: invalid alpha %g, must be > 0\n", alpha); + return TP_ERR_FAIL; + } + + // Extract 3D start/end for each subspace + PmCartesian start_xyz = {start->tran.x, start->tran.y, start->tran.z}; + PmCartesian end_xyz = {end->tran.x, end->tran.y, end->tran.z}; + PmCartesian start_abc = {start->a, start->b, start->c}; + PmCartesian end_abc = {end->a, end->b, end->c}; + PmCartesian start_uvw = {start->u, start->v, start->w}; + PmCartesian end_uvw = {end->u, end->v, end->w}; + + // Set quintic control points for each subspace (G2 continuity) + set_quintic_control_points(b->P, &start_xyz, &end_xyz, + u_start_xyz, u_end_xyz, alpha); + set_quintic_control_points(b->A, &start_abc, &end_abc, + u_start_abc, u_end_abc, alpha); + set_quintic_control_points(b->U, &start_uvw, &end_uvw, + u_start_uvw, u_end_uvw, alpha); + + // Build arc-length parameterization table + build_arc_length_table(b); + + // Check for degenerate curve + if (b->total_length < BEZIER9_MIN_LENGTH) { + tp_debug_print("bezier9Init: curve length %g below minimum %g\n", + b->total_length, BEZIER9_MIN_LENGTH); + return TP_ERR_ZERO_LENGTH; + } + + // Compute maximum curvature + int ret = bezier9MaxCurvature(b); + if (ret != TP_ERR_OK) { + return ret; + } + + tp_debug_print("bezier9Init: length=%g, max_kappa=%g, min_radius=%g, max_dkappa_ds=%g\n", + b->total_length, b->max_kappa, b->min_radius, b->max_dkappa_ds); + + return TP_ERR_OK; +} + +int bezier9Point(Bezier9 const * const b, + double progress, + EmcPose * const out) +{ + if (!b || !out) { + return TP_ERR_MISSING_INPUT; + } + + // Map arc length to Bezier parameter + double t = arc_length_to_t(b, progress); + + // Evaluate position in all three subspaces + PmCartesian xyz, abc, uvw; + bezier5_eval(b->P, t, &xyz); + bezier5_eval(b->A, t, &abc); + bezier5_eval(b->U, t, &uvw); + + // Pack into EmcPose + out->tran.x = xyz.x; + out->tran.y = xyz.y; + out->tran.z = xyz.z; + out->a = abc.x; + out->b = abc.y; + out->c = abc.z; + out->u = uvw.x; + out->v = uvw.y; + out->w = uvw.z; + + return TP_ERR_OK; +} + +int bezier9Tangent(Bezier9 const * const b, + double progress, + PmCartesian * const xyz_out, + PmCartesian * const abc_out, + PmCartesian * const uvw_out) +{ + if (!b) { + return TP_ERR_MISSING_INPUT; + } + + // Map arc length to Bezier parameter + double t = arc_length_to_t(b, progress); + + // Evaluate derivatives in all three subspaces + PmCartesian dP, dA, dU; + bezier5_deriv(b->P, t, &dP); + bezier5_deriv(b->A, t, &dA); + bezier5_deriv(b->U, t, &dU); + + // Normalize to unit tangent vectors + if (xyz_out) { + double mag = sqrt(dP.x * dP.x + dP.y * dP.y + dP.z * dP.z); + if (mag > BEZIER9_POS_EPSILON) { + xyz_out->x = dP.x / mag; + xyz_out->y = dP.y / mag; + xyz_out->z = dP.z / mag; + } else { + xyz_out->x = 0.0; + xyz_out->y = 0.0; + xyz_out->z = 0.0; + } + } + + if (abc_out) { + double mag = sqrt(dA.x * dA.x + dA.y * dA.y + dA.z * dA.z); + if (mag > BEZIER9_POS_EPSILON) { + abc_out->x = dA.x / mag; + abc_out->y = dA.y / mag; + abc_out->z = dA.z / mag; + } else { + abc_out->x = 0.0; + abc_out->y = 0.0; + abc_out->z = 0.0; + } + } + + if (uvw_out) { + double mag = sqrt(dU.x * dU.x + dU.y * dU.y + dU.z * dU.z); + if (mag > BEZIER9_POS_EPSILON) { + uvw_out->x = dU.x / mag; + uvw_out->y = dU.y / mag; + uvw_out->z = dU.z / mag; + } else { + uvw_out->x = 0.0; + uvw_out->y = 0.0; + uvw_out->z = 0.0; + } + } + + return TP_ERR_OK; +} + +double bezier9Length(Bezier9 const * const b) +{ + if (!b) { + return 0.0; + } + return b->total_length; +} + +double bezier9Curvature(Bezier9 const * const b, double t) +{ + if (!b) { + return 0.0; + } + + // Evaluate first and second derivatives (xyz only) + PmCartesian dP, ddP; + bezier5_deriv(b->P, t, &dP); + bezier5_deriv2(b->P, t, &ddP); + + // |B'(t)|² + double dP_mag2 = dP.x * dP.x + dP.y * dP.y + dP.z * dP.z; + if (dP_mag2 < BEZIER9_CURVATURE_EPSILON) { + return 0.0; + } + + // B' · B'' + double dot = dP.x * ddP.x + dP.y * ddP.y + dP.z * ddP.z; + + // B'' - (B'·B''/|B'|²)B' + double scale = dot / dP_mag2; + PmCartesian perp; + perp.x = ddP.x - scale * dP.x; + perp.y = ddP.y - scale * dP.y; + perp.z = ddP.z - scale * dP.z; + + // |perp| + double perp_mag = sqrt(perp.x * perp.x + perp.y * perp.y + perp.z * perp.z); + + // κ = |perp| / |B'|² + return perp_mag / dP_mag2; +} + +int bezier9MaxCurvature(Bezier9 * const b) +{ + if (!b) { + return TP_ERR_MISSING_INPUT; + } + + // Sample curvature at N evenly-spaced points + double max_kappa = 0.0; + int max_idx = 0; + + for (int i = 0; i <= BEZIER9_CURVATURE_SAMPLES; i++) { + double t = (double)i / BEZIER9_CURVATURE_SAMPLES; + double kappa = bezier9Curvature(b, t); + + if (kappa > max_kappa) { + max_kappa = kappa; + max_idx = i; + } + } + + // Refine using golden section search in neighborhood of maximum + double t_lo, t_hi; + if (max_idx == 0) { + t_lo = 0.0; + t_hi = 2.0 / BEZIER9_CURVATURE_SAMPLES; + } else if (max_idx == BEZIER9_CURVATURE_SAMPLES) { + t_lo = 1.0 - 2.0 / BEZIER9_CURVATURE_SAMPLES; + t_hi = 1.0; + } else { + t_lo = (double)(max_idx - 1) / BEZIER9_CURVATURE_SAMPLES; + t_hi = (double)(max_idx + 1) / BEZIER9_CURVATURE_SAMPLES; + } + + // Golden section search for 10 iterations + double phi = BEZIER9_GOLDEN_RATIO; + double t1 = t_lo + (1.0 - phi) * (t_hi - t_lo); + double t2 = t_lo + phi * (t_hi - t_lo); + double k1 = bezier9Curvature(b, t1); + double k2 = bezier9Curvature(b, t2); + + for (int iter = 0; iter < 10; iter++) { + if (k1 > k2) { + t_hi = t2; + t2 = t1; + k2 = k1; + t1 = t_lo + (1.0 - phi) * (t_hi - t_lo); + k1 = bezier9Curvature(b, t1); + } else { + t_lo = t1; + t1 = t2; + k1 = k2; + t2 = t_lo + phi * (t_hi - t_lo); + k2 = bezier9Curvature(b, t2); + } + } + + max_kappa = (k1 > k2) ? k1 : k2; + + // Cache results + b->max_kappa = max_kappa; + if (max_kappa > BEZIER9_CURVATURE_EPSILON) { + b->min_radius = 1.0 / max_kappa; + } else { + b->min_radius = 1.0e12; // Effectively infinite radius + } + + // Compute max |dκ/ds| using arc-length table nodes. + // The curvature rate determines the centripetal jerk = v³ · dκ/ds, + // which must be bounded for smooth motion at blend entry/exit. + double max_dkds = 0.0; + double prev_kappa = bezier9Curvature(b, 0.0); + for (int i = 1; i <= BEZIER9_ARC_LENGTH_SAMPLES; i++) { + double kappa_i = bezier9Curvature(b, b->t_table[i]); + double ds = b->s_table[i] - b->s_table[i - 1]; + if (ds > BEZIER9_MIN_LENGTH) { + double dk_ds = fabs(kappa_i - prev_kappa) / ds; + if (dk_ds > max_dkds) { + max_dkds = dk_ds; + } + } + prev_kappa = kappa_i; + } + b->max_dkappa_ds = max_dkds; + + return TP_ERR_OK; +} + +double bezier9AccLimit(Bezier9 const * const b, + double v_req, + double a_max, + double j_max) +{ + (void)v_req; + + if (!b || a_max <= 0.0) { + return 0.0; + } + + // Centripetal acceleration limit: v² · κ ≤ a_normal + double a_normal = BLEND_ACC_RATIO_NORMAL * a_max; + double v_limit = sqrt(a_normal * b->min_radius); + + // Centripetal jerk limit: v³ · dκ/ds ≤ j_normal + // This bounds the rate of centripetal acceleration change through + // the blend, preventing jerk spikes at entry/exit where curvature + // ramps from zero (G2 boundary) to peak. + if (j_max > 0.0 && b->max_dkappa_ds > BEZIER9_CURVATURE_EPSILON) { + double j_normal = BLEND_ACC_RATIO_NORMAL * j_max; + double v_jerk = cbrt(j_normal / b->max_dkappa_ds); + if (v_jerk < v_limit) { + v_limit = v_jerk; + } + } + + return v_limit; +} + +double bezier9Deviation(Bezier9 const * const b, + PmCartesian const * const intersection_point) +{ + if (!b || !intersection_point) { + return 0.0; + } + + // Evaluate Bezier at t = 0.5 (midpoint) + PmCartesian midpoint; + bezier5_eval(b->P, 0.5, &midpoint); + + // Compute xyz distance to intersection point + double dx = midpoint.x - intersection_point->x; + double dy = midpoint.y - intersection_point->y; + double dz = midpoint.z - intersection_point->z; + + return sqrt(dx * dx + dy * dy + dz * dz); +} diff --git a/src/emc/motion_planning/bezier9.h b/src/emc/motion_planning/bezier9.h new file mode 100644 index 00000000000..6db93ff2d8a --- /dev/null +++ b/src/emc/motion_planning/bezier9.h @@ -0,0 +1,199 @@ +#ifndef BEZIER9_H +#define BEZIER9_H + +#include "posemath.h" +#include "emcpos.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// Numerical tolerances for Bezier blend curves +#define BEZIER9_MIN_CONTROL_DIST 1e-12 +#define BEZIER9_MIN_LENGTH 1e-12 +#define BEZIER9_CURVATURE_EPSILON 1e-8 +#define BEZIER9_POS_EPSILON 1e-12 +#define BEZIER9_ARC_LENGTH_SAMPLES 1024 +#define BEZIER9_CURVATURE_SAMPLES 30 +#define BEZIER9_GOLDEN_RATIO 0.618033988749895 + +/** + * Bezier9 - 9D quintic Bezier curve for trajectory blending + * + * Represents a smooth blend between two motion segments with G2 continuity + * (matching position, tangent direction, AND curvature at entry/exit). + * The curve is defined by 18 control points (6 per subspace) split across + * xyz (translation), abc (rotation), and uvw (auxiliary) coordinate spaces. + * + * G2 continuity is achieved by placing P0,P1,P2 collinear along the + * incoming tangent and P3,P4,P5 collinear along the outgoing tangent. + * This gives B''(0) = B''(1) = 0, so curvature is zero at both endpoints + * — matching the zero curvature of adjacent line segments and eliminating + * the centripetal acceleration discontinuity that cubic blends suffer from. + * + * Arc-length parameterization is precomputed during initialization to enable + * constant-velocity traversal. Curvature analysis is performed on the xyz + * components only, as centripetal acceleration is physical (not rotational). + */ +typedef struct { + // Control points for quintic Bezier in each 3D subspace + PmCartesian P[6]; // xyz control points (translation) + PmCartesian A[6]; // abc control points (rotation) + PmCartesian U[6]; // uvw control points (auxiliary) + + // Cached geometric properties + double total_length; // Total arc length (9D magnitude) + double max_kappa; // Maximum curvature (xyz only) + double min_radius; // Minimum radius = 1/max_kappa + double max_dkappa_ds; // Maximum |dκ/ds| (curvature rate wrt arc length) + + // Arc-length parameterization lookup tables + // Map arc-length s to Bezier parameter t via binary search + double t_table[BEZIER9_ARC_LENGTH_SAMPLES + 1]; + double s_table[BEZIER9_ARC_LENGTH_SAMPLES + 1]; +} Bezier9; + +/** + * bezier9Init - Initialize a 9D quintic Bezier blend curve + * + * Constructs 6 control points from boundary conditions (start/end positions + * and tangent unit vectors) using a shape parameter alpha. Control points + * are placed for G2 continuity (zero curvature at endpoints): + * + * P[0] = start (on curve) + * P[1] = start + alpha * u_start (tangent control) + * P[2] = start + 2*alpha * u_start (curvature = 0) + * P[3] = end - 2*alpha * u_end (curvature = 0) + * P[4] = end - alpha * u_end (tangent control) + * P[5] = end (on curve) + * + * Precomputes arc-length parameterization table and maximum curvature. + * + * @param b Output Bezier9 structure to initialize + * @param start Start pose (9D position) + * @param end End pose (9D position) + * @param u_start_xyz Unit tangent vector at start (xyz) + * @param u_end_xyz Unit tangent vector at end (xyz) + * @param u_start_abc Unit tangent vector at start (abc) + * @param u_end_abc Unit tangent vector at end (abc) + * @param u_start_uvw Unit tangent vector at start (uvw) + * @param u_end_uvw Unit tangent vector at end (uvw) + * @param alpha Shape parameter controlling control point placement (> 0) + * @return TP_ERR_OK on success, error code otherwise + */ +int bezier9Init(Bezier9 * const b, + EmcPose const * const start, + EmcPose const * const end, + PmCartesian const * const u_start_xyz, + PmCartesian const * const u_end_xyz, + PmCartesian const * const u_start_abc, + PmCartesian const * const u_end_abc, + PmCartesian const * const u_start_uvw, + PmCartesian const * const u_end_uvw, + double alpha); + +/** + * bezier9Point - Evaluate position on curve at arc-length parameter + * + * Uses precomputed arc-length parameterization to map progress (in length + * units) to the Bezier parameter t, then evaluates B(t) in all 9 dimensions. + * + * @param b Input Bezier9 curve + * @param progress Arc-length parameter (0 to total_length) + * @param out Output pose at the given arc length + * @return TP_ERR_OK on success, error code otherwise + */ +int bezier9Point(Bezier9 const * const b, + double progress, + EmcPose * const out); + +/** + * bezier9Tangent - Compute unit tangent vectors at arc-length parameter + * + * Evaluates the derivative B'(t) at the Bezier parameter corresponding to + * the given arc length, then normalizes to unit length in each subspace. + * + * @param b Input Bezier9 curve + * @param progress Arc-length parameter (0 to total_length) + * @param xyz_out Output unit tangent vector (xyz), can be NULL + * @param abc_out Output unit tangent vector (abc), can be NULL + * @param uvw_out Output unit tangent vector (uvw), can be NULL + * @return TP_ERR_OK on success, error code otherwise + */ +int bezier9Tangent(Bezier9 const * const b, + double progress, + PmCartesian * const xyz_out, + PmCartesian * const abc_out, + PmCartesian * const uvw_out); + +/** + * bezier9Length - Get total arc length of curve + * + * Returns the cached total arc length computed during initialization. + * + * @param b Input Bezier9 curve + * @return Total arc length (always >= 0) + */ +double bezier9Length(Bezier9 const * const b); + +/** + * bezier9Curvature - Compute curvature at Bezier parameter t + * + * Computes curvature κ(t) = |B'' - (B'·B''/|B'|²)B'| / |B'|² using xyz + * components only. Curvature is geometric property of translation path. + * + * @param b Input Bezier9 curve + * @param t Bezier parameter (0 to 1) + * @return Curvature value (always >= 0) + */ +double bezier9Curvature(Bezier9 const * const b, double t); + +/** + * bezier9MaxCurvature - Find maximum curvature over entire curve + * + * Samples curvature at N points (BEZIER9_CURVATURE_SAMPLES), identifies + * the region with maximum curvature, then refines using golden section + * search. Caches result in max_kappa and min_radius fields. + * + * @param b Bezier9 curve to analyze (modified: caches max_kappa, min_radius) + * @return TP_ERR_OK on success, error code otherwise + */ +int bezier9MaxCurvature(Bezier9 * const b); + +/** + * bezier9AccLimit - Compute velocity limit based on centripetal acceleration + * + * Uses minimum radius (1/max_kappa) and normal acceleration budget to compute + * maximum safe velocity: v_max = sqrt(a_normal * min_radius). Follows the + * same pattern as arc9AccLimit for consistency with existing blend code. + * + * @param b Input Bezier9 curve + * @param v_req Requested velocity (unused, for API compatibility) + * @param a_max Maximum total acceleration budget + * @param j_max Maximum jerk (for curvature-rate limiting), 0 to skip + * @return Velocity limit (physical velocity units) + */ +double bezier9AccLimit(Bezier9 const * const b, + double v_req, + double a_max, + double j_max); + +/** + * bezier9Deviation - Compute maximum deviation from original corner + * + * Evaluates the Bezier curve at t=0.5 (midpoint) and measures xyz distance + * to the intersection point. This provides an estimate of blend deviation + * for G64 P tolerance checking. + * + * @param b Input Bezier9 curve + * @param intersection_point Original corner intersection point (xyz only) + * @return Deviation distance (always >= 0) + */ +double bezier9Deviation(Bezier9 const * const b, + PmCartesian const * const intersection_point); + +#ifdef __cplusplus +} +#endif + +#endif // BEZIER9_H diff --git a/src/emc/motion_planning/blend_sizing.cc b/src/emc/motion_planning/blend_sizing.cc new file mode 100644 index 00000000000..b1da56fc5d0 --- /dev/null +++ b/src/emc/motion_planning/blend_sizing.cc @@ -0,0 +1,1146 @@ +/******************************************************************** + * Description: blend_sizing.cc + * Blend sizing and infrastructure for 9D Bezier blend curves. + * + * Ported from Tormach PathPilot's biarc blend sizing logic, + * adapted to produce a single Bezier9 blend curve instead of + * two biarc segments. + * + * Author: aitekcnc + * License: GPL Version 2 + * System: Linux + ********************************************************************/ + +#include "blend_sizing.h" + +extern "C" { +#include "tp_types.h" +#include "tp_debug.h" +#include "tc.h" +#include "blendmath.h" +#include "emcpose.h" +#include "rtapi_math.h" +} + +#include +#include + +/* ---------------------------------------------------------------- + * Static helpers: per-axis planar bounds (Tormach port) + * ---------------------------------------------------------------- */ + +/** + * Compute the maximum scalar value (velocity or acceleration) achievable + * in a 2D blend plane without exceeding per-axis bounds. + * + * Given orthonormal basis vectors (u, v) spanning the blend plane in 9D, + * the worst-case per-axis loading for any direction in the plane is + * sqrt(u_i^2 + v_i^2) on axis i. The maximum scalar is constrained by + * the axis with the tightest bound relative to its loading. + * + * Port of Tormach's findMaxValueOnPlane + VecVecHarmonicAddSq. + */ +static double findMaxValueOnPlane9( + PmCartesian const *u_xyz, PmCartesian const *u_abc, PmCartesian const *u_uvw, + PmCartesian const *v_xyz, PmCartesian const *v_abc, PmCartesian const *v_uvw, + AxisBounds9 const *bounds) +{ + double m2 = TP_BIG_NUM * TP_BIG_NUM; + + /* For each axis i: if loading s_i = u_i^2+v_i^2 > 0 and bound b_i > 0, + * then max_value^2 <= b_i^2 / s_i. Track the minimum. */ +#define CHECK_AXIS(u_comp, v_comp, b_comp) do { \ + double s = (u_comp)*(u_comp) + (v_comp)*(v_comp); \ + double b = (b_comp); \ + if (s > 0.0 && b > 0.0) { \ + double ratio = (b*b) / s; \ + if (ratio < m2) m2 = ratio; \ + } \ +} while(0) + + CHECK_AXIS(u_xyz->x, v_xyz->x, bounds->xyz.x); + CHECK_AXIS(u_xyz->y, v_xyz->y, bounds->xyz.y); + CHECK_AXIS(u_xyz->z, v_xyz->z, bounds->xyz.z); + CHECK_AXIS(u_abc->x, v_abc->x, bounds->abc.x); + CHECK_AXIS(u_abc->y, v_abc->y, bounds->abc.y); + CHECK_AXIS(u_abc->z, v_abc->z, bounds->abc.z); + CHECK_AXIS(u_uvw->x, v_uvw->x, bounds->uvw.x); + CHECK_AXIS(u_uvw->y, v_uvw->y, bounds->uvw.y); + CHECK_AXIS(u_uvw->z, v_uvw->z, bounds->uvw.z); + +#undef CHECK_AXIS + + return sqrt(m2); +} + +/** + * Compute the maximum scalar value along a single direction in 9D + * without exceeding per-axis bounds. + * + * For collinear tangents (1D motion), the constraint is simply: + * max_value = min over active axes of (bound_i / |dir_i|). + * + * Port of Tormach's 1/findAbsMaxScale. + */ +static double findMaxValueAlongDir9( + PmCartesian const *dir_xyz, PmCartesian const *dir_abc, PmCartesian const *dir_uvw, + AxisBounds9 const *bounds) +{ + double max_scale = 0.0; + +#define CHECK_DIR(d_comp, b_comp) do { \ + double b = (b_comp); \ + if (b > 0.0) { \ + double s = fabs(d_comp) / b; \ + if (s > max_scale) max_scale = s; \ + } \ +} while(0) + + CHECK_DIR(dir_xyz->x, bounds->xyz.x); + CHECK_DIR(dir_xyz->y, bounds->xyz.y); + CHECK_DIR(dir_xyz->z, bounds->xyz.z); + CHECK_DIR(dir_abc->x, bounds->abc.x); + CHECK_DIR(dir_abc->y, bounds->abc.y); + CHECK_DIR(dir_abc->z, bounds->abc.z); + CHECK_DIR(dir_uvw->x, bounds->uvw.x); + CHECK_DIR(dir_uvw->y, bounds->uvw.y); + CHECK_DIR(dir_uvw->z, bounds->uvw.z); + +#undef CHECK_DIR + + return (max_scale > 0.0) ? (1.0 / max_scale) : TP_BIG_NUM; +} + +/** + * Gram-Schmidt orthonormalization of two 9D tangent vectors. + * + * Produces orthonormal basis (u, v) spanning the same plane as (u1, u2). + * Returns 0 on success, -1 if vectors are collinear (only u is valid). + * + * Port of Tormach's VecVecOrthonormal. + */ +static int orthonormalize9( + PmCartesian const *u1_xyz, PmCartesian const *u1_abc, PmCartesian const *u1_uvw, + PmCartesian const *u2_xyz, PmCartesian const *u2_abc, PmCartesian const *u2_uvw, + PmCartesian *u_xyz, PmCartesian *u_abc, PmCartesian *u_uvw, + PmCartesian *v_xyz, PmCartesian *v_abc, PmCartesian *v_uvw) +{ + /* u = normalize(u1) in 9D */ + double mag1_sq = pmSq(u1_xyz->x) + pmSq(u1_xyz->y) + pmSq(u1_xyz->z) + + pmSq(u1_abc->x) + pmSq(u1_abc->y) + pmSq(u1_abc->z) + + pmSq(u1_uvw->x) + pmSq(u1_uvw->y) + pmSq(u1_uvw->z); + double mag1 = sqrt(mag1_sq); + if (mag1 < TP_POS_EPSILON) return -1; + double inv1 = 1.0 / mag1; + + u_xyz->x = u1_xyz->x * inv1; u_xyz->y = u1_xyz->y * inv1; u_xyz->z = u1_xyz->z * inv1; + u_abc->x = u1_abc->x * inv1; u_abc->y = u1_abc->y * inv1; u_abc->z = u1_abc->z * inv1; + u_uvw->x = u1_uvw->x * inv1; u_uvw->y = u1_uvw->y * inv1; u_uvw->z = u1_uvw->z * inv1; + + /* proj = dot(u2, u) in 9D */ + double proj = u2_xyz->x * u_xyz->x + u2_xyz->y * u_xyz->y + u2_xyz->z * u_xyz->z + + u2_abc->x * u_abc->x + u2_abc->y * u_abc->y + u2_abc->z * u_abc->z + + u2_uvw->x * u_uvw->x + u2_uvw->y * u_uvw->y + u2_uvw->z * u_uvw->z; + + /* v = u2 - proj*u */ + v_xyz->x = u2_xyz->x - proj * u_xyz->x; + v_xyz->y = u2_xyz->y - proj * u_xyz->y; + v_xyz->z = u2_xyz->z - proj * u_xyz->z; + v_abc->x = u2_abc->x - proj * u_abc->x; + v_abc->y = u2_abc->y - proj * u_abc->y; + v_abc->z = u2_abc->z - proj * u_abc->z; + v_uvw->x = u2_uvw->x - proj * u_uvw->x; + v_uvw->y = u2_uvw->y - proj * u_uvw->y; + v_uvw->z = u2_uvw->z - proj * u_uvw->z; + + /* normalize v */ + double mag2_sq = pmSq(v_xyz->x) + pmSq(v_xyz->y) + pmSq(v_xyz->z) + + pmSq(v_abc->x) + pmSq(v_abc->y) + pmSq(v_abc->z) + + pmSq(v_uvw->x) + pmSq(v_uvw->y) + pmSq(v_uvw->z); + double mag2 = sqrt(mag2_sq); + if (mag2 < TP_POS_EPSILON) return -1; /* collinear */ + double inv2 = 1.0 / mag2; + + v_xyz->x *= inv2; v_xyz->y *= inv2; v_xyz->z *= inv2; + v_abc->x *= inv2; v_abc->y *= inv2; v_abc->z *= inv2; + v_uvw->x *= inv2; v_uvw->y *= inv2; v_uvw->z *= inv2; + + return 0; +} + +/** + * Compute per-axis constrained velocity and acceleration limits + * for a blend in the plane defined by two 9D tangent vectors. + * + * Port of Tormach's find_blend_vel_accel_planar_limits. + */ +static int findBlendPlanarLimits9( + BlendBoundary9 const *boundary, + AxisBounds9 const *vel_bounds, + AxisBounds9 const *acc_bounds, + double *v_max_planar, + double *a_max_planar) +{ + PmCartesian u_xyz = {0,0,0}, u_abc = {0,0,0}, u_uvw = {0,0,0}; + PmCartesian v_xyz = {0,0,0}, v_abc = {0,0,0}, v_uvw = {0,0,0}; + + int res = orthonormalize9( + &boundary->u_start_xyz, &boundary->u_start_abc, &boundary->u_start_uvw, + &boundary->u_end_xyz, &boundary->u_end_abc, &boundary->u_end_uvw, + &u_xyz, &u_abc, &u_uvw, + &v_xyz, &v_abc, &v_uvw); + + if (res == 0) { + /* 2D blend plane: use planar envelope */ + *v_max_planar = findMaxValueOnPlane9( + &u_xyz, &u_abc, &u_uvw, + &v_xyz, &v_abc, &v_uvw, + vel_bounds); + *a_max_planar = findMaxValueOnPlane9( + &u_xyz, &u_abc, &u_uvw, + &v_xyz, &v_abc, &v_uvw, + acc_bounds); + } else { + /* Collinear tangents: 1D motion, use directional constraint */ + *v_max_planar = findMaxValueAlongDir9( + &u_xyz, &u_abc, &u_uvw, vel_bounds); + *a_max_planar = findMaxValueAlongDir9( + &u_xyz, &u_abc, &u_uvw, acc_bounds); + } + + return TP_ERR_OK; +} + +/* ---------------------------------------------------------------- + * Static helpers: helix-aware circle trimming + * ---------------------------------------------------------------- */ + +/** + * Trim a PmCircle to a new angle, properly handling helices and spirals. + * + * Port of Tormach's pmCircleCut. Unlike aitekcnc's pmCircleStretch, + * this correctly scales rHelix proportionally on trim. + * + * @param circ Circle to trim (modified in place) + * @param new_angle New angular span + * @param from_end 1 = keep end (trim from start), 0 = keep start (trim from end) + */ +static int pmCircleCut9(PmCircle * const circ, double new_angle, int from_end) +{ + if (!circ || new_angle < 1e-12) { + return PM_ERR; + } + + if (from_end) { + /* Keep end point, create new start: more complex geometry update */ + double cut_angle = circ->angle - new_angle; + + PmCartesian new_start; + pmCirclePoint(circ, cut_angle, &new_start); + + PmCartesian tmp; + pmCartCartSub(&new_start, &circ->center, &tmp); + + /* Find new rPerp and rTan perpendicular to circle normal */ + pmCartCartCross(&circ->normal, &tmp, &circ->rPerp); + pmCartCartCross(&circ->rPerp, &circ->normal, &circ->rTan); + + /* Center is displaced if there is a helical component */ + pmCartCartSubEq(&tmp, &circ->rTan); + pmCartCartAddEq(&circ->center, &tmp); + + /* Update radius (may change due to spiral) */ + pmCartMag(&circ->rTan, &circ->radius); + } + + /* Scale spiral and helix proportionally */ + double angle_ratio = new_angle / circ->angle; + circ->spiral *= angle_ratio; + pmCartScalMultEq(&circ->rHelix, angle_ratio); + circ->angle = new_angle; + + return PM_OK; +} + +/* ---------------------------------------------------------------- + * Static helpers: position and tangent at arbitrary progress + * ---------------------------------------------------------------- */ + +/** + * Compute tangent of a SphericalArc at arbitrary progress. + * + * Extends arcTangent (which only supports start/end) to any arc-length + * progress by interpolating the radial vector via SLERP. + */ +static int arcTangentAtProgress(SphericalArc const * const arc, + double progress, + PmCartesian * const out) +{ + double net_progress = progress - arc->line_length; + + if (net_progress <= 0.0 && arc->line_length > 0.0) { + /* Still on the lead-in line segment */ + *out = arc->uTan; + return TP_ERR_OK; + } + + /* Convert progress to angle */ + double angle_in = (arc->radius > TP_POS_EPSILON) ? + (net_progress / arc->radius) : 0.0; + if (angle_in < 0.0) angle_in = 0.0; + if (angle_in > arc->angle) angle_in = arc->angle; + + /* Interpolate radius vector via SLERP */ + if (arc->Sangle < TP_POS_EPSILON) { + *out = arc->uTan; + return TP_ERR_OK; + } + double s0 = sin(arc->angle - angle_in) / arc->Sangle; + double s1 = sin(angle_in) / arc->Sangle; + + PmCartesian interp0, interp1, r_at_angle; + pmCartScalMult(&arc->rStart, s0, &interp0); + pmCartScalMult(&arc->rEnd, s1, &interp1); + pmCartCartAdd(&interp0, &interp1, &r_at_angle); + + /* Tangent = binormal x r (tangential direction) */ + PmCartesian r_tan; + pmCartCartCross(&arc->binormal, &r_at_angle, &r_tan); + + /* Spiral correction */ + if (fabs(arc->spiral) > TP_POS_EPSILON && arc->angle > TP_POS_EPSILON) { + double dr = arc->spiral / arc->angle; + PmCartesian d_perp; + pmCartUnit(&r_at_angle, &d_perp); + pmCartScalMultEq(&d_perp, dr); + pmCartCartAddEq(&r_tan, &d_perp); + } + + pmCartUnit(&r_tan, out); + return TP_ERR_OK; +} + +/** + * Get 9D position at an arbitrary progress along a segment. + * + * Handles all blendable motion types: TC_LINEAR, TC_CIRCULAR, + * TC_SPHERICAL, and TC_BEZIER. + */ +static int tcGetPosAtProgress(TC_STRUCT const * const tc, + double progress, + EmcPose * const pos) +{ + PmCartesian xyz = {0, 0, 0}; + PmCartesian abc = {0, 0, 0}; + PmCartesian uvw = {0, 0, 0}; + + switch (tc->motion_type) { + case TC_LINEAR: + if (!tc->coords.line.xyz.tmag_zero && tc->target > TP_POS_EPSILON) { + pmCartLinePoint(&tc->coords.line.xyz, + progress * tc->coords.line.xyz.tmag / tc->target, + &xyz); + } else { + xyz = tc->coords.line.xyz.start; + } + if (!tc->coords.line.abc.tmag_zero && tc->target > TP_POS_EPSILON) { + pmCartLinePoint(&tc->coords.line.abc, + progress * tc->coords.line.abc.tmag / tc->target, + &abc); + } else { + abc = tc->coords.line.abc.start; + } + if (!tc->coords.line.uvw.tmag_zero && tc->target > TP_POS_EPSILON) { + pmCartLinePoint(&tc->coords.line.uvw, + progress * tc->coords.line.uvw.tmag / tc->target, + &uvw); + } else { + uvw = tc->coords.line.uvw.start; + } + break; + + case TC_CIRCULAR: { + double angle = 0.0; + pmCircleAngleFromProgress(&tc->coords.circle.xyz, + &tc->coords.circle.fit, + progress, &angle); + pmCirclePoint(&tc->coords.circle.xyz, angle, &xyz); + + if (!tc->coords.circle.abc.tmag_zero && tc->target > TP_POS_EPSILON) { + pmCartLinePoint(&tc->coords.circle.abc, + progress * tc->coords.circle.abc.tmag / tc->target, + &abc); + } else { + abc = tc->coords.circle.abc.start; + } + if (!tc->coords.circle.uvw.tmag_zero && tc->target > TP_POS_EPSILON) { + pmCartLinePoint(&tc->coords.circle.uvw, + progress * tc->coords.circle.uvw.tmag / tc->target, + &uvw); + } else { + uvw = tc->coords.circle.uvw.start; + } + break; + } + + case TC_SPHERICAL: + arcPoint(&tc->coords.arc.xyz, progress, &xyz); + abc = tc->coords.arc.abc; + uvw = tc->coords.arc.uvw; + break; + + case TC_BEZIER: + bezier9Point(&tc->coords.bezier, progress, pos); + return TP_ERR_OK; + + default: + return TP_ERR_FAIL; + } + + pmCartesianToEmcPose(&xyz, &abc, &uvw, pos); + return TP_ERR_OK; +} + +/** + * Get 9D tangent unit vectors at an arbitrary progress along a segment. + * + * Handles all blendable motion types. For TC_SPHERICAL, computes tangent + * at arbitrary progress via SLERP interpolation of the radial vector. + * Zero-magnitude subspaces produce zero output vectors. + */ +static int tcGetTangentAtProgress(TC_STRUCT const * const tc, + double progress, + PmCartesian * const xyz_out, + PmCartesian * const abc_out, + PmCartesian * const uvw_out) +{ + switch (tc->motion_type) { + case TC_LINEAR: + if (xyz_out) *xyz_out = tc->coords.line.xyz.uVec; + if (abc_out) { + if (tc->coords.line.abc.tmag_zero) { + abc_out->x = abc_out->y = abc_out->z = 0.0; + } else { + *abc_out = tc->coords.line.abc.uVec; + } + } + if (uvw_out) { + if (tc->coords.line.uvw.tmag_zero) { + uvw_out->x = uvw_out->y = uvw_out->z = 0.0; + } else { + *uvw_out = tc->coords.line.uvw.uVec; + } + } + break; + + case TC_CIRCULAR: { + if (xyz_out) { + double angle = 0.0; + pmCircleAngleFromProgress(&tc->coords.circle.xyz, + &tc->coords.circle.fit, + progress, &angle); + pmCircleTangentVector(&tc->coords.circle.xyz, angle, xyz_out); + } + if (abc_out) { + if (tc->coords.circle.abc.tmag_zero) { + abc_out->x = abc_out->y = abc_out->z = 0.0; + } else { + *abc_out = tc->coords.circle.abc.uVec; + } + } + if (uvw_out) { + if (tc->coords.circle.uvw.tmag_zero) { + uvw_out->x = uvw_out->y = uvw_out->z = 0.0; + } else { + *uvw_out = tc->coords.circle.uvw.uVec; + } + } + break; + } + + case TC_SPHERICAL: + if (xyz_out) { + arcTangentAtProgress(&tc->coords.arc.xyz, progress, xyz_out); + } + /* SphericalArc abc/uvw are static offsets, no tangent component */ + if (abc_out) abc_out->x = abc_out->y = abc_out->z = 0.0; + if (uvw_out) uvw_out->x = uvw_out->y = uvw_out->z = 0.0; + break; + + case TC_BEZIER: + bezier9Tangent(&tc->coords.bezier, progress, xyz_out, abc_out, uvw_out); + break; + + default: + if (xyz_out) xyz_out->x = xyz_out->y = xyz_out->z = 0.0; + if (abc_out) abc_out->x = abc_out->y = abc_out->z = 0.0; + if (uvw_out) uvw_out->x = uvw_out->y = uvw_out->z = 0.0; + return TP_ERR_FAIL; + } + + return TP_ERR_OK; +} + +/* ---------------------------------------------------------------- + * Public API + * ---------------------------------------------------------------- */ + +/** + * Compute blend tolerance from G64 P values. + * + * Uses tolerance_ratio = 0.5 (Tormach convention). If no explicit + * tolerance is set (G64 without P word), falls back to half the + * segment's nominal length. Final tolerance is capped at half of + * each segment's nominal length to prevent over-consumption. + */ +int tcFindBlendTolerance9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double * const tolerance) +{ + if (!prev_tc || !tc || !tolerance) { + return TP_ERR_MISSING_INPUT; + } + + double T1 = prev_tc->tolerance; + double T2 = tc->tolerance; + + /* If no tolerance set (G64 without P word), use fraction of segment length */ + if (T1 <= 0.0) { + T1 = prev_tc->nominal_length * BLEND9_TOLERANCE_RATIO; + } + if (T2 <= 0.0) { + T2 = tc->nominal_length * BLEND9_TOLERANCE_RATIO; + } + + /* Blend tolerance is minimum of both, further capped by half-segment */ + double T_min = fmin(T1, T2); + T_min = fmin(T_min, prev_tc->nominal_length * BLEND9_TOLERANCE_RATIO); + T_min = fmin(T_min, tc->nominal_length * BLEND9_TOLERANCE_RATIO); + + *tolerance = T_min; + return TP_ERR_OK; +} + +/** + * Compute maximum blend region size (upper bound for optimizer). + * + * For each segment, the usable length is min(target, nominal_length/2). + * For circular arcs, additionally limit to 60 degrees of arc. + * Returns the minimum of both segments (symmetrical search). + */ +double findMaxBlendRegion9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc) +{ + if (!prev_tc || !tc) { + return 0.0; + } + + /* Per-segment usable length */ + double l_prev = fmin(prev_tc->target, prev_tc->nominal_length / 2.0); + double l_tc = fmin(tc->target, tc->nominal_length / 2.0); + + /* For circular arcs, limit to 60 degrees */ + if (prev_tc->motion_type == TC_CIRCULAR) { + double radius = prev_tc->coords.circle.xyz.radius; + if (radius > TP_POS_EPSILON) { + double circ_limit = BLEND9_CIRC_MAX_ANGLE * radius; + l_prev = fmin(l_prev, circ_limit); + } + } + + if (tc->motion_type == TC_CIRCULAR) { + double radius = tc->coords.circle.xyz.radius; + if (radius > TP_POS_EPSILON) { + double circ_limit = BLEND9_CIRC_MAX_ANGLE * radius; + l_tc = fmin(l_tc, circ_limit); + } + } + + /* Symmetrical search: return minimum */ + double Rb_max = fmin(l_prev, l_tc); + + tp_debug_print("findMaxBlendRegion9: l_prev=%g, l_tc=%g, Rb_max=%g\n", + l_prev, l_tc, Rb_max); + + return Rb_max; +} + +/** + * Sample 9D positions and tangent vectors at blend boundary points. + * + * Given Rb (blend region size), computes where the blend starts on prev_tc + * (distance Rb from its end) and where it ends on tc (distance Rb from + * its start). Samples 9D positions and tangent vectors at those points. + * + * The intersection point is computed as the start of tc (progress=0), + * which is the shared corner point. This works for any motion type. + */ +int findBlendPointsAndTangents9(double Rb, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + BlendBoundary9 * const boundary) +{ + if (!prev_tc || !tc || !boundary) { + return TP_ERR_MISSING_INPUT; + } + + /* Compute progress values on each segment */ + boundary->s_prev = fmax(prev_tc->target - Rb, 0.0); + boundary->s_tc = fmin(Rb, tc->target); + + /* Sample 9D position at blend start (near end of prev_tc) */ + int res = tcGetPosAtProgress(prev_tc, boundary->s_prev, &boundary->P_start); + if (res != TP_ERR_OK) { + return res; + } + + /* Sample 9D position at blend end (near start of tc) */ + res = tcGetPosAtProgress(tc, boundary->s_tc, &boundary->P_end); + if (res != TP_ERR_OK) { + return res; + } + + /* Get 9D tangent vectors at blend start */ + res = tcGetTangentAtProgress(prev_tc, boundary->s_prev, + &boundary->u_start_xyz, + &boundary->u_start_abc, + &boundary->u_start_uvw); + if (res != TP_ERR_OK) { + return res; + } + + /* Get 9D tangent vectors at blend end */ + res = tcGetTangentAtProgress(tc, boundary->s_tc, + &boundary->u_end_xyz, + &boundary->u_end_abc, + &boundary->u_end_uvw); + if (res != TP_ERR_OK) { + return res; + } + + /* Compute intersection point (xyz corner) from start of tc. + * This is the shared endpoint, works for any motion type. */ + EmcPose corner; + res = tcGetPosAtProgress(tc, 0.0, &corner); + if (res == TP_ERR_OK) { + boundary->intersection_point = corner.tran; + } else { + /* Fallback to legacy function for TC_LINEAR/TC_CIRCULAR */ + tcGetIntersectionPoint(prev_tc, tc, &boundary->intersection_point); + } + + return TP_ERR_OK; +} + +/** + * Compute kinematic parameters from boundary conditions. + * + * Finds the intersection angle between xyz tangent vectors, computes + * normal acceleration limit, velocity goal, and target radius. + * Per-axis bounds across all 9 dimensions constrain the velocity + * and acceleration to stay within each axis's individual limits. + */ +int findBlendParameters9(BlendBoundary9 const * const boundary, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double max_feed_scale, + AxisBounds9 const * const vel_bounds, + AxisBounds9 const * const acc_bounds, + BlendParams9 * const params) +{ + if (!boundary || !prev_tc || !tc || !params) { + return TP_ERR_MISSING_INPUT; + } + + /* Compute intersection angle from xyz tangent vectors */ + int res = findIntersectionAngle(&boundary->u_start_xyz, + &boundary->u_end_xyz, + ¶ms->theta); + if (res != TP_ERR_OK) { + return res; + } + + /* Check for nearly-collinear segments */ + if (params->theta < BLEND9_MIN_THETA) { + tp_debug_print("findBlendParameters9: theta %g too small\n", params->theta); + return TP_ERR_TOLERANCE; + } + + /* Normal acceleration budget from segment limits */ + double a_prev = prev_tc->maxaccel; + double a_tc = tc->maxaccel; + params->a_n_max = fmin(a_prev, a_tc) * BLEND_ACC_RATIO_NORMAL; + + /* Goal velocity (worst case at max feed override) */ + double v_prev = tcGetMaxTargetVel(prev_tc, max_feed_scale); + double v_tc = tcGetMaxTargetVel(tc, max_feed_scale); + params->v_goal = fmax(v_prev, v_tc); + + /* Apply per-axis bounds if provided */ + if (vel_bounds && acc_bounds) { + double v_max_planar, a_max_planar; + findBlendPlanarLimits9(boundary, vel_bounds, acc_bounds, + &v_max_planar, &a_max_planar); + + /* Constrain v_goal and a_n_max by per-axis limits */ + params->v_goal = fmin(params->v_goal, v_max_planar); + params->a_n_max = fmin(params->a_n_max, a_max_planar * BLEND_ACC_RATIO_NORMAL); + + tp_debug_print(" per-axis limits: v_max_planar=%g, a_max_planar=%g\n", + v_max_planar, a_max_planar); + } + + /* Goal radius for centripetal limit */ + if (params->a_n_max > TP_ACCEL_EPSILON) { + params->R_goal = (params->v_goal * params->v_goal) / params->a_n_max; + } else { + params->R_goal = TP_BIG_NUM; + } + + /* v_plan starts at v_goal, refined by optimizer */ + params->v_plan = params->v_goal; + + tp_debug_print("findBlendParameters9: theta=%g, a_n_max=%g, v_goal=%g, R_goal=%g\n", + params->theta, params->a_n_max, params->v_goal, params->R_goal); + + return TP_ERR_OK; +} + +/** + * Binary search optimizer for blend region size. + * + * Searches for the largest Rb (blend region size) that satisfies both: + * 1. G64 P tolerance: bezier9Deviation <= tolerance + * 2. Velocity goal: bezier9AccLimit >= v_goal (within tolerance) + * + * Growing Rb increases blend radius (better velocity) but also increases + * deviation from the corner. The search finds the sweet spot. + */ +int optimizeBlendSize9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance, + double max_feed_scale, + AxisBounds9 const * const vel_bounds, + AxisBounds9 const * const acc_bounds, + BlendSolution9 * const result) +{ + if (!prev_tc || !tc || !result) { + return TP_ERR_MISSING_INPUT; + } + + memset(result, 0, sizeof(BlendSolution9)); + result->status = BLEND9_FAIL; + + /* Get maximum blend region */ + double Rb_max = findMaxBlendRegion9(prev_tc, tc); + if (Rb_max < TP_POS_EPSILON) { + tp_debug_print("optimizeBlendSize9: Rb_max too small (%g)\n", Rb_max); + result->status = BLEND9_ZERO_LENGTH; + return TP_ERR_FAIL; + } + + /* First pass: check if blend is geometrically feasible */ + BlendBoundary9 boundary; + BlendParams9 params; + + int res = findBlendPointsAndTangents9(Rb_max, prev_tc, tc, &boundary); + if (res != TP_ERR_OK) { + return res; + } + + res = findBlendParameters9(&boundary, prev_tc, tc, max_feed_scale, + vel_bounds, acc_bounds, ¶ms); + if (res != TP_ERR_OK) { + if (res == TP_ERR_TOLERANCE) { + result->status = BLEND9_COLLINEAR; + } + return res; + } + + /* Binary search between epsilon and Rb_max */ + double Rb_lo = TP_POS_EPSILON; + double Rb_hi = Rb_max; + int found_valid = 0; + + for (int iter = 0; iter < BLEND9_MAX_ITERATIONS; iter++) { + double Rb = (Rb_lo + Rb_hi) / 2.0; + + /* Sample boundary at this Rb */ + res = findBlendPointsAndTangents9(Rb, prev_tc, tc, &boundary); + if (res != TP_ERR_OK) { + Rb_hi = Rb; + continue; + } + + /* Compute parameters */ + res = findBlendParameters9(&boundary, prev_tc, tc, max_feed_scale, + vel_bounds, acc_bounds, ¶ms); + if (res != TP_ERR_OK) { + Rb_hi = Rb; + continue; + } + + /* Construct trial Bezier9 */ + double alpha = Rb * BLEND9_ALPHA_FACTOR; + Bezier9 trial; + res = bezier9Init(&trial, + &boundary.P_start, + &boundary.P_end, + &boundary.u_start_xyz, + &boundary.u_end_xyz, + &boundary.u_start_abc, + &boundary.u_end_abc, + &boundary.u_start_uvw, + &boundary.u_end_uvw, + alpha); + if (res != TP_ERR_OK) { + Rb_hi = Rb; + continue; + } + + /* Check deviation against tolerance */ + double deviation = bezier9Deviation(&trial, &boundary.intersection_point); + + /* Check velocity limit from curvature and curvature rate */ + double a_max_blend = fmin(prev_tc->maxaccel, tc->maxaccel); + double j_max_blend = fmin(prev_tc->maxjerk, tc->maxjerk); + double v_limit = bezier9AccLimit(&trial, params.v_goal, a_max_blend, + j_max_blend); + + tp_debug_print(" iter=%d Rb=%g alpha=%g dev=%g tol=%g v_limit=%g v_goal=%g len=%g\n", + iter, Rb, alpha, deviation, tolerance, v_limit, params.v_goal, + bezier9Length(&trial)); + + /* Convergence logic */ + int tol_ok = (deviation <= tolerance); + int vel_ok = (v_limit >= params.v_goal * (1.0 - BLEND9_VEL_REL_TOL)) + || (v_limit >= params.v_goal - BLEND9_VEL_ABS_TOL); + + if (tol_ok && vel_ok) { + /* Found a good solution -- try to grow for smoother blend */ + result->Rb = Rb; + result->bezier = trial; + result->boundary = boundary; + result->params = params; + result->params.v_plan = fmin(v_limit, params.v_goal); + found_valid = 1; + Rb_lo = Rb; + } else if (!tol_ok) { + /* Deviation too large -- shrink */ + Rb_hi = Rb; + } else { + /* Velocity too low but tolerance OK -- accept with reduced + * velocity. Sharp corners can't reach full feed, and that's + * fine: the blend still smooths the path. Try to grow for + * a larger radius (better velocity). */ + result->Rb = Rb; + result->bezier = trial; + result->boundary = boundary; + result->params = params; + result->params.v_plan = v_limit; + found_valid = 1; + Rb_lo = Rb; + } + + /* Check if search interval has converged */ + if ((Rb_hi - Rb_lo) < (Rb_max * 0.01) && found_valid) { + tp_debug_print(" converged at iter=%d, Rb=%g\n", iter, result->Rb); + break; + } + } + + /* If no valid solution found during search, try Rb_max as fallback */ + if (!found_valid) { + double Rb = Rb_max; + res = findBlendPointsAndTangents9(Rb, prev_tc, tc, &boundary); + if (res == TP_ERR_OK) { + res = findBlendParameters9(&boundary, prev_tc, tc, max_feed_scale, + vel_bounds, acc_bounds, ¶ms); + } + if (res == TP_ERR_OK) { + double alpha = Rb * BLEND9_ALPHA_FACTOR; + Bezier9 trial; + res = bezier9Init(&trial, + &boundary.P_start, + &boundary.P_end, + &boundary.u_start_xyz, + &boundary.u_end_xyz, + &boundary.u_start_abc, + &boundary.u_end_abc, + &boundary.u_start_uvw, + &boundary.u_end_uvw, + alpha); + if (res == TP_ERR_OK) { + double deviation = bezier9Deviation(&trial, &boundary.intersection_point); + if (deviation <= tolerance) { + double a_max_blend = fmin(prev_tc->maxaccel, tc->maxaccel); + double j_max_blend = fmin(prev_tc->maxjerk, tc->maxjerk); + double v_limit = bezier9AccLimit(&trial, params.v_goal, + a_max_blend, j_max_blend); + result->Rb = Rb; + result->bezier = trial; + result->boundary = boundary; + result->params = params; + result->params.v_plan = fmin(v_limit, params.v_goal); + found_valid = 1; + } + } + } + } + + if (!found_valid) { + tp_debug_print("optimizeBlendSize9: no valid solution found\n"); + result->status = BLEND9_TOLERANCE_EXCEEDED; + return TP_ERR_FAIL; + } + + /* Compute trim amounts */ + result->trim_prev = prev_tc->target - result->boundary.s_prev; + result->trim_tc = result->boundary.s_tc; + result->status = BLEND9_OK; + + tp_debug_print("optimizeBlendSize9: Rb=%g, v_plan=%g, length=%g, trim_prev=%g, trim_tc=%g\n", + result->Rb, result->params.v_plan, + bezier9Length(&result->bezier), + result->trim_prev, result->trim_tc); + + return TP_ERR_OK; +} + +/** + * Create a TC_STRUCT for the blend segment from optimization result. + * + * Initializes a new segment with motion_type = TC_BEZIER, sets kinematic + * limits from the Bezier curvature analysis, and marks the segment for + * tangent continuity with adjacent segments. + */ +int createBlendSegment9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + BlendSolution9 const * const solution, + TC_STRUCT * const blend_tc, + double cycle_time) +{ + if (!prev_tc || !tc || !solution || !blend_tc) { + return TP_ERR_MISSING_INPUT; + } + + /* Zero-initialize */ + memset(blend_tc, 0, sizeof(TC_STRUCT)); + + /* Initialize as TC_BEZIER */ + tcInit(blend_tc, + TC_BEZIER, + prev_tc->canon_motion_type, + cycle_time, + prev_tc->enables, + 0); /* no atspeed for blend */ + + /* Set tolerance from parent segments */ + blend_tc->tolerance = fmin(prev_tc->tolerance, tc->tolerance); + blend_tc->synchronized = TC_SYNC_NONE; + + /* Kinematic limits */ + double a_max = fmin(prev_tc->maxaccel, tc->maxaccel); + double v_plan = solution->params.v_plan; + double v_req = fmax(prev_tc->reqvel, tc->reqvel); + double jerk = fmin(prev_tc->maxjerk, tc->maxjerk); + + tcSetupMotion(blend_tc, v_req, v_plan, a_max, jerk); + + /* Hard cap: curvature-rate jerk limit (like kink_vel, ignores feed override). + * Centripetal jerk = v³ · dκ/ds must stay within the jerk budget. */ + double v_jerk_limit = bezier9AccLimit(&solution->bezier, v_req, a_max, jerk); + blend_tc->kink_vel = v_jerk_limit; + + /* Store bezier curve in coords union */ + blend_tc->coords.bezier = solution->bezier; + + /* Set target length */ + double length = bezier9Length(&solution->bezier); + blend_tc->target = length; + blend_tc->nominal_length = length; + + /* Terminal condition: tangent continuity with next segment */ + blend_tc->term_cond = TC_TERM_COND_TANGENT; + + /* Tangential acceleration ratio for bezier + * Accounts for centripetal acceleration consuming part of the budget */ + if (solution->bezier.min_radius > TP_POS_EPSILON) { + double a_n = (v_plan * v_plan) / solution->bezier.min_radius; + if (a_n < a_max) { + blend_tc->acc_ratio_tan = sqrt(1.0 - (a_n * a_n) / (a_max * a_max)); + } else { + blend_tc->acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + } + } else { + blend_tc->acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + } + + /* Clamp velocity by segment length (prevents exceeding one sample per cycle) */ + tcClampVelocityByLength(blend_tc); + + /* Copy metadata from parent segments */ + blend_tc->syncdio = prev_tc->syncdio; + blend_tc->tag = prev_tc->tag; + blend_tc->indexer_jnum = -1; + blend_tc->active_depth = 1; + + tp_debug_print("createBlendSegment9: target=%g, v_plan=%g, acc=%g, acc_ratio_tan=%g\n", + blend_tc->target, v_plan, a_max, blend_tc->acc_ratio_tan); + + return TP_ERR_OK; +} + +/** + * Trim a TC_STRUCT by removing length from its end or start. + * + * Dispatches by motion type to handle xyz, abc, uvw components: + * - TC_LINEAR: pmCartLineStretch on each PmCartLine component + * - TC_CIRCULAR: pmCircleCut9 on xyz circle (helix-safe), pmCartLineStretch on abc/uvw + * - TC_SPHERICAL: adjusts arc angle range + * + * @param tc Segment to trim (modified in place) + * @param trim_amount Length to remove + * @param from_end 1 = keep end (trim start), 0 = keep start (trim end) + */ +int trimSegment9(TC_STRUCT * const tc, + double trim_amount, + int from_end) +{ + if (!tc || trim_amount < TP_POS_EPSILON) { + return TP_ERR_OK; /* Nothing to trim */ + } + + double old_target = tc->target; + double new_target = old_target - trim_amount; + + if (new_target < TP_POS_EPSILON) { + tp_debug_print("trimSegment9: trim_amount %g exceeds target %g\n", + trim_amount, old_target); + return TP_ERR_FAIL; + } + + double ratio = new_target / old_target; + + switch (tc->motion_type) { + case TC_LINEAR: { + /* Trim xyz */ + if (!tc->coords.line.xyz.tmag_zero) { + double xyz_new = tc->coords.line.xyz.tmag * ratio; + pmCartLineStretch(&tc->coords.line.xyz, xyz_new, from_end); + } + + /* Trim abc proportionally */ + if (!tc->coords.line.abc.tmag_zero) { + double abc_new = tc->coords.line.abc.tmag * ratio; + pmCartLineStretch(&tc->coords.line.abc, abc_new, from_end); + } + + /* Trim uvw proportionally */ + if (!tc->coords.line.uvw.tmag_zero) { + double uvw_new = tc->coords.line.uvw.tmag * ratio; + pmCartLineStretch(&tc->coords.line.uvw, uvw_new, from_end); + } + + tc->target = new_target; + tc->nominal_length = new_target; + break; + } + + case TC_CIRCULAR: { + /* Convert trim ratio to new angle */ + double new_angle = tc->coords.circle.xyz.angle * ratio; + + /* Trim xyz circle with helix-safe cut */ + int res = pmCircleCut9(&tc->coords.circle.xyz, new_angle, from_end); + if (res != PM_OK) { + tp_debug_print("trimSegment9: pmCircleCut9 failed\n"); + return TP_ERR_FAIL; + } + + /* Recompute spiral arc length fit */ + findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit); + + /* Trim abc proportionally */ + if (!tc->coords.circle.abc.tmag_zero) { + double abc_new = tc->coords.circle.abc.tmag * ratio; + pmCartLineStretch(&tc->coords.circle.abc, abc_new, from_end); + } + + /* Trim uvw proportionally */ + if (!tc->coords.circle.uvw.tmag_zero) { + double uvw_new = tc->coords.circle.uvw.tmag * ratio; + pmCartLineStretch(&tc->coords.circle.uvw, uvw_new, from_end); + } + + /* Recompute target from new geometry */ + tc->target = pmCircle9Target(&tc->coords.circle); + tc->nominal_length = tc->target; + break; + } + + case TC_SPHERICAL: { + /* SphericalArc: adjust angular range. + * The arc is parameterized by progress (arc length). + * Trimming means reducing the angular span and updating + * start/end radial vectors via SLERP. */ + SphericalArc *arc = &tc->coords.arc.xyz; + double new_angle = arc->angle * ratio; + + if (from_end) { + /* Keep end, trim start: compute new rStart via SLERP */ + double cut_angle = arc->angle - new_angle; + if (arc->Sangle > TP_POS_EPSILON) { + double s0 = sin(arc->angle - cut_angle) / arc->Sangle; + double s1 = sin(cut_angle) / arc->Sangle; + PmCartesian new_rStart; + PmCartesian i0, i1; + pmCartScalMult(&arc->rStart, s0, &i0); + pmCartScalMult(&arc->rEnd, s1, &i1); + pmCartCartAdd(&i0, &i1, &new_rStart); + arc->rStart = new_rStart; + + /* Update start point and tangent */ + pmCartCartAdd(&arc->center, &new_rStart, &arc->start); + arcTangent(arc, &arc->uTan, 0); + } + } else { + /* Keep start, trim end: compute new rEnd via SLERP */ + if (arc->Sangle > TP_POS_EPSILON) { + double s0 = sin(arc->angle - new_angle) / arc->Sangle; + double s1 = sin(new_angle) / arc->Sangle; + PmCartesian new_rEnd; + PmCartesian i0, i1; + pmCartScalMult(&arc->rStart, s0, &i0); + pmCartScalMult(&arc->rEnd, s1, &i1); + pmCartCartAdd(&i0, &i1, &new_rEnd); + arc->rEnd = new_rEnd; + + /* Update end point */ + pmCartCartAdd(&arc->center, &new_rEnd, &arc->end); + } + } + + /* Scale spiral proportionally */ + double angle_ratio = new_angle / arc->angle; + arc->spiral *= angle_ratio; + arc->angle = new_angle; + arc->Sangle = sin(new_angle); + + /* Update arc target */ + tc->target = new_angle * arc->radius + arc->line_length * ratio; + tc->nominal_length = tc->target; + break; + } + + default: + tp_debug_print("trimSegment9: unsupported motion type %d\n", + tc->motion_type); + return TP_ERR_FAIL; + } + + tp_debug_print("trimSegment9: type=%d, from_end=%d, trim=%g, new_target=%g\n", + tc->motion_type, from_end, trim_amount, tc->target); + + return TP_ERR_OK; +} diff --git a/src/emc/motion_planning/blend_sizing.h b/src/emc/motion_planning/blend_sizing.h new file mode 100644 index 00000000000..bc86fa4f1cf --- /dev/null +++ b/src/emc/motion_planning/blend_sizing.h @@ -0,0 +1,181 @@ +/******************************************************************** + * Description: blend_sizing.h + * Blend sizing and infrastructure for 9D Bezier blend curves. + * + * Ported from Tormach PathPilot's biarc blend sizing logic, + * adapted to produce a single Bezier9 blend curve instead of + * two biarc segments. + * + * Author: aitekcnc + * License: GPL Version 2 + * System: Linux + ********************************************************************/ +#ifndef BLEND_SIZING_H +#define BLEND_SIZING_H + +#include "posemath.h" +#include "emcpos.h" +#include "tc_types.h" +#include "bezier9.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Blend sizing constants */ +#define BLEND9_TOLERANCE_RATIO 0.5 +#define BLEND9_ALPHA_FACTOR 0.30 +#define BLEND9_MAX_ITERATIONS 12 +#define BLEND9_VEL_REL_TOL 0.01 +#define BLEND9_VEL_ABS_TOL 0.1 +#define BLEND9_CIRC_MAX_ANGLE (PM_PI / 3.0) +#define BLEND9_MIN_THETA (PM_PI / 180.0) + +/** + * Per-axis velocity and acceleration bounds in 9D. + * Each PmCartesian holds bounds for 3 axes of a subspace. + * Zero bounds mean the axis is not active (no constraint applied). + */ +typedef struct { + PmCartesian xyz; /* Per-axis bounds for X, Y, Z */ + PmCartesian abc; /* Per-axis bounds for A, B, C */ + PmCartesian uvw; /* Per-axis bounds for U, V, W */ +} AxisBounds9; + +/** + * Blend boundary conditions sampled from two adjacent segments. + * Contains 9D positions and tangent vectors at the blend region edges, + * plus the xyz intersection (corner) point for tolerance checking. + */ +typedef struct { + EmcPose P_start; /* 9D position at blend start (on prev_tc) */ + EmcPose P_end; /* 9D position at blend end (on tc) */ + + PmCartesian u_start_xyz; /* Unit tangent at start (xyz) */ + PmCartesian u_end_xyz; /* Unit tangent at end (xyz) */ + PmCartesian u_start_abc; /* Unit tangent at start (abc) */ + PmCartesian u_end_abc; /* Unit tangent at end (abc) */ + PmCartesian u_start_uvw; /* Unit tangent at start (uvw) */ + PmCartesian u_end_uvw; /* Unit tangent at end (uvw) */ + + PmCartesian intersection_point; /* XYZ corner point */ + + double s_prev; /* Progress on prev_tc at blend start */ + double s_tc; /* Progress on tc at blend end */ +} BlendBoundary9; + +/** + * Kinematic and geometric parameters for blend optimization. + */ +typedef struct { + double theta; /* Half-angle between tangent vectors */ + double a_n_max; /* Normal acceleration budget */ + double v_goal; /* Target velocity at max feed override */ + double R_goal; /* Goal radius = v_goal^2 / a_n_max */ + double v_plan; /* Planned velocity after constraints */ +} BlendParams9; + +/** + * Blend optimization status codes. + */ +typedef enum { + BLEND9_OK = 0, + BLEND9_FAIL = -1, + BLEND9_ANGLE_TOO_SMALL = -2, + BLEND9_TOLERANCE_EXCEEDED = -3, + BLEND9_ZERO_LENGTH = -4, + BLEND9_COLLINEAR = -5 +} blend9_status_t; + +/** + * Complete blend optimization result. + * Contains everything needed to create the blend segment and trim neighbors. + */ +typedef struct { + double Rb; /* Final blend region size */ + Bezier9 bezier; /* Constructed blend curve */ + BlendParams9 params; /* Computed kinematic parameters */ + BlendBoundary9 boundary; /* Boundary conditions used */ + double trim_prev; /* Amount to trim from prev_tc end */ + double trim_tc; /* Amount to trim from tc start */ + blend9_status_t status; /* Result status */ +} BlendSolution9; + +/** + * Compute blend tolerance from G64 P values. + * Caps at half of each segment's nominal length (tolerance_ratio = 0.5). + */ +int tcFindBlendTolerance9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double * const tolerance); + +/** + * Compute maximum blend region size (upper bound for optimizer). + * For circular arcs, limits to 60 degrees of arc. + */ +double findMaxBlendRegion9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc); + +/** + * Sample 9D positions and tangent vectors at blend boundary points. + * Given Rb (blend region size), finds where the blend starts on prev_tc + * and ends on tc, then samples positions and tangent vectors there. + */ +int findBlendPointsAndTangents9(double Rb, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + BlendBoundary9 * const boundary); + +/** + * Compute kinematic parameters from boundary conditions. + * Finds intersection angle, normal acceleration limit, velocity goal, + * and target radius. Per-axis bounds constrain velocity and acceleration + * across all 9 dimensions in the blend plane. + */ +int findBlendParameters9(BlendBoundary9 const * const boundary, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double max_feed_scale, + AxisBounds9 const * const vel_bounds, + AxisBounds9 const * const acc_bounds, + BlendParams9 * const params); + +/** + * Binary search optimizer for blend region size. + * Finds optimal Rb that satisfies both G64 P tolerance and velocity + * constraints. Per-axis bounds limit blend velocity and acceleration + * to stay within each axis's individual limits. + */ +int optimizeBlendSize9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance, + double max_feed_scale, + AxisBounds9 const * const vel_bounds, + AxisBounds9 const * const acc_bounds, + BlendSolution9 * const result); + +/** + * Create a TC_STRUCT for the blend segment from optimization result. + * Sets motion_type = TC_BEZIER, kinematic limits from Bezier curvature, + * and term_cond = TC_TERM_COND_TANGENT. + */ +int createBlendSegment9(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + BlendSolution9 const * const solution, + TC_STRUCT * const blend_tc, + double cycle_time); + +/** + * Trim a TC_STRUCT by removing length from its end or start. + * Dispatches by motion type to handle xyz, abc, uvw components. + * @param from_end 1 = keep end, trim start; 0 = keep start, trim end + */ +int trimSegment9(TC_STRUCT * const tc, + double trim_amount, + int from_end); + +#ifdef __cplusplus +} +#endif + +#endif /* BLEND_SIZING_H */ diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..1577cfcf4c2 --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,104 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +// emcpos.h includes posemath.h which has C++ function overloads +// so we can't use extern "C" around it +#include "emcpos.h" + +extern "C" { +#include "kinematics_userspace/kinematics_user.h" +} + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..2aa8fe488d9 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,238 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +// emcpos.h includes posemath.h which has C++ function overloads +#include "emcpos.h" + +extern "C" { +#include "kinematics_userspace/kinematics_user.h" +} + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/motion_planning_9d.cc b/src/emc/motion_planning/motion_planning_9d.cc new file mode 100644 index 00000000000..b19ef09b3c6 --- /dev/null +++ b/src/emc/motion_planning/motion_planning_9d.cc @@ -0,0 +1,4796 @@ +/******************************************************************** +* Description: motion_planning_9d.cc +* Tormach 9D trajectory planner - Userspace planning implementation +* +* Ported from Tormach LinuxCNC implementation +* Provides lookahead optimization for 9-axis coordinated motion +* +* Author: Tormach (original), Port by LinuxCNC community +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +* RT SAFETY: This file is USERSPACE ONLY. It uses C++ std::vector +* and dynamic allocation which are FORBIDDEN in RT context. +* Communication with RT layer is via atomic operations only. +* +********************************************************************/ + +#include "motion_planning_9d.hh" +#include "smoothing_data.hh" +#include "motion.h" // For emcmot_status_t (must be before tp.h) +#include "tc.h" +#include "tp.h" +#include "atomic_9d.h" +#include "posemath.h" +#include "rtapi.h" // For rtapi_print_msg +#include "motion_types.h" // For EMC_MOTION_TYPE_TRAVERSE +#include // Ruckig integration + +#include +#include +#include +#include +#include +#include + +#include // For gettimeofday (etime equivalent in userspace) + +//============================================================================ +// USERSPACE IMPLEMENTATIONS OF TC/TCQ FUNCTIONS +// These are userspace-specific versions that don't depend on RT globals +//============================================================================ + +/** + * @brief Userspace version of tcqLen for planner_type 2 + * Works directly with atomic queue indices + */ +extern "C" int tcqLen_user(TC_QUEUE_STRUCT const * const tcq) +{ + if (!tcq) return -1; + + // Read atomic indices (planner_type 2 mode) + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + // Calculate length with circular buffer wraparound + int len = (current_end - current_start + tcq->size) % tcq->size; + return len; +} + +/** + * @brief Userspace version of tcqBack for planner_type 2 + * Gets n-th item from back of queue (n=0 is last, n=-1 is second-to-last, etc.) + */ +static TC_STRUCT * tcqBack_user(TC_QUEUE_STRUCT * const tcq, int n) +{ + if (!tcq) return NULL; + + // Read atomic indices + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + // Calculate length + int len = (current_end - current_start + tcq->size) % tcq->size; + + // Check for empty queue or invalid index + if (len == 0 || n > 0) return NULL; + + // Check if index is within valid range + // n is negative: n=0 means last, n=-1 means second-to-last + // So we need -n < len, i.e., n > -len + if (n <= -len) return NULL; + + // Calculate index from end: end + n - 1 + // Fix for negative modulus by adding tcq->size + int k = current_end + n - 1 + tcq->size; + int idx = k % tcq->size; + return &(tcq->queue[idx]); +} + +/** + * @brief Userspace version of tcqItem for planner_type 2 + * Gets item at index n from front of queue (n=0 is first/active, n=1 is next, etc.) + */ +static TC_STRUCT * tcqItem_user(TC_QUEUE_STRUCT * const tcq, int n) +{ + if (!tcq || n < 0) return NULL; + + // Read atomic indices + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + // Calculate length + int len = (current_end - current_start + tcq->size) % tcq->size; + + // Check if index is within valid range + if (n >= len) return NULL; + + // Calculate index from start + int idx = (current_start + n) % tcq->size; + return &(tcq->queue[idx]); +} + +/** + * @brief Userspace version of tcqPut for planner_type 2 + * Writes a TC_STRUCT to the shared memory queue + */ +extern "C" int tcqPut_user(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc) +{ + if (!tcq || !tc) return -1; + + // Read current indices atomically (Tormach pattern: ACQUIRE for reads) + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + // Calculate next end index + int next_end = (current_end + 1) % tcq->size; + + // Check if queue is full (with margin for reverse history) + int available_space = (current_start - next_end + tcq->size) % tcq->size; + const int TCQ_REVERSE_MARGIN = 100; + const int QUEUE_MARGIN = 20; + if (available_space < TCQ_REVERSE_MARGIN + QUEUE_MARGIN) { + return -1; // Queue full + } + + // Copy TC element to queue at current end + tcq->queue[current_end] = *tc; + + // Atomically update end index using exchange (Tormach pattern) + // __atomic_exchange_n is a read-modify-write that provides stronger visibility guarantees + __atomic_exchange_n(&tcq->end_atomic, next_end, __ATOMIC_ACQ_REL); + + return 0; +} + +/** + * @brief Userspace version of tcGetTangentialMaxAccel_9D + * + * Returns maximum acceleration considering: + * 1. Machine acceleration limit (tc->maxaccel) + * 2. Joint-space acceleration limit from Jacobian analysis (if valid) + */ +static double tcGetTangentialMaxAccel_9D_user(TC_STRUCT const * const tc) +{ + if (!tc) return 0.0; + + double acc = tc->maxaccel; + + // Apply joint-space acceleration limit from userspace kinematics + if (tc->joint_space.valid) { + double joint_acc_limit = tc->joint_space.acc_limit; + if (joint_acc_limit > 0.0 && joint_acc_limit < acc) { + acc = joint_acc_limit; + } + } + + // Reduce tangential acceleration for curved segments (centripetal uses part of budget) + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL || + tc->motion_type == TC_BEZIER) { + acc *= tc->acc_ratio_tan; + } + + return acc; +} + +/** + * @brief Userspace version of tcGetMaxVel_9D + * + * Returns maximum velocity considering: + * 1. Requested velocity with feed override + * 2. Machine velocity limit (tc->maxvel) + * 3. Joint-space velocity limit from Jacobian analysis (if valid) + */ +static double tcGetMaxVel_9D_user(TC_STRUCT const * const tc, double max_feed_scale) +{ + if (!tc) return 0.0; + + // Start with requested velocity + double vel = tc->reqvel; + + // Apply feed override + vel *= max_feed_scale; + + // Clamp to machine velocity limit + if (tc->maxvel > 0.0 && vel > tc->maxvel) { + vel = tc->maxvel; + } + + // Apply joint-space velocity limit from userspace kinematics + // This accounts for Jacobian-derived limits near singularities + // and joint velocity constraints + if (tc->joint_space.valid) { + double joint_vel_limit = tc->joint_space.vel_limit_end; + if (joint_vel_limit > 0.0 && joint_vel_limit < vel) { + vel = joint_vel_limit; + } + } + + return vel; +} + +/** + * @brief Get base velocity limit (before feed scaling) + * + * Returns the velocity limit considering: + * 1. Requested velocity (tc->reqvel from F-word) + * 2. Machine velocity limit (tc->maxvel) + * + * NOTE: Does NOT include vLimit. vLimit is an absolute cap that must be + * applied AFTER feed scaling, not before. Use applyVLimit() after scaling. + */ +static double getEffectiveVelLimit(TP_STRUCT const * const tp, TC_STRUCT const * const tc) +{ + if (!tp || !tc) return 0.0; + + // Start with reqvel clamped to maxvel + double vel_limit = tc->reqvel > 0 ? fmin(tc->reqvel, tc->maxvel) : tc->maxvel; + + // NOTE: vLimit is NOT applied here - it's an absolute cap applied after feed scaling + return vel_limit; +} + +/** + * @brief Apply vLimit as absolute cap after feed scaling + * + * vLimit from the max velocity slider is an absolute limit, not scaled by feed override. + * This must be applied AFTER multiplying by feed_scale. + * + * @param tp Trajectory planner (for vLimit value) + * @param tc Segment (for rotary/sync checks) + * @param scaled_vel Velocity after feed scaling (vel_limit * feed_scale) + * @return Velocity capped by vLimit if applicable + */ +static double applyVLimit(TP_STRUCT const * const tp, TC_STRUCT const * const tc, double scaled_vel) +{ + if (!tp || !tc) return scaled_vel; + + // maxvel is an absolute physics cap — feed override cannot exceed it. + // Matches original planner: tcGetMaxTargetVel returns min(reqvel*scale, maxvel). + // For arcs, maxvel includes the centripetal jerk velocity limit. + scaled_vel = fmin(scaled_vel, tc->maxvel); + + // Apply vLimit (max velocity slider) with same conditions as tp.c: + // - Not a pure rotary move + // - Not position synchronized (spindle sync) + if (tp->vLimit > 0.0 && !tcPureRotaryCheck(tc) && tc->synchronized != TC_SYNC_POSITION) { + return fmin(scaled_vel, tp->vLimit); + } + + return scaled_vel; +} + +// Configuration defaults (can be overridden via HAL/INI) +static int g_optimization_depth = 8; + +// Static smoothing data (pre-allocated to avoid reallocation) +static SmoothingData g_smoothing_data; + +//============================================================================ +// PHASE 3: PREDICTIVE HANDOFF STATE +//============================================================================ + +// Global configuration (loaded from INI) +static PredictiveHandoffConfig g_handoff_config; + + +// State for feed override monitoring +static double g_last_feed_scale = 1.0; +static double g_last_replan_time_ms = 0.0; + +// Committed feed: the feed scale that has been fully propagated to all segments. +// Updated only when the cursor walk completes a full pass. +// computeRuckigProfiles_9D uses this instead of live feed to avoid overwriting +// profiles that the reactive path (recomputeDownstreamProfiles / cursor walk) +// is actively updating at a newer feed. +static double g_committed_feed = -1.0; // -1 = uninitialized +static double g_committed_rapid = -1.0; + +// Incremental downstream profile recompute cursor +// After a feed change, walk through the queue recomputing profiles a few per tick. +// Both slider values are snapshotted at trigger time so the cursor walk uses a +// consistent target even if the operator keeps moving the slider. +static int g_recompute_cursor = 0; // 0 = idle, >0 = queue index to resume from +static double g_recompute_feed_scale = 1.0; // snapshot of feed_scale at trigger time +static double g_recompute_rapid_scale = 1.0; // snapshot of rapid_scale at trigger time +static bool g_recompute_first_batch_done = false; // settling gate: true after first cursor tick completes +static constexpr int RECOMPUTE_MIN_PER_TICK = 1; // always do at least this many + +// Queued feed change: stored when a new feed change arrives while cursor is active. +// Only the next-in-line can be freely overwritten; the working change is locked. +static double g_next_feed_scale = -1.0; // -1 = no pending change +static double g_next_rapid_scale = -1.0; + +// Commit point: queue index where the current feed change takes effect. +// Segments before this index keep old-feed profiles. +static int g_commit_segment = 1; // default: active+1 (existing behavior) + +// Cursor walk throughput tracking (segments per tick, EMA) +static double g_segments_per_tick = 3.0; // conservative initial estimate + +// Ruckig computation timing stats +struct RuckigTimingStats { + double total_us = 0.0; // Total computation time in microseconds + int count = 0; // Number of computations + double last_us = 0.0; // Most recent computation time + double max_us = 0.0; // Maximum computation time seen +}; +static RuckigTimingStats g_ruckig_timing; + +// Adaptive handoff horizon: starts at base_ms, backs off on missed branches +struct AdaptiveHorizon { + static constexpr double BASE_MS = 5.0; // Aggressive starting point + static constexpr double MAX_MS = 100.0; // Cap to prevent runaway + static constexpr double BACKOFF = 2.0; // Multiply on failure + static constexpr double DECAY = 0.9; // Multiply on success (decay toward base) + + double current_ms = BASE_MS; + int consecutive_misses = 0; + int total_misses = 0; + int total_takes = 0; + + double get() const { return current_ms; } + + void onMiss() { + consecutive_misses++; + total_misses++; + current_ms = fmin(current_ms * BACKOFF, MAX_MS); + } + + void onTake() { + consecutive_misses = 0; + total_takes++; + // Decay toward base, but not below + current_ms = fmax(current_ms * DECAY, BASE_MS); + } +}; +static AdaptiveHorizon g_adaptive_horizon; + +// Reference to emcmotStatus for feed scale reading +extern emcmot_status_t *emcmotStatus; + +/** + * @brief Userspace equivalent of etime() - returns current time in seconds + */ +static double etime_user() { + struct timeval tv; + gettimeofday(&tv, NULL); + return (double)tv.tv_sec + (double)tv.tv_usec / 1000000.0; +} + +//============================================================================ +// INTERNAL HELPER FUNCTIONS +//============================================================================ + +/** + * @brief Get per-segment feed scale for planning + * + * Returns the correct feed override scale factor for a given segment, + * based on its motion type. Rapid traverses (G0) use rapid_scale, + * feed moves (G1/G2/G3) use feed_scale. This avoids the problem + * where net_feed_scale reflects the currently active segment's motion + * type, which is wrong for queued segments of a different type. + * + * @param tc Trajectory segment (used to determine motion type) + * @return Feed scale factor (0.0 to 10.0) + */ +static inline double tpGetSegmentFeedScale(TC_STRUCT const * const tc) { + if (!emcmotStatus) return 1.0; + + double scale; + if (tc && tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) { + scale = emcmotStatus->rapid_scale; + } else { + scale = emcmotStatus->feed_scale; + } + if (scale < 0.0) scale = 0.0; + if (scale > 10.0) scale = 10.0; + return scale; +} + +/** + * @brief Get per-segment feed scale from snapshot values + * + * Used by the cursor walk and recomputeDownstreamProfiles to pick the + * correct snapshotted slider value for a given segment type. The snapshots + * are captured once at trigger time so the walk uses a consistent target. + * + * @param tc Trajectory segment (used to determine motion type) + * @param snap_feed Snapshotted feed_scale value + * @param snap_rapid Snapshotted rapid_scale value + * @return Clamped feed scale for this segment + */ +static inline double tpGetSnapshotFeedScale(TC_STRUCT const * const tc, + double snap_feed, double snap_rapid) { + double scale; + if (tc && tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) { + scale = snap_rapid; + } else { + scale = snap_feed; + } + if (scale < 0.0) scale = 0.0; + if (scale > 10.0) scale = 10.0; + return scale; +} + +/** + * @brief Get unscaled exit velocity from a profile + * + * Reads the profile's final velocity and divides by the feed scale the + * profile was computed at. All call sites that need "what base velocity + * does this profile exit at?" should use this helper so the divisor is + * always the profile's own feed scale (not some other snapshot). + */ +static inline double profileExitVelUnscaled(const ruckig_profile_t *prof) +{ + double pf = prof->computed_feed_scale; + if (pf < 0.001) return 0.0; + return prof->v[RUCKIG_PROFILE_PHASES] / pf; +} + +/** + * @brief Get maximum target velocity for a segment + * + * Computes the maximum allowable velocity for this segment + * considering geometry, feed override, and machine limits. + * + * Uses 9D-aware velocity calculation from tc_9d.h + * + * @param tc Trajectory component + * @param max_feed_scale Feed override scale + * @return Maximum target velocity + */ +static double tcGetPlanMaxTargetVel(TC_STRUCT const * const tc, double max_feed_scale) { + if (!tc) return 0.0; + + // Use 9D velocity calculation + return tcGetMaxVel_9D_user(tc, max_feed_scale); +} + +/** + * @brief Get tangential maximum acceleration for a segment (9D wrapper) + * + * Returns the maximum acceleration in the direction of motion, + * considering all axis limits and geometry. + * + * Uses userspace 9D-aware calculation + * + * @param tc Trajectory component + * @return Maximum tangential acceleration + */ +static double tcGetTangentialMaxAccel_9D_local(TC_STRUCT const * const tc) { + if (!tc) return 0.0; + + // Use userspace 9D acceleration calculation + return tcGetTangentialMaxAccel_9D_user(tc); +} + +/** + * @brief Get final velocity limit for a segment + * + * Returns the maximum safe final velocity for this segment, + * which may be reduced by blending or termination conditions. + * + * Uses atomic operations to read from shared_9d structure. + * + * @param tc Trajectory component + * @return Final velocity limit + */ +static double tcGetFinalVelLimit(TC_STRUCT const * const tc) { + if (!tc) return 0.0; + + // Read final_vel_limit atomically from shared optimization data + double final_vel_limit = atomicLoadDouble(&tc->shared_9d.final_vel_limit); + + // If not yet set by optimizer, fall back to blend velocity or target + if (final_vel_limit > 0.0) { + return final_vel_limit; + } + + if (tc->blend_vel > 0.0) { + return tc->blend_vel; + } + + return tc->target_vel; +} + +/** + * @brief Apply kink velocity limit + * + * Reduces velocity at sharp corners (kinks) to avoid violating + * acceleration limits during direction changes. + * + * @param tc Trajectory component + * @param v_max Unadjusted maximum velocity + * @return Adjusted velocity accounting for kink + */ +static double applyKinkVelLimit(TC_STRUCT const * const tc, double v_max) { + if (!tc) return 0.0; + + // TODO: Implement proper kink detection and limiting + // For now, check if we have a kink velocity computed + if (tc->kink_vel > 0.0 && tc->kink_vel < v_max) { + return tc->kink_vel; + } + + return v_max; +} + +/** + * @brief Apply kink velocity cap with correct feed override handling + * + * The backward pass runs at feed_scale=1.0 and stores + * final_vel = min(vel_cap, kink_vel) + * When kink_vel was the binding constraint (not deceleration distance), + * final_vel equals kink_vel. Simply scaling kink_vel by feed_scale is + * wrong when feed < 1.0 because the physical kink limit doesn't change. + * + * Detection: if unscaled_v_exit >= kink_vel, the backward pass was + * kink-limited (not decel-limited). In that case, the correct exit + * velocity is min(max_vel_at_feed, kink_vel). + * + * @param scaled_v_exit Feed-scaled exit velocity (already clamped to max_vel) + * @param unscaled_v_exit Unscaled backward pass exit velocity (shared_9d.final_vel) + * @param max_vel Feed-scaled programmed max velocity for this segment + * @param kink_vel Physical kink limit (tc->kink_vel, negative if unset) + * @return Adjusted exit velocity + */ +static inline double applyKinkVelCap(double scaled_v_exit, double unscaled_v_exit, + double max_vel, double kink_vel) +{ + if (kink_vel <= 0.0) return scaled_v_exit; + + double result; + if (unscaled_v_exit >= kink_vel - 1e-6) { + // Backward pass was kink-limited, not decel-limited. + // At current feed scale, exit = min(programmed max at feed, physical kink limit) + result = fmin(max_vel, kink_vel); + } else { + // Decel-limited or vel-cap-limited: scale normally, cap by kink + result = fmin(scaled_v_exit, kink_vel); + } + return result; +} + +//============================================================================ +// CORE OPTIMIZATION FUNCTIONS +//============================================================================ + +/** + * @brief Compute minimum distance for jerk-limited velocity change + * + * Given start velocity v_s and end velocity v_f, with max acceleration a_max + * and max jerk j_max, compute the minimum distance required for a jerk-limited + * deceleration (or acceleration). + * + * Two cases: + * Triangular accel profile (dv <= a_max²/j_max): + * Acceleration never reaches a_max. Peak accel = sqrt(dv * j_max). + * t_total = 2 * sqrt(dv / j_max) + * d = v_f * t + dv * sqrt(dv / j_max) + * (exact: integral of velocity over symmetric jerk profile) + * + * Trapezoidal accel profile (dv > a_max²/j_max): + * Acceleration ramps to a_max, holds, then ramps back to 0. + * t_ramp = a_max / j_max + * dv_ramp = a_max² / j_max (velocity change during both ramp phases) + * t_const = (dv - dv_ramp) / a_max + * t_total = 2 * t_ramp + t_const + * d = v_f * t_total + 0.5*dv*t_total - (dv² - dv_ramp²)/(6*a_max) + * (exact integration, simplified) + */ +static double jerkLimitedBrakingDistance(double v_s, double v_f, + double a_max, double j_max) +{ + double dv = fabs(v_s - v_f); + if (dv < 1e-9) return 0.0; + + double v_lo = fmin(v_s, v_f); // lower velocity (endpoint for decel) + double dv_ramp = a_max * a_max / j_max; // velocity change in triangular profile + + if (dv <= dv_ramp) { + // Triangular acceleration profile + double t_half = sqrt(dv / j_max); + double t_total = 2.0 * t_half; + // Exact distance: v_lo * t_total + dv * t_half + // (the dv*t_half term comes from integrating the triangular velocity bump) + return v_lo * t_total + dv * t_half; + } else { + // Trapezoidal acceleration profile + double t_ramp = a_max / j_max; + double t_const = (dv - dv_ramp) / a_max; + double t_total = 2.0 * t_ramp + t_const; + // For any symmetric acceleration profile (a(t) = a(T-t)), the area + // under the velocity-change curve is exactly 0.5 * dv * t_total. + // Proof: symmetry implies ∫t·a(t)dt = T·dv/2, giving + // area = ∫(T-t)·a(t)dt = T·dv - T·dv/2 = T·dv/2. + double area = 0.5 * dv * t_total; + return v_lo * t_total + area; + } +} + +/** + * @brief Create a feed hold profile for a segment + * + * When feed scale drops below 0.001, the segment needs a minimal profile + * that indicates a hold state. This creates a uniform 1.0 second profile + * with feed_scale=0 to signal feed hold to the realtime layer. + * + * @param tc Segment to create profile for + * @param vel_limit Velocity limit to record in profile metadata + * @param vLimit Machine velocity limit to record in profile metadata + */ +static void createFeedHoldProfile(TC_STRUCT *tc, double vel_limit, double vLimit, + const char *caller = "unknown") +{ + double canonical = tc->shared_9d.canonical_feed_scale; + + // FH_VIOLATION: createFeedHoldProfile should only be called when canonical <= 0.6% + // (either in Phase 2 after decelerating to 0.1%, or when already at low speed). + // If canonical > 0.6%, the two-phase guard failed. + if (canonical > 0.006) { + rtapi_print_msg(RTAPI_MSG_ERR, + "FH_VIOLATION seg %d: createFeedHoldProfile at canonical=%.3f (should be <=0.006) caller=%s\n", + tc->id, canonical, caller); + } + + __atomic_store_n(&tc->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + memset(&tc->shared_9d.profile, 0, sizeof(tc->shared_9d.profile)); + tc->shared_9d.profile.duration = 1.0; + tc->shared_9d.profile.computed_feed_scale = 0.0; + tc->shared_9d.profile.computed_vel_limit = vel_limit; + tc->shared_9d.profile.computed_vLimit = vLimit; + for (int j = 0; j < RUCKIG_PROFILE_PHASES; j++) { + tc->shared_9d.profile.t[j] = tc->shared_9d.profile.duration / RUCKIG_PROFILE_PHASES; + tc->shared_9d.profile.t_sum[j] = ((j + 1) * tc->shared_9d.profile.duration) / RUCKIG_PROFILE_PHASES; + } + __atomic_thread_fence(__ATOMIC_SEQ_CST); + __atomic_store_n(&tc->shared_9d.profile.valid, 1, __ATOMIC_RELEASE); +} + +/** + * @brief Compute max entry velocity given jerk-limited braking to v_f within distance d + * + * Binary search: find max v_s such that jerkLimitedBrakingDistance(v_s, v_f) <= d + * Falls back to trapezoidal formula v² = v_f² + 2*a*d as upper bound. + */ +static double jerkLimitedMaxEntryVelocity(double v_f, double d, + double a_max, double j_max) +{ + if (d <= 0.0) return v_f; + + // Trapezoidal upper bound (always >= jerk-limited answer) + double vs_trap = sqrt(v_f * v_f + 2.0 * a_max * d); + + // Check if trapezoidal answer is already jerk-feasible + double d_needed = jerkLimitedBrakingDistance(vs_trap, v_f, a_max, j_max); + if (d_needed <= d) return vs_trap; + + // Binary search between v_f and vs_trap + double v_lo = v_f; + double v_hi = vs_trap; + for (int i = 0; i < 30; i++) { + double v_mid = (v_lo + v_hi) * 0.5; + double d_mid = jerkLimitedBrakingDistance(v_mid, v_f, a_max, j_max); + if (d_mid <= d) { + v_lo = v_mid; + } else { + v_hi = v_mid; + } + if (v_hi - v_lo < 1e-6) break; + } + return v_lo; +} + +/** + * @brief Check if exit velocity at this queue index is safe for the next segment. + * + * "Safe" means the next segment can accept exit_vel as its entry velocity + * given its kinematic limits and distance. Used to determine whether a + * profile chain can be terminated here without creating an impossible + * junction for the next segment. + * + * Returns true when: + * - This segment has STOP/EXACT term_cond (exit is 0) + * - End of queue (no next segment) + * - Next segment can decelerate from exit_vel to its own exit constraint + * within its length, at the given feed scale + */ +static bool canStopChainHere(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, + int index, double exit_vel, + double snap_feed, double snap_rapid, + double default_jerk) +{ + TC_STRUCT *tc = tcqItem_user(queue, index); + if (!tc) return true; + + // Non-tangent: exit is 0, always safe + if (tc->term_cond != TC_TERM_COND_TANGENT) return true; + + // Check next segment + TC_STRUCT *next = tcqItem_user(queue, index + 1); + if (!next || next->target < 1e-9) return true; // end of queue + + double next_feed = tpGetSnapshotFeedScale(next, snap_feed, snap_rapid); + double next_vel_limit = getEffectiveVelLimit(tp, next); + double next_max_vel = applyVLimit(tp, next, next_vel_limit * next_feed); + double next_jrk = next->maxjerk > 0 ? next->maxjerk : default_jerk; + + double next_fv = atomicLoadDouble(&next->shared_9d.final_vel); + double next_exit = (next->term_cond == TC_TERM_COND_TANGENT) + ? fmin(next_fv * next_feed, next_max_vel) : 0.0; + if (next->kink_vel > 0) next_exit = fmin(next_exit, next->kink_vel); + + double next_max_entry = jerkLimitedMaxEntryVelocity( + next_exit, next->target, next->maxaccel, next_jrk); + next_max_entry = fmin(next_max_entry, next_max_vel); + + // Also consider kink at this junction + if (tc->kink_vel > 0) next_max_entry = fmin(next_max_entry, tc->kink_vel); + + return (exit_vel <= next_max_entry + 0.5); +} + +/** + * @brief Compute chain exit cap: maximum safe exit velocity for the active + * segment, accounting for downstream constraints at the given feed. + * + * Walks forward through the queue to find the first "safe endpoint" — + * a segment whose physics naturally allow stopping (STOP condition, or + * long enough to decelerate from max_vel to its exit constraint). + * Then walks backward, cascading reachability limits, to determine the + * tightest entry constraint that propagates back to the active segment. + * + * This replaces the old 1-segment lookahead (which only checked index 1) + * and prevents the active segment from exiting at a velocity that + * creates impossible junctions downstream. + */ +static double computeChainExitCap(TP_STRUCT *tp, TC_STRUCT *active_tc, + double new_feed_scale, double default_jerk, + int max_depth = 16, int start_index = 1) +{ + if (active_tc->term_cond != TC_TERM_COND_TANGENT) return 1e10; + + double rapid = emcmotStatus ? emcmotStatus->rapid_scale : 1.0; + + // Phase 1: Walk forward to find chain depth (first safe endpoint). + // A segment is "safe" if it's STOP/EXACT, end of queue, or long enough + // to brake from max_vel to its exit constraint within its length. + int chain_end = 0; + for (int i = start_index; i <= start_index + max_depth - 1; i++) { + TC_STRUCT *seg = tcqItem_user(&tp->queue, i); + if (!seg || seg->target < 1e-9) { + chain_end = i - 1; + break; + } + + chain_end = i; + + // STOP/EXACT condition: exit is 0, natural boundary + if (seg->term_cond != TC_TERM_COND_TANGENT) break; + + // Long segment: can brake from max_vel to exit constraint + double seg_feed = tpGetSnapshotFeedScale(seg, new_feed_scale, rapid); + double seg_vel_limit = getEffectiveVelLimit(tp, seg); + double seg_max_vel = applyVLimit(tp, seg, seg_vel_limit * seg_feed); + double seg_jrk = seg->maxjerk > 0 ? seg->maxjerk : default_jerk; + double seg_fv = atomicLoadDouble(&seg->shared_9d.final_vel); + double seg_exit = fmin(seg_fv * seg_feed, seg_max_vel); + if (seg->kink_vel > 0) seg_exit = fmin(seg_exit, seg->kink_vel); + + double brake_dist = jerkLimitedBrakingDistance( + seg_max_vel, seg_exit, seg->maxaccel, seg_jrk); + if (seg->target > brake_dist * 1.2) break; // safe: can absorb any entry + } + + if (chain_end < start_index) return 1e10; // no downstream constraints + + // Phase 2: Walk backward from chain_end to start_index, cascading + // reachability limits. Each segment's max entry depends on its + // exit constraint AND what the next segment can accept. + double next_entry_cap = 1e10; + + for (int i = chain_end; i >= start_index; i--) { + TC_STRUCT *seg = tcqItem_user(&tp->queue, i); + if (!seg) continue; + + double seg_feed = tpGetSnapshotFeedScale(seg, new_feed_scale, rapid); + double seg_vel_limit = getEffectiveVelLimit(tp, seg); + double seg_max_vel = applyVLimit(tp, seg, seg_vel_limit * seg_feed); + double seg_jrk = seg->maxjerk > 0 ? seg->maxjerk : default_jerk; + + // This segment's exit constraint (from backward pass + kink) + double seg_fv = atomicLoadDouble(&seg->shared_9d.final_vel); + double seg_exit = (seg->term_cond == TC_TERM_COND_TANGENT) + ? fmin(seg_fv * seg_feed, seg_max_vel) : 0.0; + if (seg->kink_vel > 0) seg_exit = fmin(seg_exit, seg->kink_vel); + + // Cap exit by what next segment can accept (cascading backward) + seg_exit = fmin(seg_exit, next_entry_cap); + + // Max entry this segment can accept + double max_entry = jerkLimitedMaxEntryVelocity( + seg_exit, seg->target, seg->maxaccel, seg_jrk); + max_entry = fmin(max_entry, seg_max_vel); + + // Apply kink at the junction before this segment + TC_STRUCT *prev_seg = tcqItem_user(&tp->queue, i - 1); + if (prev_seg && prev_seg->kink_vel > 0) + max_entry = fmin(max_entry, prev_seg->kink_vel); + + next_entry_cap = max_entry; + } + + return next_entry_cap; +} + +/** + * @brief Compute optimal velocity at segment boundary + * + * Implements the three-constraint algorithm: + * 1. Backward kinematic limit: can we decelerate from entry to v_f_this + * within the segment length, respecting jerk limits? + * 2. Current segment velocity limit + * 3. Previous segment velocity limit (with kink handling) + * + * Returns the minimum of all three constraints. + */ +double tpComputeOptimalVelocity_9D(TC_STRUCT const * const tc, + TC_STRUCT const * const prev_tc, + double v_f_this, + int opt_step) +{ + (void)opt_step; // Reserved for future multi-pass optimization + if (!tc || !prev_tc) return 0.0; + + // Get acceleration and jerk limits for this segment + double acc_this = tcGetTangentialMaxAccel_9D_local(tc); + if (acc_this <= 0.0) return 0.0; + double jrk_this = tc->maxjerk > 0 ? tc->maxjerk : g_handoff_config.default_max_jerk; + + // Constraint 1: Backward kinematic limit (jerk-aware) + // Only apply jerk-aware braking when v_f_this is a real velocity constraint + // (set by kink computation). When v_f_this=0, the segment just arrived in + // the queue and its exit velocity hasn't been computed yet — use trapezoidal + // to avoid over-constraining with an artificially low starting velocity. + double vs_back; + if (jrk_this > 0.0 && v_f_this > 1e-6) { + vs_back = jerkLimitedMaxEntryVelocity(v_f_this, tc->target, acc_this, jrk_this); + } else { + // Fallback to trapezoidal if no jerk limit or v_f not yet set + vs_back = sqrt(v_f_this * v_f_this + 2.0 * acc_this * tc->target); + } + + // Constraint 2: Current segment velocity limit + // Use max_feed_scale=1.0: backward pass works in UNSCALED velocity space. + // Feed override is applied once in the Ruckig profile computation. + // This ensures shared_9d.final_vel is feed-override-independent and + // consistent with kink_vel (also absolute/unscaled). + double vf_limit_this = tcGetPlanMaxTargetVel(tc, 1.0); + + // Constraint 3: Previous segment velocity limit (with kink) + double v_max_prev = tcGetPlanMaxTargetVel(prev_tc, 1.0); + double vf_limit_prev = applyKinkVelLimit(prev_tc, v_max_prev); + + // Return minimum of all constraints + double v_optimal = std::min({vs_back, vf_limit_this, vf_limit_prev}); + + return v_optimal; +} + +/** + * @brief Compute limiting velocities using backward propagation + * + * This is the core "rising tide" optimization algorithm. + * Walks backward through the queue and computes maximum safe + * final velocity for each segment. + * + * Also builds time profile for peak smoothing. + */ +int computeLimitingVelocities_9D(TC_QUEUE_STRUCT *queue, + int optimization_depth, + SmoothingData &smoothing) +{ + if (!queue) return -1; + if (optimization_depth <= 0) return 0; + + static const double LOCAL_VEL_EPSILON = 1e-6; + + // Clear and reset smoothing data + smoothing = SmoothingData(); + smoothing.reserve(optimization_depth + 10); + + // Initialize with dummy starting point at t=0, v=0 + smoothing.ds.push_back(0.0); + smoothing.v_smooth.push_back(0.0); + smoothing.t.push_back(0.0); + smoothing.t_orig.push_back(0.0); + smoothing.ignore.push_back(true); // Ignore the dummy point + + // Walk backward through queue, chaining freshly computed exit velocities + double chained_v_f = -1.0; // Exit vel computed for tc in previous iteration + for (int k = 0; k < optimization_depth; ++k) { + // Access segment at position -k from end using tcqBack + // tcqBack_user(queue, 0) is the last item, tcqBack_user(queue, -1) is second-to-last, etc. + TC_STRUCT const *tc = tcqBack_user(queue, -k); + TC_STRUCT *prev1_tc = tcqBack_user(queue, -(k + 1)); + + if (!tc || !prev1_tc) { + break; // Reached end of queue + } + + // If current segment is dwell/zero-length, previous must end at v=0 + if (tc->target < 1e-9) { + atomicStoreDouble(&prev1_tc->shared_9d.final_vel, 0.0); + chained_v_f = 0.0; + continue; + } + + double v_f_prev = 0.0; + double ds = tc->target; + + // Get final velocity limit for this segment. + // Chain freshly computed values within this sweep: at k=0 we must read + // the stored value (nothing computed yet), at k>0 use the min of chained + // and stored to respect both fresh propagation and external constraints. + double v_f_stored = tcGetFinalVelLimit(tc); + double v_f_this = (k == 0) ? v_f_stored + : std::min(chained_v_f, v_f_stored); + + // Compute optimal velocity for previous segment + v_f_prev = tpComputeOptimalVelocity_9D(tc, prev1_tc, v_f_this, k); + + chained_v_f = v_f_prev; + + // Estimate time for this segment + // Use average velocity between start and end + double v_avg_upper_bound = (tcGetFinalVelLimit(prev1_tc) + v_f_prev) / 2.0; + double v_avg_clamped = std::max(v_avg_upper_bound, LOCAL_VEL_EPSILON); + double dt = ds / v_avg_clamped; + double t_prev = smoothing.t.back(); + + // Check if this segment should be ignored for smoothing + // (e.g., rapids, spindle-synchronized moves) + // TODO: Check tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE + // TODO: Check tc->synchronized + bool ignore_smoothing = false; // For now, don't ignore any + + // Add to smoothing data + smoothing.v_smooth.push_back(v_f_prev); + smoothing.ds.push_back(ds); + smoothing.t.push_back(t_prev + dt); + smoothing.ignore.push_back(ignore_smoothing); + } + + // Add tail dummy point for peak finding algorithm + smoothing.v_smooth.push_back(0.0); + smoothing.ignore.push_back(true); + smoothing.t.push_back(smoothing.t.back() + 10.0); // Far in the future + smoothing.ds.push_back(0.0); + + // Save original time profile for comparison + smoothing.t_orig = smoothing.t; + + return 0; +} + +/** + * @brief Get search end index for optimization + * + * Returns the valid range for searching in smoothing data vectors. + */ +static size_t getOptimizationSearchEnd(const SmoothingData &smoothing) +{ + // We pad the data with extra dummy entries for the peak-finding algorithm + return std::min(smoothing.v_smooth.size() - 1, (size_t)g_optimization_depth); +} + +/** + * @brief Linear interpolation helper + * Reserved for future use in peak smoothing algorithm + */ +#if 0 // Currently unused - reserved for future peak smoothing implementation +static double interp1(double t0, double t1, double v0, double v1, double t) +{ + static const double LOCAL_POS_EPSILON = 1e-9; + if (std::abs(t1 - t0) < LOCAL_POS_EPSILON) { + // If time interval is very short, doesn't matter which side we pick + return v1; + } + return (v1 - v0) / (t1 - t0) * (t - t0) + v0; +} +#endif + +/** + * @brief Apply peak smoothing to velocity profile + * + * Flattens local velocity peaks to improve motion smoothness. + * This reduces jerk and provides more consistent motion. + * + * Algorithm: + * 1. Find local peaks in velocity profile (v[k] >= v[k-1] && v[k] >= v[k+1]) + * 2. Search backward/forward for troughs within t_window + * 3. Flatten peak to maximum of bracketing edge minimum values + * 4. Recalculate time base for next iteration + */ +int tpDoPeakSmoothing_9D(SmoothingData &smoothing, + int optimization_depth, + double t_window) +{ + if (optimization_depth <= 2) return 0; // Need at least 3 points + if (t_window <= 0.0) return 0; + + // Reference to velocity and time vectors for cleaner code + SmoothingVector &v_smooth = smoothing.v_smooth; + const SmoothingVector &t = smoothing.t; + + // Validate vector sizes match + if (v_smooth.size() != t.size()) { + return -1; // Error: mismatched vector sizes + } + + if (v_smooth.empty()) { + return -1; // Error: empty velocity vector + } + + size_t search_depth = getOptimizationSearchEnd(smoothing); + + // Tolerance for ripple detection (how much variation we tolerate before declaring a trough) + static const double tol = 1e-6; + + // Search for local peaks and smooth them + for (size_t k = 1; k < search_depth; ++k) { + // Check if this is a local peak + if (!smoothing.ignore[k] && + (v_smooth[k] >= v_smooth[k-1]) && + (v_smooth[k] >= v_smooth[k+1])) + { + // Found a local peak at k + + // Search backward for the minimum velocity within t_window/2 + double min_v_bwd = v_smooth[k]; + size_t k_bwd = k - 1; + + for (; k_bwd >= 1; --k_bwd) { + double v_bwd = v_smooth[k_bwd]; + + // Stop if we've found a rising edge or ignored segment + if (v_bwd > (min_v_bwd + tol) || smoothing.ignore[k_bwd]) { + break; + } + + // Stop if we've exceeded the time window + double t_cutoff = t[k] - t_window / 2.0; + if (t[k_bwd] < t_cutoff) { + break; + } else { + min_v_bwd = std::min(min_v_bwd, v_bwd); + } + } + size_t peak_idx_bwd = k_bwd + 1; + + // Search forward for the minimum velocity within t_window/2 + double min_v_fwd = v_smooth[k]; + size_t k_fwd = k + 1; + + for (; k_fwd < search_depth; ++k_fwd) { + double v_fwd = v_smooth[k_fwd]; + + // Stop if we've found a rising edge or ignored segment + if (v_fwd > (min_v_fwd + tol) || smoothing.ignore[k_fwd]) { + break; + } + + // Stop if we've exceeded the time window + double t_cutoff = t[k] + t_window / 2.0; + if (t[k_fwd] > t_cutoff) { + break; + } else { + min_v_fwd = std::min(min_v_fwd, v_fwd); + } + } + size_t peak_idx_fwd = k_fwd - 1; + + // Flatten the peak by forcing intermediate values to the larger of the edge values + // This ensures we don't create dips below the troughs + double edge_val = std::max(min_v_bwd, min_v_fwd); + + for (size_t j = peak_idx_bwd; j <= peak_idx_fwd; ++j) { + if (smoothing.ignore[j]) { + return -1; // Error: Cannot smooth an ignored segment + } + + // Flatten the peak + if (v_smooth[j] > edge_val) { + v_smooth[j] = edge_val; + } + } + + // Update k to skip past the smoothed region + k = std::max(peak_idx_fwd, k); + } + } + + // Update timebase for next iteration + // This is critical - as velocities change, the time profile must update + static const double LOCAL_VEL_EPSILON = 1e-6; + for (size_t k = 1; k < v_smooth.size(); ++k) { + // Average velocity over this segment + double v_avg_clamped = std::max((v_smooth[k] + v_smooth[k-1]) / 2.0, LOCAL_VEL_EPSILON); + + // Distance for this segment + double ds_new = smoothing.ds[k]; + + // Time = distance / velocity + double dt_new = ds_new / v_avg_clamped; + + // Accumulate time + smoothing.t[k] = smoothing.t[k-1] + dt_new; + } + + return 0; +} + +/** + * @brief Copy Ruckig profile to TC_STRUCT storage + * + * Copies Ruckig's phase boundaries directly. These accurately represent the + * 7-phase jerk-limited trajectory structure. + */ +static void copyRuckigProfile(ruckig::Trajectory<1> &traj, ruckig_profile_t *profile) +{ + // Invalidate profile BEFORE writing data — prevents RT from sampling + // a partially-written (torn) profile. RT checks valid with acquire + // semantics and falls back to trapezoidal when invalid. + __atomic_store_n(&profile->valid, 0, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + + double traj_duration = traj.get_duration(); + + if (traj_duration < 1e-9) { + profile->duration = traj_duration; + return; // valid already 0 + } + + const auto& profiles = traj.get_profiles(); + if (profiles.empty() || profiles[0].empty()) { + profile->valid = 0; + return; + } + const auto& ruckig_prof = profiles[0][0]; + + // 9-phase layout: phases 0-1 = brake pre-trajectory, phases 2-8 = main S-curve. + // Brake brings initial velocity/acceleration within limits before main profile. + // Each phase uses native jerk from Ruckig's arrays (not derived from acceleration). + double brake_dur = ruckig_prof.brake.duration; + profile->duration = traj_duration; + + // --- Phase durations --- + // Phases 0-1: brake pre-trajectory (zero-duration when no brake) + if (brake_dur > 1e-12) { + profile->t[0] = ruckig_prof.brake.t[0]; + profile->t[1] = (ruckig_prof.brake.t[1] > 0.0) ? ruckig_prof.brake.t[1] : 0.0; + } else { + profile->t[0] = 0.0; + profile->t[1] = 0.0; + } + // Phases 2-8: main 7-phase S-curve + for (int i = 0; i < 7; i++) { + profile->t[2 + i] = ruckig_prof.t[i]; + } + + // --- Cumulative times --- + profile->t_sum[0] = profile->t[0]; + for (int i = 1; i < RUCKIG_PROFILE_PHASES; i++) { + profile->t_sum[i] = profile->t_sum[i - 1] + profile->t[i]; + } + + // --- Native jerk values (NOT derived from acceleration differences) --- + // This is the key fix: brake phases get brake.j[], main phases get prof.j[] + if (brake_dur > 1e-12) { + profile->j[0] = ruckig_prof.brake.j[0]; + profile->j[1] = (ruckig_prof.brake.t[1] > 0.0) ? ruckig_prof.brake.j[1] : 0.0; + } else { + profile->j[0] = 0.0; + profile->j[1] = 0.0; + } + for (int i = 0; i < 7; i++) { + profile->j[2 + i] = ruckig_prof.j[i]; + } + + // --- Boundary p/v/a via traj.at_time() (guaranteed correct) --- + std::array pos, vel, acc; + traj.at_time(0.0, pos, vel, acc); + profile->p[0] = pos[0]; + profile->v[0] = vel[0]; + profile->a[0] = acc[0]; + + for (int i = 0; i < RUCKIG_PROFILE_PHASES; i++) { + traj.at_time(profile->t_sum[i], pos, vel, acc); + profile->p[i + 1] = pos[0]; + profile->v[i + 1] = vel[0]; + profile->a[i + 1] = acc[0]; + } + + // Increment generation counter so RT can detect profile swap + // (stopwatch reset mechanism — mirrors manageBranches pattern) + int gen = __atomic_load_n(&profile->generation, __ATOMIC_RELAXED); + __atomic_store_n(&profile->generation, gen + 1, __ATOMIC_RELAXED); + + // All data written — publish to RT with release semantics + __atomic_thread_fence(__ATOMIC_SEQ_CST); + __atomic_store_n(&profile->valid, 1, __ATOMIC_RELEASE); +} + +/** + * @brief Parameters for computing and storing a Ruckig profile on a TC segment + */ +struct RuckigProfileParams { + double v_entry, v_exit; + double max_vel, max_acc, max_jrk; + double target_dist; + double feed_scale, vel_limit, vLimit; + double desired_fvel; +}; + +/** + * @brief Compute a Ruckig profile and store it on a TC segment + * + * Sets up Ruckig input from the given parameters, runs the solver, and on + * success writes metadata + profile data to tc->shared_9d.profile. On failure, + * marks the profile invalid. + * + * Callers still handle velocity scaling, reachability capping, and + * propagation of exit velocities — this just does the Ruckig call + storage. + * + * @param tc Segment to store the profile on + * @param p Profile parameters (velocities, limits, metadata) + * @return true on success, false on Ruckig failure (profile marked invalid) + */ +static bool computeAndStoreProfile(TC_STRUCT *tc, const RuckigProfileParams &p) +{ + static ruckig::Ruckig<1> otg(g_handoff_config.servo_cycle_time_sec); + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + input.current_position = {0.0}; + input.current_velocity = {p.v_entry}; + input.current_acceleration = {0.0}; + input.target_position = {p.target_dist}; + input.target_velocity = {p.v_exit}; + input.target_acceleration = {0.0}; + input.max_velocity = {p.max_vel}; + input.max_acceleration = {p.max_acc}; + input.max_jerk = {p.max_jrk}; + + auto result = otg.calculate(input, traj); + + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { + tc->shared_9d.profile.computed_feed_scale = p.feed_scale; + tc->shared_9d.profile.computed_vel_limit = p.vel_limit; + tc->shared_9d.profile.computed_vLimit = p.vLimit; + tc->shared_9d.profile.computed_desired_fvel = p.desired_fvel; + tc->shared_9d.profile.locked = (result == ruckig::Result::Working) ? 1 : 0; + copyRuckigProfile(traj, &tc->shared_9d.profile); + return true; + } else { + __atomic_store_n(&tc->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + return false; + } +} + +/** + * @brief Find minimum achievable exit velocity without overshooting + * + * Given current kinematic state and remaining distance, find the minimum + * velocity we can reach at the target position without overshooting. + * + * This is used for the "achievable feed" cascade: when a segment is too + * short to achieve the requested feed override, we apply what's achievable + * and let the next segment continue the deceleration. + * + * Algorithm: O(1) Ruckig + O(log n) time-domain search + * 1. Compute ONE velocity-control trajectory (decel from current to target) + * 2. Check if total distance <= remaining_dist (target achievable) + * 3. If not, binary search TIME on the trajectory to find velocity at remaining_dist + * + * This replaces the previous O(15 * Ruckig) binary search with O(1 * Ruckig + 20 samples). + * + * Phase 4 TODO: With segment blending, this exit velocity becomes the + * entry velocity constraint for the next segment, enabling efficient + * cascaded deceleration without the jerk ramp-up/ramp-down overhead + * at each segment boundary. + * + * @param current_vel Current velocity (may exceed target max) + * @param current_acc Current acceleration + * @param remaining_dist Distance to segment end + * @param max_accel Maximum acceleration + * @param max_jerk Maximum jerk + * @param max_vel Maximum velocity (profile velocity limit, i.e. new_max_vel) + * @param target_vel Target exit velocity (e.g., from requested feed scale) + * @return Achievable exit velocity closest to target_vel given remaining distance. + * For deceleration: >= target_vel (can't slow down enough → higher). + * For acceleration: <= target_vel (can't speed up enough → lower). + */ +static double findAchievableExitVelocity(double current_vel, double current_acc, + double remaining_dist, + double max_accel, double max_jerk, + double max_vel, + double target_vel) +{ + if (remaining_dist <= 1e-9) return current_vel; // No room to change velocity + + try { + static ruckig::Ruckig<1> otg(g_handoff_config.servo_cycle_time_sec); + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + // Use position-control mode — same as the actual branch profile. + // This correctly accounts for the dual constraint (reach position AND velocity). + input.control_interface = ruckig::ControlInterface::Position; + input.current_position = {0.0}; + input.current_velocity = {current_vel}; + input.current_acceleration = {current_acc}; + input.target_position = {remaining_dist}; + input.target_velocity = {target_vel}; + input.target_acceleration = {0.0}; + input.max_velocity = {max_vel}; + input.max_acceleration = {max_accel}; + input.max_jerk = {max_jerk}; + + auto result = otg.calculate(input, traj); + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { + // Verify the profile doesn't reverse direction + double duration = traj.get_duration(); + std::array p, v, a; + bool reverses = false; + for (int i = 1; i <= 16; i++) { + traj.at_time(duration * i / 16, p, v, a); + if (v[0] < -0.01) { reverses = true; break; } + } + if (!reverses) return target_vel; // Feasible without reversal + } + + // Target not feasible — binary search on exit velocity + double v_lo, v_hi; + if (current_vel <= target_vel) { + v_lo = current_vel; // safe: no change needed + v_hi = target_vel; // desired but infeasible + } else { + v_lo = target_vel; // desired but infeasible + v_hi = current_vel; // safe: no change needed + + // Validate that v_hi (current_vel) is actually feasible. + // When current_acc is large and negative, even maintaining + // current velocity requires reversal — v_hi is NOT safe. + input.target_velocity = {v_hi}; + result = otg.calculate(input, traj); + bool v_hi_ok = false; + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { + v_hi_ok = true; + std::array pv, vv, av; + double dur = traj.get_duration(); + for (int i = 1; i <= 8; i++) { + traj.at_time(dur * i / 8, pv, vv, av); + if (vv[0] < -0.01) { v_hi_ok = false; break; } + } + } + if (!v_hi_ok) { + // current_vel also reverses — search between 0 and current_vel + v_lo = 0.0; + v_hi = current_vel; + } + } + + // Analytical upper bound: max velocity reachable under constant max_accel + // (ignores jerk, so always an overestimate). Tightens bracket for short segments. + double v_energy = sqrt(current_vel * current_vel + 2.0 * max_accel * remaining_dist); + if (v_energy < v_hi) v_hi = v_energy; + + for (int iter = 0; iter < 20; iter++) { + if (v_hi - v_lo < 0.5) break; // 0.5 mm/s precision sufficient (callers use 0.01 tolerance) + double v_mid = (v_lo + v_hi) * 0.5; + input.target_velocity = {v_mid}; + + result = otg.calculate(input, traj); + bool feasible = false; + if (result == ruckig::Result::Working || result == ruckig::Result::Finished) { + double duration = traj.get_duration(); + std::array p, v, a; + feasible = true; + for (int i = 1; i <= 8; i++) { + traj.at_time(duration * i / 8, p, v, a); + if (v[0] < -0.01) { feasible = false; break; } + } + } + + if (current_vel <= target_vel) { + // Acceleration: feasible → try higher, infeasible → go lower + if (feasible) v_lo = v_mid; else v_hi = v_mid; + } else { + // Deceleration: feasible → try lower, infeasible → go higher + if (feasible) v_hi = v_mid; else v_lo = v_mid; + } + } + + // Return the conservative bound (last known feasible) + double ret; + if (current_vel <= target_vel) { + ret = v_lo; // Highest confirmed feasible (acceleration) + } else { + ret = v_hi; // Lowest confirmed feasible (deceleration) + } + return ret; + + } catch (std::exception &e) { + rtapi_print_msg(RTAPI_MSG_ERR, "findAchievableExitVelocity exception: %s", e.what()); + return current_vel; + } catch (...) { + rtapi_print_msg(RTAPI_MSG_ERR, "findAchievableExitVelocity unknown exception"); + return current_vel; + } +} + +/** + * @brief Compute kinematic-aware handoff margin for feed override changes + * + * Instead of a fixed margin, compute the minimum safe margin based on the + * kinematic state and required velocity change. This provides: + * - Fast response for small velocity changes (small margin) + * - Adequate runway for large velocity changes (larger margin) + * + * The margin accounts for: + * 1. Base RT timing jitter (~5-10ms) + * 2. Time needed for jerk-limited velocity transition + * 3. Safety factor for profile stabilization + * + * For jerk-limited motion, braking time is approximately: + * t_brake ≈ max(2*sqrt(Δv/j), Δv/a) + * + * where Δv = current_vel - target_vel, j = max_jerk, a = max_accel + * + * @param current_vel Current velocity at estimated handoff point + * @param target_vel Target velocity after feed change (vel_limit * new_feed_scale) + * @param max_accel Maximum acceleration limit + * @param max_jerk Maximum jerk limit + * @return Minimum safe handoff margin in seconds + */ +static double computeKinematicHandoffMargin(double current_vel, double target_vel, + double max_accel, double max_jerk) +{ + // All timing constants derived from servo cycle time: + // 1 cycle — RT detects branch (polls branch->valid each cycle) + // 1 cycle — RT takes branch and starts executing new profile + // 1 cycle — elapsed_time measurement jitter (read vs actual) + // = 3 cycles base margin for the handoff mechanics. + double servo_sec = g_handoff_config.servo_cycle_time_sec; + double base_margin = servo_sec * 3; + double min_margin = base_margin; + // Cap: half the branch window — beyond this we're waiting too long + // and should defer to the next segment instead. + double max_margin = g_handoff_config.branch_window_ms / 2000.0; + if (max_margin < min_margin * 2) max_margin = min_margin * 2; + + // If velocity is already at or below target, minimal margin needed + if (current_vel <= target_vel + 0.01) { + return min_margin; + } + + // Delta velocity that needs to be braked + double delta_v = current_vel - target_vel; + + // Estimate braking time under jerk-limited motion + // Two regimes: + // 1. Jerk-dominated (short brakes): t ≈ 2*sqrt(Δv/j) + // 2. Accel-limited (long brakes): t ≈ Δv/a + a/j + double t_jerk_dominated = 2.0 * sqrt(delta_v / max_jerk); + double t_accel_limited = delta_v / max_accel + max_accel / max_jerk; + double t_brake_estimate = fmax(t_jerk_dominated, t_accel_limited); + + // Margin = base + fraction of brake time for stabilization + // The 0.5 factor accounts for profile needing time to "settle" after + // the main deceleration phase (final jerk phase + numerical settling) + double kinematic_margin = base_margin + t_brake_estimate * 0.5; + + // Clamp to reasonable bounds + double result = fmax(min_margin, fmin(max_margin, kinematic_margin)); + + return result; +} + +/** + * @brief Check whether a segment has enough remaining time for computeBranch() + * to place a handoff point. + * + * Uses the worst-case handoff margin (the cap from computeKinematicHandoffMargin) + * so the answer is conservative: if this returns false, computeBranch() will + * certainly REJECT(seg_done). If it returns true, computeBranch() may still + * reject for other reasons, but at least the margin isn't the blocker. + * + * @param tc Active segment to check + * @return true if the segment has enough remaining time for a branch attempt + */ +static bool segmentHasBranchRoom(const TC_STRUCT *tc) +{ + if (!tc || !tc->shared_9d.profile.valid) + return false; + + double elapsed = atomicLoadDouble(&tc->elapsed_time); + double duration = tc->shared_9d.profile.duration; + double remaining = duration - elapsed; + if (remaining < 1e-6) + return false; + + // Worst-case margin: same derivation as computeKinematicHandoffMargin's cap + double servo_sec = g_handoff_config.servo_cycle_time_sec; + double worst_margin = g_handoff_config.branch_window_ms / 2000.0; + if (worst_margin < servo_sec * 6) + worst_margin = servo_sec * 6; + + return remaining > worst_margin; +} + +//============================================================================ +// PREDICTIVE HANDOFF IMPLEMENTATION +//============================================================================ + +/** + * @brief Set predictive handoff configuration parameters + * + * Called from initraj.cc after parsing INI file. + */ +extern "C" void setHandoffConfig(double handoff_horizon_ms, + double branch_window_ms, + double min_buffer_time_ms, + double target_buffer_time_ms, + double max_buffer_time_ms, + double feed_override_debounce_ms, + double servo_cycle_time_sec, + double default_max_jerk) +{ + g_handoff_config.handoff_horizon_ms = handoff_horizon_ms; + g_handoff_config.branch_window_ms = branch_window_ms; + g_handoff_config.min_buffer_time_ms = min_buffer_time_ms; + g_handoff_config.target_buffer_time_ms = target_buffer_time_ms; + g_handoff_config.max_buffer_time_ms = max_buffer_time_ms; + g_handoff_config.feed_override_debounce_ms = feed_override_debounce_ms; + g_handoff_config.servo_cycle_time_sec = servo_cycle_time_sec; + g_handoff_config.default_max_jerk = default_max_jerk; + + rtapi_print_msg(RTAPI_MSG_DBG, + "Predictive handoff config: horizon=%.0fms, window=%.0fms, buffer=[%.0f,%.0f,%.0f]ms," + "debounce=%.0fms, servo_period=%.3fms, default_jerk=%.0f\n", + g_handoff_config.handoff_horizon_ms, + g_handoff_config.branch_window_ms, + g_handoff_config.min_buffer_time_ms, + g_handoff_config.target_buffer_time_ms, + g_handoff_config.max_buffer_time_ms, + g_handoff_config.feed_override_debounce_ms, + g_handoff_config.servo_cycle_time_sec * 1000.0, + g_handoff_config.default_max_jerk); +} + +double getDefaultMaxJerk() +{ + return g_handoff_config.default_max_jerk; +} + +/** + * @brief Get minimum buffer time threshold + */ +extern "C" double getBufferMinTimeMs(void) +{ + return g_handoff_config.min_buffer_time_ms; +} + +/** + * @brief Get target buffer time threshold + */ +extern "C" double getBufferTargetTimeMs(void) +{ + return g_handoff_config.target_buffer_time_ms; +} + +/** + * @brief Initialize predictive handoff system + * + * Note: Configuration should be set first via setHandoffConfig() from initraj.cc. + * If not called, defaults from PredictiveHandoffConfig constructor are used. + */ +extern "C" int initPredictiveHandoff(void) +{ + g_last_feed_scale = 1.0; + g_last_replan_time_ms = 0.0; + g_recompute_cursor = 0; + g_recompute_feed_scale = 1.0; + g_recompute_rapid_scale = 1.0; + g_recompute_first_batch_done = false; + g_committed_feed = -1.0; // Initialized from live on first computeRuckigProfiles_9D call + g_committed_rapid = -1.0; + g_next_feed_scale = -1.0; + g_next_rapid_scale = -1.0; + g_commit_segment = 1; + g_segments_per_tick = 3.0; + return 0; +} + +/** + * @brief Calculate how much buffered motion time remains + * + * Sum of remaining time in active segment + all queued segments. + */ +extern "C" double calculateBufferTimeMs(TP_STRUCT *tp) +{ + if (!tp) return 0.0; + + double buffer_time = 0.0; + TC_QUEUE_STRUCT *queue = &tp->queue; + + int queue_len = tcqLen_user(queue); + for (int i = 0; i < queue_len; i++) { + TC_STRUCT *tc = tcqItem_user(queue, i); + if (!tc) continue; + + if (i == 0) { + // Active segment: remaining time + double elapsed = atomicLoadDouble(&tc->elapsed_time); + if (tc->shared_9d.profile.valid) { + double remaining = tc->shared_9d.profile.duration - elapsed; + if (remaining > 0) buffer_time += remaining; + } + } else { + // Queued segment: full duration (if profile valid) + if (tc->shared_9d.profile.valid) { + buffer_time += tc->shared_9d.profile.duration; + } else { + // Estimate from distance/velocity if profile not yet computed + double est_vel = tc->reqvel > 0 ? tc->reqvel : 1.0; + double est_time = tc->target / est_vel; + buffer_time += est_time; + } + } + } + + return buffer_time * 1000.0; // Return milliseconds +} + +/** + * @brief Predict kinematic state at a future time + * + * Uses sequence number protocol to detect torn reads during RT branch handoff. + * RT increments copy_sequence before/after profile copy (odd = in progress). + * If sequence is odd or changes during our read, we retry. + */ +PredictedState predictStateAtTime(TC_STRUCT *tc, double target_elapsed_time) +{ + PredictedState state; + + if (!tc || !tc->shared_9d.profile.valid) { + return state; + } + + // Retry loop to handle torn reads during branch handoff + // RT uses sequence counter: odd = copy in progress, even = stable + for (int attempts = 0; attempts < 5; attempts++) { + // Read sequence BEFORE reading any profile data + unsigned int seq_before = __atomic_load_n(&tc->shared_9d.copy_sequence, __ATOMIC_ACQUIRE); + + // If sequence is odd, RT is mid-copy - spin briefly and retry + if (seq_before & 1) { + for (int i = 0; i < 100; i++) { + __asm__ volatile("" ::: "memory"); // Prevent loop optimization + } + continue; + } + + // Read position base atomically + double pos_base = atomicLoadDouble(&tc->position_base); + + // Memory barrier to ensure profile is read after pos_base and sequence + __atomic_thread_fence(__ATOMIC_ACQUIRE); + + // Make a local copy of the profile to avoid torn reads during sampling + ruckig_profile_t profile_copy = tc->shared_9d.profile; + + // Memory barrier before checking sequence again + __atomic_thread_fence(__ATOMIC_ACQUIRE); + + // Read sequence AFTER reading profile data + unsigned int seq_after = __atomic_load_n(&tc->shared_9d.copy_sequence, __ATOMIC_ACQUIRE); + + // If sequence changed, we got torn data - retry + if (seq_before != seq_after) { + continue; + } + + // Consistent read - use the local copy for sampling + double sample_time = target_elapsed_time; + if (sample_time > profile_copy.duration) { + sample_time = profile_copy.duration; + } + + // Sample from local copy (avoids any further torn read risk) + double p, v, a, j; + int ok = ruckigProfileSample(&profile_copy, sample_time, &p, &v, &a, &j); + + if (ok == 0) { + state.position = pos_base + p; + state.velocity = v; + state.acceleration = a; + state.jerk = j; + state.valid = true; + } + return state; + } + + // Failed to get consistent read after retries - return invalid state + // This is extremely rare and indicates heavy contention + return state; +} + +/** + * @brief Validate a profile before committing + * + * Checks that the profile is well-formed and has expected terminal conditions. + * Rejects corrupted profiles that could cause velocity explosions. + * + * @param profile The profile to validate + * @param expected_final_vel Expected terminal velocity (0 for position control, + * target_vel for velocity control brake profiles) + * @return true if profile is valid, false otherwise + */ +static bool validateProfile(const ruckig_profile_t *profile, double expected_final_vel = 0.0) +{ + if (!profile || !profile->valid) return false; + + // Terminal velocity must match expected (with tolerance) + // For position control: expected ~0 + // For velocity control (brake): expected = target velocity + // TODO Phase 4: For blending, expected_final_vel = blend velocity into next segment + const double VEL_TOLERANCE = 0.5; // mm/s - slightly larger for velocity control + double v_final = profile->v[RUCKIG_PROFILE_PHASES]; + if (fabs(v_final - expected_final_vel) > VEL_TOLERANCE) return false; + + // Terminal acceleration must be ~0 + const double ACC_TOLERANCE = 10.0; // mm/s^2 + double a_final = profile->a[RUCKIG_PROFILE_PHASES]; + if (fabs(a_final) > ACC_TOLERANCE) return false; + + // Duration must be positive + if (profile->duration <= 0.0) return false; + + return true; +} + +/** + * @brief Commit a branch for RT to take + * + * Branch/Merge protocol: + * - Userspace writes branch data and sets valid=1 + * - RT takes branch when elapsed_time reaches handoff_time (if before window_end) + * - RT sets taken=1 when it takes the branch + * - Userspace merges (sets canonical_feed_scale) when it sees taken=1 + * - Userspace clears valid=0 and taken=0 after merge + * + * Two-stage support: + * - If brake_profile is provided, RT executes brake first, then main profile + * - brake_end_position marks where brake ends and main begins + * + * @return true if committed, false if previous branch still pending + */ +bool commitBranch(shared_optimization_data_9d_t *shared, + const ruckig_profile_t *main_profile, + const ruckig_profile_t *brake_profile, // NULL if no brake needed + double brake_target_vel, // Expected final velocity of brake (0 if no brake) + double brake_end_position, // Position after brake completes + double handoff_time, + double handoff_position, + double feed_scale, + double window_end_time, + double expected_exit_vel) // Expected final velocity of main profile +{ + if (!shared || !main_profile) return false; + + // Validate main profile exit velocity + // For tangent transitions, exit velocity is non-zero (final_vel * feed_scale) + if (!validateProfile(main_profile, expected_exit_vel)) { + return false; + } + + // Validate brake profile - should end at target velocity (velocity control) + if (brake_profile && !validateProfile(brake_profile, brake_target_vel)) { + return false; + } + + // Check branch state + int valid = __atomic_load_n(&shared->branch.valid, __ATOMIC_ACQUIRE); + int taken = __atomic_load_n(&shared->branch.taken, __ATOMIC_ACQUIRE); + + if (taken) { + // Branch is being executed by RT - don't overwrite. + // During two-stage brake: merge sets valid=0 but keeps taken=1. + // RT reads branch.profile and brake_end_position at brake→main + // transition — overwriting causes position/profile mismatch. + return false; + } + + // If previous branch is pending (valid && !taken), invalidate and overwrite + // This handles rapid feed changes - newer feed always wins + if (valid && !taken) { + __atomic_store_n(&shared->branch.valid, 0, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + } + + // Write all branch data (not yet visible to RT) + shared->branch.profile = *main_profile; + shared->branch.handoff_time = handoff_time; + shared->branch.handoff_position = handoff_position; + shared->branch.feed_scale = feed_scale; + shared->branch.window_end_time = window_end_time; + + // Two-stage brake handling + if (brake_profile) { + shared->branch.brake_profile = *brake_profile; + shared->branch.has_brake = 1; + shared->branch.brake_end_position = brake_end_position; + } else { + shared->branch.has_brake = 0; + shared->branch.brake_end_position = 0.0; + } + + // Clear flags (in case they were set from previous branch) + __atomic_store_n(&shared->branch.taken, 0, __ATOMIC_RELEASE); + __atomic_store_n(&shared->branch.brake_done, 0, __ATOMIC_RELEASE); + + // Full memory barrier + __atomic_thread_fence(__ATOMIC_RELEASE); + + // Set valid flag last - makes everything above visible atomically + __atomic_store_n(&shared->branch.valid, 1, __ATOMIC_RELEASE); + + return true; +} + +/** + * @brief Estimate the commit point for a feed override change. + * + * Finds the earliest queue index where we can guarantee all segments from + * there forward will be recomputed before RT arrives. If the active segment + * has enough remaining time for a handoff (adaptive horizon + 3 servo cycles), + * returns 1 (branch on active). Otherwise scans forward comparing RT arrival + * time vs our estimated recompute time. + * + * Segment durations are scaled by the ratio of their computed feed to the + * new requested feed, so RT arrival estimates reflect the actual speed + * segments will run at after recomputation. + */ +static int estimateCommitSegment(TP_STRUCT *tp, double new_feed) +{ + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + if (queue_len < 3) return 1; // short queue: start from active+1 + + TC_STRUCT *active = tcqItem_user(queue, 0); + if (!active) return 1; + + // Determine how much time RT needs to finish the active segment. + // The train is "parked" if: no profile, zero duration, or schedule expired. + // In all cases, treat as infinite arrival time → branch on active (return 1). + double elapsed = atomicLoadDouble(&active->elapsed_time); + double active_remaining = 0.0; + if (active->shared_9d.profile.valid && active->shared_9d.profile.duration > 1e-6) { + active_remaining = active->shared_9d.profile.duration - elapsed; + if (active_remaining < 0) active_remaining = 0; + // NOTE: Do NOT scale active_remaining by feed ratio here. + // The branch hasn't happened yet — RT is still running the old profile + // at the old speed. The wall-clock time until this segment ends is + // exactly (duration - elapsed), unscaled. Downstream segments (below) + // ARE scaled because they'll be recomputed at new_feed. + } else { + return 1; // no schedule → train is parked, must branch on active + } + if (active_remaining < 1e-6) return 1; // schedule expired → branch on active + + // Quick check: does the active segment have enough remaining time for + // computeBranch() to place a handoff? Uses worst-case margin so we + // don't send segments that will certainly be rejected. + if (segmentHasBranchRoom(active)) { + return 1; + } + + double servo_sec = g_handoff_config.servo_cycle_time_sec; + double cycle_time = tp->cycleTime > 0 ? tp->cycleTime : servo_sec; + // Safety margin: 10 servo cycles to absorb timing jitter between + // userspace computation and RT execution. + double margin = servo_sec * 10; + double spt = g_segments_per_tick > 0.5 ? g_segments_per_tick : 1.0; + + double rt_time = active_remaining; // cumulative time until RT reaches segment i + + for (int i = 1; i < queue_len; i++) { + int segs_to_buffer = queue_len - i; + double recompute_time = (segs_to_buffer / spt) * cycle_time + margin; + + if (rt_time > recompute_time) { + // We're in this scan because the active segment failed + // segmentHasBranchRoom — it's too short for a branch. + // Never return 1 here: manageBranches would interpret it as + // "branch on active", which will certainly fail. Return ≥2 + // to force deferral; the active finishes in <25ms and the + // next segment becomes branchable. + int commit = (i >= 2) ? i : 2; + return commit; + } + + // Add segment i's duration to RT arrival time. + // Scale by feed ratio: the stored duration reflects the old feed, but + // after recompute these segments will run at new_feed. + TC_STRUCT *tc = tcqItem_user(queue, i); + if (tc && tc->shared_9d.profile.valid && tc->shared_9d.profile.duration > 1e-6) { + double duration = tc->shared_9d.profile.duration; + double seg_feed = tc->shared_9d.profile.computed_feed_scale; + if (seg_feed > 0.001 && new_feed > 0.001) { + duration *= seg_feed / new_feed; + } + rt_time += duration; + } else { + return i; // blank timetable → train won't pass here soon, commit here + } + } + + return queue_len - 1; // fallback: commit at last segment +} + +/** + * @brief Invalidate next N segments for cascade re-optimization + */ +void invalidateNextNSegments(TP_STRUCT *tp, int n, int start_index) +{ + if (!tp || n <= 0) return; + + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + int invalidated = 0; + + // Invalidate n segments starting from start_index (default: skip active at 0) + for (int i = start_index; i < queue_len && invalidated < n; i++) { + TC_STRUCT *tc = tcqItem_user(queue, i); + if (!tc) continue; + + // Mark for re-optimization + // DON'T invalidate profile.valid immediately! + // Let RT use old profile until new one is ready + __atomic_store_n((int*)&tc->shared_9d.optimization_state, + TC_PLAN_UNTOUCHED, __ATOMIC_RELEASE); + + invalidated++; + } +} + +/** + * @brief Synchronously recompute Ruckig profiles for downstream segments + * + * Called immediately after a branch is committed or merged, so that downstream + * segments have fresh profiles before RT reaches them. This eliminates the race + * condition where RT starts executing a stale profile (computed at old feed scale) + * before the periodic optimizer gets around to recomputing it. + * + * Time-budgeted: computes as many segments as fit within the time budget + * (half the servo cycle), returns the number actually recomputed so the + * cursor walk can continue from where this left off. + * + * @param tp Trajectory planner + * @param snap_feed Snapshotted feed_scale (for feed moves) + * @param snap_rapid Snapshotted rapid_scale (for traverses) + * @param start_index Queue index to start from (default 1 = first after active) + * @return Number of segments recomputed + */ +static int recomputeDownstreamProfiles(TP_STRUCT *tp, + double snap_feed, double snap_rapid, + int start_index = 1) +{ + if (!tp) return 0; + + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + if (start_index >= queue_len) return 0; + + // Run backward pass at the new feed before writing profiles. + // Without this, final_vel values reflect the OLD feed's backward pass, + // and the optimizer will rewrite our profiles with different velocities + // once it runs its own backward pass at the new committed feed. + int depth = queue_len; + if (depth > MAX_LOOKAHEAD_DEPTH) depth = MAX_LOOKAHEAD_DEPTH; + computeLimitingVelocities_9D(queue, depth, g_smoothing_data); + applyLimitingVelocities_9D(queue, g_smoothing_data, depth); + + // Seed entry velocity from the predecessor of start_index. + // Always use final_vel * new_feed rather than reading the predecessor's + // profile exit velocity. The predecessor's profile may still be at the OLD + // feed (branch not yet taken by RT, or commit-point predecessor not yet + // rewritten by optimizer). final_vel is feed-independent (backward pass + // computes it at feed=1.0), so final_vel * new_feed predicts the exit + // velocity that will be achieved at the new feed. + TC_STRUCT *prev = tcqItem_user(queue, start_index - 1); + if (!prev) return 0; + + double prev_exit_vel_scaled; + if (prev->term_cond == TC_TERM_COND_TANGENT) { + double prev_feed = tpGetSnapshotFeedScale(prev, snap_feed, snap_rapid); + double final_vel = atomicLoadDouble(&prev->shared_9d.final_vel); + prev_exit_vel_scaled = final_vel * prev_feed; + // Cap by kink_vel if set + if (prev->kink_vel > 0) + prev_exit_vel_scaled = fmin(prev_exit_vel_scaled, prev->kink_vel); + // On the merge path (start_index == 1), the branch has been taken + // by RT. achieved_exit_vel IS the actual profile exit velocity — + // use it directly instead of the final_vel * feed estimate. + if (start_index == 1 && prev->shared_9d.achieved_exit_vel > 0) { + prev_exit_vel_scaled = prev->shared_9d.achieved_exit_vel; + } + } else { + prev_exit_vel_scaled = 0.0; + } + + // Get default jerk + double default_jerk = prev->maxjerk > 0 ? prev->maxjerk : g_handoff_config.default_max_jerk; + if (default_jerk < 1.0) default_jerk = g_handoff_config.default_max_jerk; + + int recomputed = 0; + double budget_us = g_handoff_config.servo_cycle_time_sec * 0.5e6; // half servo cycle + double tick_start = etime_user(); + for (int i = start_index; i < queue_len; i++) { + // Time-budget check: after minimum 3 segments, stop if budget exhausted + // AND we've reached a safe endpoint (next segment can handle our exit). + // Without the safe-endpoint check, stopping mid-chain leaves downstream + // segments with stale profiles that expect different entry velocities. + // Hard cap at 32 segments to prevent unbounded computation. + if (recomputed >= 32) break; + if (recomputed >= 3) { + double elapsed_us = (etime_user() - tick_start) * 1e6; + if (elapsed_us >= budget_us) { + // Only stop if previous segment's exit is safe for this one + if (canStopChainHere(tp, queue, i - 1, prev_exit_vel_scaled, + snap_feed, snap_rapid, default_jerk)) + break; + } + } + TC_STRUCT *tc = tcqItem_user(queue, i); + if (!tc || tc->target < 1e-9) { + prev_exit_vel_scaled = 0.0; + continue; + } + + // Per-segment feed scale from snapshots + double feed_scale = tpGetSnapshotFeedScale(tc, snap_feed, snap_rapid); + + // Entry velocity comes from previous segment's exit (already scaled) + double scaled_v_entry = prev_exit_vel_scaled; + + // Get previous segment's kink_vel for entry cap check + TC_STRUCT *prev_tc = (i > 0) ? tcqItem_user(queue, i - 1) : NULL; + double prev_kink = (prev_tc && prev_tc->kink_vel > 0) ? prev_tc->kink_vel : -1.0; + + // Exit velocity: unscaled final_vel, scale it now + double v_exit_unscaled = (tc->term_cond == TC_TERM_COND_TANGENT) + ? atomicLoadDouble(&tc->shared_9d.final_vel) : 0.0; + + // Store entry velocity for RT layer (unscaled) + double unscaled_entry = (feed_scale > 0.001) ? scaled_v_entry / feed_scale : 0.0; + atomicStoreDouble(&tc->shared_9d.entry_vel, unscaled_entry); + + double vel_limit = getEffectiveVelLimit(tp, tc); + double max_jrk = tc->maxjerk > 0 ? tc->maxjerk : default_jerk; + + if (feed_scale < 0.001) { + // Don't clobber a valid non-hold profile with a feed-hold + // profile. Same guard as computeRuckig forward/backward. + if (tc->shared_9d.profile.valid && + tc->shared_9d.profile.computed_feed_scale > 0.001) { + prev_exit_vel_scaled = 0.0; + continue; + } + createFeedHoldProfile(tc, vel_limit, tp->vLimit, "recomputeDownstream"); + prev_exit_vel_scaled = 0.0; + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + } else { + double max_vel = applyVLimit(tp, tc, vel_limit * feed_scale); + double max_acc = tcGetTangentialMaxAccel_9D_user(tc); + + scaled_v_entry = fmin(scaled_v_entry, max_vel); + + // Cap entry velocity by previous segment's kink limit + if (prev_kink > 0) + scaled_v_entry = fmin(scaled_v_entry, prev_kink); + + double scaled_v_exit = fmin(v_exit_unscaled * feed_scale, max_vel); + scaled_v_exit = applyKinkVelCap(scaled_v_exit, v_exit_unscaled, max_vel, tc->kink_vel); + double desired_fvel_for_profile = scaled_v_exit; + + // Reachability cap: ensure exit velocity is achievable from entry + // within segment distance (and vice versa). Without this, Ruckig + // returns Working (overshoot profiles) that cross the target position + // at an intermediate velocity much lower than the intended exit. + { + double v_fwd = jerkLimitedMaxEntryVelocity( + scaled_v_entry, tc->target, max_acc, max_jrk); + double v_bwd = jerkLimitedMaxEntryVelocity( + scaled_v_exit, tc->target, max_acc, max_jrk); + scaled_v_exit = fmin(scaled_v_exit, v_fwd); + scaled_v_entry = fmin(scaled_v_entry, v_bwd); + } + + // Skip if profile already matches: same feed, entry velocity, and exit target. + // Avoids redundant Ruckig solves when forward pass already wrote correct profiles. + if (tc->shared_9d.profile.valid && + fabs(tc->shared_9d.profile.computed_feed_scale - feed_scale) < 0.005 && + fabs(tc->shared_9d.profile.v[0] - scaled_v_entry) < 0.5 && + fabs(tc->shared_9d.profile.computed_desired_fvel - desired_fvel_for_profile) < 0.5) { + prev_exit_vel_scaled = tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + continue; + } + + try { + RuckigProfileParams rp = {scaled_v_entry, scaled_v_exit, + max_vel, max_acc, max_jrk, tc->target, + feed_scale, vel_limit, tp->vLimit, desired_fvel_for_profile}; + + if (computeAndStoreProfile(tc, rp)) { + prev_exit_vel_scaled = tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + + // One-step backtrack: fix backward reachability gap + // Skip active segment — rewriting triggers STOPWATCH_RESET + // with velocity discontinuity worse than the chain gap. + TC_STRUCT *prev_seg = (i > start_index) ? tcqItem_user(queue, i - 1) : NULL; + if (prev_seg && !prev_seg->active + && prev_seg->term_cond == TC_TERM_COND_TANGENT + && prev_seg->shared_9d.profile.valid) { + double prev_v_end = prev_seg->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double curr_v0 = tc->shared_9d.profile.v[0]; + if (prev_v_end > curr_v0 + 0.01) { + double p_entry = prev_seg->shared_9d.profile.v[0]; + double p_vel_limit = getEffectiveVelLimit(tp, prev_seg); + double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * feed_scale); + double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg); + double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk; + RuckigProfileParams rp_prev = {p_entry, curr_v0, + p_max_vel, p_max_acc, p_max_jrk, prev_seg->target, + feed_scale, p_vel_limit, tp->vLimit, curr_v0}; + if (computeAndStoreProfile(prev_seg, rp_prev)) { + atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state, + TC_PLAN_FINALIZED); + } + } + } + } else { + prev_exit_vel_scaled = 0.0; + rtapi_print_msg(RTAPI_MSG_ERR, "Downstream recompute fail seg=%d", + tc->id); + } + } catch (std::exception &e) { + __atomic_store_n(&tc->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + prev_exit_vel_scaled = 0.0; + rtapi_print_msg(RTAPI_MSG_ERR, "Downstream recompute exception seg=%d: %s", + tc->id, e.what()); + } + } + + recomputed++; + } + + return recomputed; +} + +/** + * @brief Check if a profile has negative velocity (backward motion) + * + * Scans the profile at uniform time intervals and returns true if any + * sample has velocity below the threshold. Used as a safety gate to + * prevent committing profiles that would drive the machine backward. + */ +static bool profileHasNegativeVelocity(const ruckig_profile_t *profile, double threshold = -0.01) { + if (!profile->valid) return false; + double dur = profile->duration; + if (dur < 1e-9) return false; + int n_pts = 40; + for (int i = 0; i <= n_pts; i++) { + double t = dur * i / n_pts; + double sp, sv, sa, sj; + ruckigProfileSample(const_cast(profile), t, &sp, &sv, &sa, &sj); + if (sv < threshold) return true; + } + return false; +} + +/** + * @brief Estimate achievable exit velocity at a given feed scale + * + * Lightweight estimator used by the feed-limiting binary search. + * Uses kinematic formulas for brake/stabilize distance estimation and + * Ruckig-based findAchievableExitVelocity for the main-stage exit. + * Does NOT build full profiles — just estimates whether a given feed + * can produce an exit velocity within the specified hard limit. + * + * @param tp Trajectory planner + * @param tc Current segment + * @param entry_vel Velocity at handoff point (from predicted state) + * @param entry_acc Acceleration at handoff point + * @param remaining_dist Distance from handoff to segment end + * @param vel_limit Base velocity limit (before feed scaling) + * @param feed_scale Feed scale to test + * @param default_jerk Default jerk limit + * @param exit_hard_limit Hard exit velocity constraint (kink or downstream cap) + * @return Estimated achievable exit velocity at this feed + */ +static double estimateExitAtFeed(TP_STRUCT const *tp, TC_STRUCT const *tc, + double entry_vel, double entry_acc, + double remaining_dist, + double vel_limit, double feed_scale, + double default_jerk, + double exit_hard_limit) +{ + double max_vel = applyVLimit(tp, tc, vel_limit * feed_scale); + + double eff_entry_vel = entry_vel; + double eff_entry_acc = entry_acc; + double eff_remaining = remaining_dist; + + // Determine if this feed would need a brake or stabilize stage + if (entry_vel > max_vel + 0.01) { + // Two-stage brake: estimate distance kinematically + double brake_dist = jerkLimitedBrakingDistance(entry_vel, max_vel, + tc->maxaccel, default_jerk); + eff_remaining = remaining_dist - brake_dist; + if (eff_remaining < 1e-6) return entry_vel; // Can't brake in time + eff_entry_vel = max_vel; + eff_entry_acc = 0.0; + } else if (fabs(entry_acc) > 0.5 * tc->maxaccel) { + // Stabilize: estimate acceleration-arrest distance + double stab_vel = fmin(entry_vel, max_vel); + if (stab_vel < 0.0) stab_vel = 0.0; + double t_arrest = fabs(entry_acc) / default_jerk; + double stab_dist = fmax(stab_vel * t_arrest, 0.0); + eff_remaining = remaining_dist - stab_dist; + if (eff_remaining < 1e-6) return entry_vel; + eff_entry_vel = stab_vel; + eff_entry_acc = 0.0; + } + + // Target: the hard limit or max_vel, whichever is lower + double target = fmin(exit_hard_limit, max_vel); + + return findAchievableExitVelocity(eff_entry_vel, eff_entry_acc, eff_remaining, + tc->maxaccel, default_jerk, max_vel, target); +} + +/** + * @brief Write an alternate entry profile for the next segment. + * + * When a brake branch on the current segment changes its exit velocity, + * the next segment's pre-computed profile has a stale entry velocity. + * This writes an alternate profile with v0 = brake's exit velocity. + * RT picks whichever profile (main or alt_entry) has v0 closer to the + * actual junction velocity — smooth regardless of whether brake was taken. + * + * Called BEFORE commitBranch so the cushion is in place before the jump. + */ +static void writeAltEntry(TP_STRUCT *tp, TC_STRUCT *tc, + double branch_exit_vel, double new_feed_scale) +{ + if (tc->term_cond != TC_TERM_COND_TANGENT) return; + + TC_STRUCT *next = tcqItem_user(&tp->queue, 1); + if (!next) return; + + // Skip if alt_entry wouldn't differ meaningfully from main profile + if (__atomic_load_n(&next->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + double main_v0 = next->shared_9d.profile.v[0]; + if (fabs(branch_exit_vel - main_v0) < 0.1) return; + } + + double next_vel_limit = getEffectiveVelLimit(tp, next); + double next_max_vel = next_vel_limit * new_feed_scale; + next_max_vel = applyVLimit(tp, next, next_max_vel); + double next_jerk = next->maxjerk > 0 ? next->maxjerk : g_handoff_config.default_max_jerk; + + // Determine target exit velocity for next segment + double unscaled_fv = atomicLoadDouble(&next->shared_9d.final_vel); + double target_exit_vel = (next->term_cond == TC_TERM_COND_TANGENT) + ? fmin(unscaled_fv * new_feed_scale, next_max_vel) + : 0.0; + target_exit_vel = applyKinkVelCap(target_exit_vel, unscaled_fv, next_max_vel, next->kink_vel); + + double achievable_exit = findAchievableExitVelocity( + branch_exit_vel, 0.0, + next->target, next->maxaccel, next_jerk, + next_max_vel, target_exit_vel); + + // Compute Ruckig profile for next segment with adjusted entry velocity + try { + ruckig::Ruckig<1> otg(g_handoff_config.servo_cycle_time_sec); + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + input.current_position = {0.0}; + input.current_velocity = {branch_exit_vel}; + input.current_acceleration = {0.0}; + input.target_position = {next->target}; + input.target_velocity = {achievable_exit}; + input.target_acceleration = {0.0}; + input.max_velocity = {next_max_vel}; + input.max_acceleration = {next->maxaccel}; + input.max_jerk = {next_jerk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) + return; + + ruckig_profile_t alt_profile; + memset(&alt_profile, 0, sizeof(alt_profile)); + copyRuckigProfile(traj, &alt_profile); + if (!alt_profile.valid) return; + if (profileHasNegativeVelocity(&alt_profile)) return; + + alt_profile.computed_feed_scale = new_feed_scale; + alt_profile.computed_vel_limit = next_vel_limit; + alt_profile.computed_vLimit = tp->vLimit; + + // Write alt_entry — cushion in place before the jump + next->shared_9d.alt_entry.profile = alt_profile; + next->shared_9d.alt_entry.v0 = branch_exit_vel; + __atomic_thread_fence(__ATOMIC_RELEASE); + __atomic_store_n(&next->shared_9d.alt_entry.valid, 1, __ATOMIC_RELEASE); + } catch (...) { + // Alt-entry is best-effort; failure falls back to v0 correction in RT + } +} + +/** + * @brief Compute a branch trajectory for feed override change + * + * Branch/Merge architecture with achievable feed cascade: + * - Computes a speculative trajectory starting from predicted state at handoff + * - If requested feed isn't achievable (segment too short), computes best achievable + * - Commits as a branch that RT can choose to take + * - RT has a window [handoff_time, window_end_time) to take the branch + * - If RT doesn't take it by window_end, the branch is stale and discarded + * + * Achievable Feed Cascade: + * - Instead of "can I apply 50%?", asks "what's the best I CAN achieve?" + * - Applies achievable portion, lets next segment continue deceleration + * - Never overshoots - physically safe for CNC + * + * Feed Limiting: + * - When requested feed would produce exit velocity exceeding junction constraints + * (kink velocity or downstream reachability), binary-searches for the maximum + * feed that keeps exit within limits. + * - Always commits a feasible profile at a reduced feed, instead of rejecting + * and leaving the old (potentially worse) profile running. + * + * @return true if branch was successfully computed and committed + */ +static void computeSpillOver(const ruckig_profile_t *profile, + double remaining_dist, + double *spill_vel, + double *spill_acc); + +bool computeBranch(TP_STRUCT *tp, TC_STRUCT *tc, double new_feed_scale) +{ + if (!tp || !tc) return false; + + // Get base kinematic limits (vLimit applied after feed scaling via applyVLimit) + double vel_limit = getEffectiveVelLimit(tp, tc); + double default_jerk = tc->maxjerk > 0 ? tc->maxjerk : g_handoff_config.default_max_jerk; + + // Check if we're resuming from stopped state + // Two cases marked by computed_feed_scale: + // -1.0 = pause/abort stopped (cycle-by-cycle), profile is invalid + // 0.0 = feed hold profile (Ruckig computed), profile is valid + double feed_scale = tc->shared_9d.profile.computed_feed_scale; + bool resuming_from_pause = (feed_scale < -0.5); // -1.0 marker + bool resuming_from_feed_hold = (feed_scale >= -0.5 && feed_scale < 0.001); // 0.0 marker + + double elapsed = atomicLoadDouble(&tc->elapsed_time); + double profile_duration = tc->shared_9d.profile.duration; + double remaining_time = profile_duration - elapsed; + + // Initial pessimistic profiles (from forward pass with v_exit=0) have + // computed_feed_scale=0 just like real feed-hold profiles, but the machine + // is actively decelerating — not stopped. Probe the profile at elapsed time: + // if velocity is significant, this is NOT a feed-hold resume but a normal + // profile that needs standard handoff timing. Misclassifying it causes + // Ruckig to receive (v>0, a<<0) as initial state → negative velocity profile. + if (resuming_from_feed_hold) { + PredictedState probe = predictStateAtTime(tc, elapsed); + if (probe.valid && probe.velocity > 1.0) { + resuming_from_feed_hold = false; + } + } + + // Get current velocity estimate for kinematic margin calculation + // We need to know velocity early to compute proper handoff margin + double current_vel_estimate = 0.0; + if (!resuming_from_feed_hold && !resuming_from_pause) { + PredictedState current_state = predictStateAtTime(tc, elapsed); + if (current_state.valid) { + current_vel_estimate = current_state.velocity; + } + } + + // Compute kinematic-aware handoff margin based on velocity change required + // Small velocity changes -> small margin (fast response) + // Large velocity changes -> larger margin (stability) + double new_max_vel = applyVLimit(tp, tc, vel_limit * new_feed_scale); + double min_handoff_margin = computeKinematicHandoffMargin( + current_vel_estimate, new_max_vel, tc->maxaccel, default_jerk); + + double horizon_sec = g_adaptive_horizon.get() / 1000.0; + double window_sec = g_handoff_config.branch_window_ms / 1000.0; + double handoff_time; + double window_end_time; + + if (resuming_from_feed_hold || resuming_from_pause) { + // RESUME FROM STOPPED: Machine is stopped, need immediate handoff. + // Set handoff_time = elapsed so RT takes branch immediately. + handoff_time = elapsed; + window_end_time = handoff_time + window_sec + 1.0; // Extended window for resume + } else { + // Normal case: calculate handoff time based on remaining profile time + + // Adaptive horizon: starts at 5ms, backs off on missed branches + // This allows fast response while self-tuning if system is slow + + // For short segments, place handoff as early as safely possible + // Phase 4 TODO: With proper blending, we can be more aggressive here + // since exit velocity handoff eliminates the need for full deceleration + if (remaining_time < horizon_sec + min_handoff_margin) { + // Short segment: handoff at current time + small margin + // This means "apply override now" rather than "schedule for later" + horizon_sec = fmax(0.001, remaining_time - min_handoff_margin); + } + + handoff_time = elapsed + horizon_sec; + window_end_time = handoff_time + window_sec; + + // Clamp handoff to before segment end + if (handoff_time >= profile_duration - min_handoff_margin) { + if (tp->aborting) { + // Abort: bypass margin — we must stop no matter where we are + // in the profile. Place handoff at current time for immediate takeover. + handoff_time = elapsed; + window_end_time = handoff_time + window_sec + 1.0; + } else { + handoff_time = profile_duration - min_handoff_margin; + if (handoff_time <= elapsed) { + // Segment is essentially done - let next segment handle it + tc->shared_9d.requested_feed_scale = new_feed_scale; + return false; + } + } + } + } + + // Predict state at handoff time + PredictedState state; + if (resuming_from_pause) { + // PAUSE/ABORT RESUME: Profile is the original motion profile (invalid). + // Use actual stopped state from RT. + state.position = atomicLoadDouble(&tc->progress); + state.velocity = 0.0; + state.acceleration = 0.0; + state.jerk = 0.0; + state.valid = true; + } else if (resuming_from_feed_hold) { + // FEED HOLD RESUME: Profile is a valid stop-in-place profile. + // Sample it - gives correct state whether still decelerating or complete. + state = predictStateAtTime(tc, handoff_time); + // Clamp velocity/acceleration to non-negative zero. + // Feed hold profiles (memset to 0) can return -0.0 from floating point + // arithmetic, which propagates into Ruckig and causes profileHasNegativeVelocity + // to reject the resume profile — permanently blocking the segment. + if (state.valid && fabs(state.velocity) < 1e-6) state.velocity = 0.0; + if (state.valid && fabs(state.acceleration) < 1e-6) state.acceleration = 0.0; + } else { + // NORMAL: Sample the active profile + state = predictStateAtTime(tc, handoff_time); + } + + if (!state.valid) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Branch: failed to predict state at handoff seg=%d reason=state_invalid", + tc->id); + return false; + } + + // NOTE: We do NOT delay handoff when cur_vel > new_max_vel. + // The branch profile will handle deceleration from cur_vel down to new_max_vel. + // This is the "track 2 continues, track 3 takes over at handoff" approach: + // - Track 2 (old profile) runs until handoff at current velocity + // - Track 3 (new profile) starts at handoff, decelerates to new max velocity + // The key is that copyRuckigProfile samples at uniform time intervals, + // avoiding Ruckig's internal phase shift when it prepends a brake trajectory. + + // Calculate remaining distance after handoff + double remaining = tc->target - state.position; + if (remaining < 1e-6) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=no_dist remaining=%.6f pos=%.6f target=%.6f " + "resume_fh=%d\n", + tc->id, remaining, state.position, tc->target, resuming_from_feed_hold); + tc->shared_9d.requested_feed_scale = new_feed_scale; + return false; + } + + // Reject branch when segment is committed to final deceleration. + // If decelerating at more than half max accel and remaining distance is + // less than the stopping distance, the existing profile is already locked + // into its stop — replanning would produce backward-motion profiles. + if (state.acceleration < -0.5 * tc->maxaccel && state.velocity > 0) { + double stop_dist = (state.velocity * state.velocity) / (2.0 * tc->maxaccel); + if (remaining <= stop_dist * 1.5) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=decel_locked vel=%.2f acc=%.2f " + "remaining=%.4f stop_dist=%.4f resume_fh=%d\n", + tc->id, state.velocity, state.acceleration, remaining, stop_dist, + resuming_from_feed_hold); + tc->shared_9d.requested_feed_scale = new_feed_scale; + return false; + } + } + + // Build Ruckig trajectories + try { + ruckig::Ruckig<1> otg(g_handoff_config.servo_cycle_time_sec); + double t_start = etime_user(); + + double effective_feed_scale = new_feed_scale; + bool need_brake = (state.velocity > new_max_vel + 0.01); + + // Fix C: Also route through two-stage path when acceleration is large. + // Large |acceleration| means velocity will overshoot before Ruckig can + // arrest it, causing position-control mode to produce backward-motion + // profiles. The stabilize stage (velocity control) cleanly brings + // state to (stable_vel, 0 acc) before position control takes over. + bool need_stabilize = false; + double stabilize_target_vel = new_max_vel; + if (!need_brake && fabs(state.acceleration) > 0.5 * tc->maxaccel) { + need_stabilize = true; + // Clamp stabilize target: don't exceed new_max_vel + stabilize_target_vel = fmin(state.velocity, new_max_vel); + if (stabilize_target_vel < 0.0) stabilize_target_vel = 0.0; + } + + // Store cascade info + tc->shared_9d.requested_feed_scale = new_feed_scale; + + ruckig_profile_t brake_profile; + ruckig_profile_t main_profile; + memset(&brake_profile, 0, sizeof(brake_profile)); + memset(&main_profile, 0, sizeof(main_profile)); + + double brake_end_position = state.position; // Position after brake (relative to segment start) + + // Feed hold: single-stage velocity control to near-zero + if (new_feed_scale < 0.001) { + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + + + // True feed hold: decelerate to complete stop + double target_vel = 0.0; + + input.control_interface = ruckig::ControlInterface::Velocity; + input.current_position = {0.0}; + input.current_velocity = {state.velocity}; + input.current_acceleration = {state.acceleration}; // Must match actual state! + input.target_velocity = {target_vel}; + input.target_acceleration = {0.0}; + input.max_velocity = {vel_limit}; + input.max_acceleration = {tc->maxaccel}; + input.max_jerk = {default_jerk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) { + return false; + } + + copyRuckigProfile(traj, &main_profile); + if (!main_profile.valid) { + return false; + } + main_profile.computed_feed_scale = 0.0; + main_profile.computed_vel_limit = vel_limit; + main_profile.computed_vLimit = tp->vLimit; + + // Fix A: Reject profile if it contains backward motion + if (profileHasNegativeVelocity(&main_profile)) { + return false; + } + + double t_end = etime_user(); + double calc_us = (t_end - t_start) * 1e6; + g_ruckig_timing.last_us = calc_us; + g_ruckig_timing.total_us += calc_us; + g_ruckig_timing.count++; + if (calc_us > g_ruckig_timing.max_us) g_ruckig_timing.max_us = calc_us; + + bool committed = commitBranch(&tc->shared_9d, &main_profile, NULL, 0.0, 0.0, + handoff_time, state.position, 0.0, window_end_time); + + // Spill-over: if stop distance exceeds remaining segment, the + // machine will cross into downstream segments mid-deceleration. + // Write alt-entry stop profiles so RT has correct v0 at each + // boundary (same pattern as tpRequestAbortBranch_9D). + if (committed && tc->term_cond == TC_TERM_COND_TANGENT) { + double spill_vel, spill_acc; + computeSpillOver(&main_profile, remaining, &spill_vel, &spill_acc); + + if (spill_vel > TP_VEL_EPSILON) { + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + double entry_vel = spill_vel; + double entry_acc = spill_acc; + + for (int i = 1; i < queue_len && entry_vel > TP_VEL_EPSILON; i++) { + TC_STRUCT *next = tcqItem_user(queue, i); + if (!next || next->target < 1e-9) break; + + double this_entry_vel = entry_vel; + double next_vel_limit = getEffectiveVelLimit(tp, next); + double next_jerk = next->maxjerk > 0 ? next->maxjerk : default_jerk; + + try { + ruckig::Ruckig<1> otg_spill(g_handoff_config.servo_cycle_time_sec); + ruckig::InputParameter<1> in_spill; + ruckig::Trajectory<1> traj_spill; + + in_spill.control_interface = ruckig::ControlInterface::Velocity; + in_spill.current_position = {0.0}; + in_spill.current_velocity = {entry_vel}; + in_spill.current_acceleration = {entry_acc}; + in_spill.target_velocity = {0.0}; + in_spill.target_acceleration = {0.0}; + in_spill.max_velocity = {next_vel_limit}; + in_spill.max_acceleration = {next->maxaccel}; + in_spill.max_jerk = {next_jerk}; + + auto result = otg_spill.calculate(in_spill, traj_spill); + if (result != ruckig::Result::Working && + result != ruckig::Result::Finished) + break; + + ruckig_profile_t stop_prof; + memset(&stop_prof, 0, sizeof(stop_prof)); + copyRuckigProfile(traj_spill, &stop_prof); + if (!stop_prof.valid || profileHasNegativeVelocity(&stop_prof)) + break; + stop_prof.computed_feed_scale = 0.0; + stop_prof.computed_vel_limit = next_vel_limit; + stop_prof.computed_vLimit = tp->vLimit; + + // Check if this segment also spills over + computeSpillOver(&stop_prof, next->target, + &entry_vel, &entry_acc); + + // Write as alt-entry so RT picks it at the junction + next->shared_9d.alt_entry.profile = stop_prof; + next->shared_9d.alt_entry.v0 = this_entry_vel; + __atomic_thread_fence(__ATOMIC_RELEASE); + __atomic_store_n(&next->shared_9d.alt_entry.valid, 1, + __ATOMIC_RELEASE); + } catch (...) { + break; + } + } + } + } + + return committed; + } + + // Chain exit cap: walk downstream segments at the NEW feed scale, + // cascading reachability limits backward to find the tightest + // constraint on the active segment's exit velocity. Replaces the + // old 1-segment lookahead that missed tight constraints further + // downstream (short segments, sharp kinks at index 2+). + double downstream_exit_cap = computeChainExitCap( + tp, tc, new_feed_scale, default_jerk); + + // ================================================================ + // Feed limiting: find max feed where exit constraints are met + // ================================================================ + // Instead of rejecting branches that exceed kink or downstream limits, + // binary-search for the highest feed that produces a feasible exit. + double exit_hard_limit = downstream_exit_cap; + if (tc->kink_vel > 0) + exit_hard_limit = fmin(exit_hard_limit, tc->kink_vel); + + if (exit_hard_limit < 1e9 && remaining > 1e-6) { + double test_exit = estimateExitAtFeed(tp, tc, state.velocity, + state.acceleration, remaining, vel_limit, new_feed_scale, + default_jerk, exit_hard_limit); + + if (test_exit > exit_hard_limit + 1.0) { + // Feed produces exit velocity exceeding junction constraints. + // Binary search for the maximum feed that satisfies the limit. + double lo = 0.001, hi = new_feed_scale; + double best_feed = lo; + + for (int iter = 0; iter < 12; iter++) { + double mid = (lo + hi) / 2.0; + double mid_exit = estimateExitAtFeed(tp, tc, state.velocity, + state.acceleration, remaining, vel_limit, mid, + default_jerk, exit_hard_limit); + if (mid_exit <= exit_hard_limit + 0.01) { + best_feed = mid; + lo = mid; + } else { + hi = mid; + } + } + + if (best_feed > 0.002) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: FEED_LIMIT seg=%d feed=%.3f->%.3f " + "exit_limit=%.2f (kink=%.2f ds_cap=%.2f)\n", + tc->id, new_feed_scale, best_feed, + exit_hard_limit, tc->kink_vel, downstream_exit_cap); + new_feed_scale = best_feed; + new_max_vel = applyVLimit(tp, tc, vel_limit * new_feed_scale); + effective_feed_scale = new_feed_scale; + // Recompute brake/stabilize determination at reduced feed + need_brake = (state.velocity > new_max_vel + 0.01); + need_stabilize = false; + stabilize_target_vel = new_max_vel; + if (!need_brake && fabs(state.acceleration) > 0.5 * tc->maxaccel) { + need_stabilize = true; + stabilize_target_vel = fmin(state.velocity, new_max_vel); + if (stabilize_target_vel < 0.0) stabilize_target_vel = 0.0; + } + } else { + // Even minimum feed can't satisfy — segment is genuinely locked + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: FEED_LIMIT_LOCKED seg=%d exit_limit=%.2f " + "min_feed_exit=%.2f vel=%.2f remaining=%.4f\n", + tc->id, exit_hard_limit, test_exit, + state.velocity, remaining); + return false; + } + } + } + + if (need_brake || need_stabilize) { + // TWO-STAGE APPROACH: Separate brake/stabilize + main profiles + // This avoids uniform sampling and keeps both profiles exact (7 phases each) + // need_brake: velocity > max, decelerate to max + // need_stabilize: |acceleration| is large, arrest it before position control + + // Stage 1: Brake/stabilize profile (velocity control) + // Bring velocity to target and acceleration to zero + double stage1_target_vel = need_brake ? new_max_vel : stabilize_target_vel; + { + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + + input.control_interface = ruckig::ControlInterface::Velocity; + input.current_position = {0.0}; + input.current_velocity = {state.velocity}; + input.current_acceleration = {state.acceleration}; + input.target_velocity = {stage1_target_vel}; + input.target_acceleration = {0.0}; + input.max_velocity = {fmax(state.velocity, new_max_vel)}; + input.max_acceleration = {tc->maxaccel}; + input.max_jerk = {default_jerk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Branch brake Ruckig failed: result=%d v=%.1f->%.1f a=%.1f maxacc=%.1f jerk=%.1f", + static_cast(result), state.velocity, stage1_target_vel, + state.acceleration, tc->maxaccel, default_jerk); + return false; + } + + // Brake profile is clean 7-phase, use phase-based sampling + copyRuckigProfile(traj, &brake_profile); + if (!brake_profile.valid) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=brake_invalid resume_fh=%d\n", + tc->id, resuming_from_feed_hold); + return false; + } + + // Enforce zero final acceleration for brake profile + // Ruckig velocity control targets a[end]=0 but may have numerical residual + // This ensures clean handoff to main profile which starts with a=0 + brake_profile.a[RUCKIG_PROFILE_PHASES] = 0.0; + + // Recompute final phase jerk for consistency + int last_phase = RUCKIG_PROFILE_PHASES - 1; + if (brake_profile.t[last_phase] > 1e-9) { + brake_profile.j[last_phase] = + (brake_profile.a[RUCKIG_PROFILE_PHASES] - brake_profile.a[last_phase]) + / brake_profile.t[last_phase]; + } else { + brake_profile.j[last_phase] = 0.0; + } + + // Get distance traveled during brake + double brake_dist = brake_profile.p[RUCKIG_PROFILE_PHASES]; + brake_end_position = state.position + brake_dist; + + brake_profile.computed_feed_scale = effective_feed_scale; + brake_profile.computed_vel_limit = vel_limit; + brake_profile.computed_vLimit = tp->vLimit; + } + + bool use_single_stage = false; // set true if sub-cycle gap detected + + // Stage 2: Main profile (position control) + // Start from stage1_target_vel (after brake/stabilize), go to segment end + { + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + + + double remaining_after_brake = tc->target - brake_end_position; + + // Find achievable exit velocity for main profile + // For tangent transitions, target the backward-pass exit velocity scaled by feed + double unscaled_fv = atomicLoadDouble(&tc->shared_9d.final_vel); + double target_exit_vel = (tc->term_cond == TC_TERM_COND_TANGENT) + ? fmin(unscaled_fv * new_feed_scale, new_max_vel) + : 0.0; + target_exit_vel = applyKinkVelCap(target_exit_vel, unscaled_fv, new_max_vel, tc->kink_vel); + target_exit_vel = fmin(target_exit_vel, downstream_exit_cap); + double achievable_exit = findAchievableExitVelocity( + stage1_target_vel, 0.0, // Start from stage1 target with zero acc + remaining_after_brake, tc->maxaccel, default_jerk, + new_max_vel, target_exit_vel); + + // Non-blending segments (STOP, EXACT) must end at zero velocity. + // If we can't decelerate to zero in the remaining distance, + // reject the branch and let the existing profile finish. + // Phase 4 TODO (Blending): review PARABOLIC handling here. + if (tc->term_cond != TC_TERM_COND_TANGENT && achievable_exit > 0.01) { + return false; + } + + // Kink-limited junctions: reject if can't decel to kink_vel + if (tc->kink_vel > 0 && achievable_exit > tc->kink_vel + 0.01) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=kink(2stg) exit=%.2f kink=%.2f resume_fh=%d\n", + tc->id, achievable_exit, tc->kink_vel, resuming_from_feed_hold); + return false; + } + + // Downstream reachability: reject if can't decel to what + // next segment accepts. Feed change deferred to next cycle. + if (achievable_exit > downstream_exit_cap + 0.01) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=downstream(2stg) exit=%.2f cap=%.2f resume_fh=%d\n", + tc->id, achievable_exit, downstream_exit_cap, resuming_from_feed_hold); + tc->shared_9d.requested_feed_scale = new_feed_scale; + return false; + } + + if (achievable_exit > target_exit_vel + 0.01) { + effective_feed_scale = achievable_exit / vel_limit; + if (effective_feed_scale > 1.0) effective_feed_scale = 1.0; + } + tc->shared_9d.achieved_exit_vel = achievable_exit; + + input.control_interface = ruckig::ControlInterface::Position; + input.current_position = {0.0}; + input.current_velocity = {stage1_target_vel}; // Start at stabilized vel + input.current_acceleration = {0.0}; // Stage 1 ends with zero acc + input.target_position = {remaining_after_brake}; + input.target_velocity = {achievable_exit}; + input.target_acceleration = {0.0}; + input.max_velocity = {new_max_vel}; + input.max_acceleration = {tc->maxaccel}; + input.max_jerk = {default_jerk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Branch main Ruckig failed: result=%d v=%.1f->%.1f dist=%.3f maxvel=%.1f maxacc=%.1f", + static_cast(result), stage1_target_vel, achievable_exit, + remaining_after_brake, new_max_vel, tc->maxaccel); + return false; + } + + // Main profile starts at/below max_vel, so no brake prepend - use phase-based + copyRuckigProfile(traj, &main_profile); + if (!main_profile.valid) return false; + main_profile.computed_feed_scale = effective_feed_scale; + main_profile.computed_vel_limit = vel_limit; + main_profile.computed_vLimit = tp->vLimit; + + // Sub-cycle gap fix: when the main profile is shorter than + // one servo cycle, the brake→main transition causes a + // displacement dip (jerk spike). Replace with a single-stage + // position-control profile from current state to segment end. + { + double cycle_time = g_handoff_config.servo_cycle_time_sec; + if (main_profile.duration < cycle_time) { + ruckig::InputParameter<1> alt_input; + ruckig::Trajectory<1> alt_traj; + + double alt_target_exit = (tc->term_cond == TC_TERM_COND_TANGENT) + ? fmin(atomicLoadDouble(&tc->shared_9d.final_vel) * new_feed_scale, new_max_vel) + : 0.0; + alt_target_exit = applyKinkVelCap(alt_target_exit, + atomicLoadDouble(&tc->shared_9d.final_vel), new_max_vel, tc->kink_vel); + alt_target_exit = fmin(alt_target_exit, downstream_exit_cap); + double alt_achievable = findAchievableExitVelocity( + state.velocity, state.acceleration, + remaining, tc->maxaccel, default_jerk, + fmax(state.velocity, new_max_vel), alt_target_exit); + + alt_input.control_interface = ruckig::ControlInterface::Position; + alt_input.current_position = {0.0}; + alt_input.current_velocity = {state.velocity}; + alt_input.current_acceleration = {state.acceleration}; + alt_input.target_position = {remaining}; + alt_input.target_velocity = {alt_achievable}; + alt_input.target_acceleration = {0.0}; + alt_input.max_velocity = {fmax(state.velocity, new_max_vel)}; + alt_input.max_acceleration = {tc->maxaccel}; + alt_input.max_jerk = {default_jerk}; + + auto alt_result = otg.calculate(alt_input, alt_traj); + bool alt_ok = (alt_result == ruckig::Result::Working || + alt_result == ruckig::Result::Finished); + + ruckig_profile_t alt_profile = {}; + if (alt_ok) { + copyRuckigProfile(alt_traj, &alt_profile); + alt_ok = alt_profile.valid && + !profileHasNegativeVelocity(&alt_profile); + } + + if (alt_ok) { + // Replace two-stage with single-stage + main_profile = alt_profile; + main_profile.computed_feed_scale = effective_feed_scale; + main_profile.computed_vel_limit = vel_limit; + main_profile.computed_vLimit = tp->vLimit; + achievable_exit = alt_achievable; + if (alt_achievable > alt_target_exit + 0.01) { + effective_feed_scale = alt_achievable / vel_limit; + if (effective_feed_scale > 1.0) effective_feed_scale = 1.0; + } + tc->shared_9d.achieved_exit_vel = alt_achievable; + use_single_stage = true; + } + } + } + } + + // Fix A: Reject if brake or main profile contains backward motion + // (skip brake check when using single-stage — no brake profile) + if (!use_single_stage && + (profileHasNegativeVelocity(&brake_profile) || + profileHasNegativeVelocity(&main_profile))) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=negvel(2stg) brake_neg=%d main_neg=%d " + "brake_dist=%.4f seg_remaining=%.4f seg_target=%.4f progress=%.4f " + "resume_fh=%d\n", + tc->id, profileHasNegativeVelocity(&brake_profile), + profileHasNegativeVelocity(&main_profile), + brake_profile.p[RUCKIG_PROFILE_PHASES], + tc->target - state.position, + tc->target, state.position, resuming_from_feed_hold); + return false; + } + + double t_end = etime_user(); + double calc_us = (t_end - t_start) * 1e6; + g_ruckig_timing.last_us = calc_us; + g_ruckig_timing.total_us += calc_us; + g_ruckig_timing.count++; + if (calc_us > g_ruckig_timing.max_us) g_ruckig_timing.max_us = calc_us; + + // Cushion before jump: write alt-entry on N+1 before committing brake on N + writeAltEntry(tp, tc, tc->shared_9d.achieved_exit_vel, effective_feed_scale); + + // Commit profile(s) + // When use_single_stage, commit as single-stage (no brake). + // Otherwise commit two-stage with brake_target_vel = stage1_target_vel. + bool committed = use_single_stage + ? commitBranch(&tc->shared_9d, &main_profile, NULL, 0.0, 0.0, + handoff_time, state.position, + effective_feed_scale, window_end_time, + tc->shared_9d.achieved_exit_vel) + : commitBranch(&tc->shared_9d, &main_profile, &brake_profile, + stage1_target_vel, brake_end_position, + handoff_time, state.position, + effective_feed_scale, window_end_time, + tc->shared_9d.achieved_exit_vel); + + return committed; + + } else { + // SINGLE-STAGE: Current velocity already at or below new max + // Standard position-control profile, no brake needed + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + + + // For tangent transitions, target the backward-pass exit velocity scaled by feed + double unscaled_fv = atomicLoadDouble(&tc->shared_9d.final_vel); + double target_exit_vel = (tc->term_cond == TC_TERM_COND_TANGENT) + ? fmin(unscaled_fv * new_feed_scale, new_max_vel) + : 0.0; + target_exit_vel = applyKinkVelCap(target_exit_vel, unscaled_fv, new_max_vel, tc->kink_vel); + target_exit_vel = fmin(target_exit_vel, downstream_exit_cap); + double achievable_exit = findAchievableExitVelocity( + state.velocity, state.acceleration, + remaining, tc->maxaccel, default_jerk, + new_max_vel, target_exit_vel); + + // Non-blending segments (STOP, EXACT) must end at zero velocity. + // If we can't decelerate to zero in the remaining distance, reject + // the branch and let the existing profile (computed with enough + // room to stop) finish. + // Phase 4 TODO (Blending): When PARABOLIC is used for planner type 2 + // blending, decide whether it should be treated as zero-exit or + // non-zero-exit here. + if (tc->term_cond != TC_TERM_COND_TANGENT && achievable_exit > 0.01) { + return false; + } + + // Kink-limited junctions: if we can't decelerate to the kink + // velocity in the remaining distance, reject the branch. + // Otherwise the profile exits above kink_vel, downstream gets + // seeded with an unreachable entry velocity, and RT sees a + // velocity discontinuity at the segment transition. + if (tc->kink_vel > 0 && achievable_exit > tc->kink_vel + 0.01) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=kink(1stg) exit=%.2f kink=%.2f resume_fh=%d\n", + tc->id, achievable_exit, tc->kink_vel, resuming_from_feed_hold); + return false; + } + + // Downstream reachability: reject if can't decel to what + // next segment accepts. Feed change deferred to next cycle. + if (achievable_exit > downstream_exit_cap + 0.01) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=downstream(1stg) exit=%.2f cap=%.2f resume_fh=%d\n", + tc->id, achievable_exit, downstream_exit_cap, resuming_from_feed_hold); + tc->shared_9d.requested_feed_scale = new_feed_scale; + return false; + } + + if (achievable_exit > target_exit_vel + 0.01) { + effective_feed_scale = achievable_exit / vel_limit; + if (effective_feed_scale > 1.0) effective_feed_scale = 1.0; + } + tc->shared_9d.achieved_exit_vel = achievable_exit; + + input.control_interface = ruckig::ControlInterface::Position; + input.current_position = {0.0}; + input.current_velocity = {state.velocity}; + input.current_acceleration = {state.acceleration}; + input.target_position = {remaining}; + input.target_velocity = {achievable_exit}; + input.target_acceleration = {0.0}; + input.max_velocity = {new_max_vel}; + input.max_acceleration = {tc->maxaccel}; + input.max_jerk = {default_jerk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Branch Ruckig failed: result=%d v=%.1f a=%.1f->v_exit=%.1f dist=%.3f maxvel=%.1f maxacc=%.1f", + static_cast(result), state.velocity, state.acceleration, + achievable_exit, remaining, new_max_vel, tc->maxaccel); + return false; + } + + // No brake prepend - use phase-based sampling + copyRuckigProfile(traj, &main_profile); + if (!main_profile.valid) return false; + main_profile.computed_feed_scale = effective_feed_scale; + main_profile.computed_vel_limit = vel_limit; + main_profile.computed_vLimit = tp->vLimit; + + // Fix A: Reject profile if it contains backward motion. + // Exception: when resuming from feed hold at rest (v≈0, a≈0), + // Ruckig may produce a trajectory with a brief negative-velocity + // "wind-up" in the initial jerk phase. This is a valid position- + // control artefact — the machine is stationary and the trajectory + // reaches the target correctly. Blocking it causes a permanent + // freeze because computeBranch sets requested_feed_scale on failure, + // which suppresses retry detection in manageBranches. + bool skip_negvel_check = (resuming_from_feed_hold || resuming_from_pause) + && fabs(state.velocity) < 1e-3 + && fabs(state.acceleration) < 1e-3; + if (!skip_negvel_check && profileHasNegativeVelocity(&main_profile)) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Branch: REJECT seg=%d reason=negvel(1stg) v=%.2f a=%.2f " + "seg_remaining=%.4f seg_target=%.4f progress=%.4f resume_fh=%d\n", + tc->id, state.velocity, state.acceleration, + remaining, tc->target, state.position, resuming_from_feed_hold); + return false; + } + + double t_end = etime_user(); + double calc_us = (t_end - t_start) * 1e6; + g_ruckig_timing.last_us = calc_us; + g_ruckig_timing.total_us += calc_us; + g_ruckig_timing.count++; + if (calc_us > g_ruckig_timing.max_us) g_ruckig_timing.max_us = calc_us; + + // Cushion before jump: write alt-entry on N+1 before committing branch on N + writeAltEntry(tp, tc, tc->shared_9d.achieved_exit_vel, effective_feed_scale); + + bool committed = commitBranch(&tc->shared_9d, &main_profile, NULL, 0.0, 0.0, + handoff_time, state.position, + effective_feed_scale, window_end_time, + achievable_exit); + + return committed; + } + + } catch (std::exception &e) { + rtapi_print_msg(RTAPI_MSG_ERR, "Branch computation exception: %s", e.what()); + return false; + } +} + +/** + * @brief Check if feed scale changed from canonical + * + * No deadband - any change triggers branch computation. + * The debounce mechanism prevents rapid recomputation. + * + * @param current_feed Current feed scale + * @param canonical_feed Canonical (last merged) feed scale + * @return true if feed changed (any amount) + */ +static bool feedChangedSignificantly(double current_feed, double canonical_feed) +{ + // No deadband - any change triggers branch (tiny epsilon for FP noise only) + double change = fabs(current_feed - canonical_feed); + return change > 1e-6; +} + +/** + * @brief Manage branches for feed override using branch/merge architecture + * + * Called each iteration of the optimization loop. Implements the branch/merge + * protocol for feed override handling: + * + * 1. Check for successful merge: branch.taken=1 means RT took the branch + * - Update canonical_feed_scale to branch's feed_scale + * - Clear branch.valid and branch.taken + * - Invalidate downstream segments for re-optimization + * + * 2. Check for missed branch: RT past window_end without taking + * - Clear branch.valid + * - Fall through to create new branch if feed still different + * + * 3. Check if new branch needed: feed changed from canonical + * - Only if no branch currently pending + * - Compute new branch trajectory + * + * @param tp Trajectory planner structure + */ +extern "C" void manageBranches(TP_STRUCT *tp) +{ + if (!tp) return; + + // Get active segment + TC_STRUCT *tc = tcqItem_user(&tp->queue, 0); + if (!tc || !tc->active || !tc->shared_9d.profile.valid) { + return; + } + + // PAUSE/ABORT: Compute smooth stop branch using Ruckig (same as 0% feed hold) + // This replaces the cycle-by-cycle stopping in RT with time-optimal deceleration + // + // Phase 4 TODO (Blending): When blend segments (bezier) are in the queue, + // verify that the abort_profiles_written gate in checkFeedOverride() still + // covers any new optimization paths added for blending. Also verify that + // tpRequestAbortBranch_9D's spill-over loop correctly walks through + // interleaved blend segments (line-bezier-line-...) — blend segments are + // short so spill-over will cross them quickly, but their maxaccel/maxjerk + // must be valid for the velocity-control stop profiles written there. + if (tp->pausing || tp->aborting) { + int branch_valid = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (!branch_valid) { + computeBranch(tp, tc, 0.0); + } + return; // Don't process feed override while pausing/aborting + } + + // Check if active segment has stale profile (race condition fix) + // This happens when segment becomes active before its profile was recomputed. + // Two cases: + // 1. profile_feed != canonical_feed — profile was written at old feed + // 2. profile_feed ≈ 0 but slider is live — initial pessimistic profile was + // never replaced because the cursor walk (from a prior merge) blocked + // branching. canonical_feed is also 0 (never set), so case 1 misses it. + // This check runs BEFORE the cursor guard so these segments get fixed + // even while a cursor walk is in progress (cursor never touches index 0). + double profile_feed = tc->shared_9d.profile.computed_feed_scale; + double canonical_feed = tc->shared_9d.canonical_feed_scale; + double current_feed_now = tpGetSegmentFeedScale(tc); + double feed_diff = fabs(profile_feed - canonical_feed); + bool stale_initial = (profile_feed < 0.001 && current_feed_now > 0.01); + + if (feed_diff > 0.005 || stale_initial) { + int branch_valid = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (!branch_valid) { + double target_feed = stale_initial ? current_feed_now : canonical_feed; + bool ok = computeBranch(tp, tc, target_feed); + if (stale_initial && !ok) { + double elapsed = atomicLoadDouble(&tc->elapsed_time); + double dur = tc->shared_9d.profile.duration; + rtapi_print_msg(RTAPI_MSG_DBG, + "STALE_INITIAL seg %d: computeBranch REJECTED feed=%.3f " + "elapsed=%.4f dur=%.4f remaining=%.4f cursor=%d\n", + tc->id, target_feed, elapsed, dur, dur - elapsed, + g_recompute_cursor); + } + } + } + + // Kink velocity check: if active segment's profile exits above its + // kink constraint, force a branch computation. The feed limiter + // inside computeBranch will cap the exit velocity by reducing the + // feed scale. This catches segments that became active before the + // optimizer could fix their exit velocity. + if (tc->kink_vel > 0 && tc->shared_9d.profile.valid) { + double prof_exit = tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + if (prof_exit > tc->kink_vel + 0.5) { + int bv = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (!bv) { + if (computeBranch(tp, tc, current_feed_now)) { + return; + } + } + } + } + + // Chain exit cap check: if active segment's profile exits above what + // the downstream chain can handle at the current feed, force a branch. + // The backward fixup pass skips the active segment, so this is the + // only place where the active's exit gets checked against full + // downstream constraints. + if (tc->term_cond == TC_TERM_COND_TANGENT && tc->shared_9d.profile.valid) { + int bv = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (!bv) { + double default_jerk = tc->maxjerk > 0 ? tc->maxjerk : g_handoff_config.default_max_jerk; + double chain_cap = computeChainExitCap(tp, tc, current_feed_now, default_jerk); + double prof_exit = tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + if (prof_exit > chain_cap + 1.0) { + computeBranch(tp, tc, current_feed_now); + // Don't return — branch may be rejected (LOCKED) if the + // current velocity is already too high. Let normal flow + // continue so feed changes are still processed. + } + } + } + + // Blend exit velocity check: if active segment was promoted to TANGENT + // by tpSetupBlend9D (a downstream blend was created), but its profile + // still exits at ~0 from Fix 4 (first profile computed before blend existed), + // trigger a branch to update the exit velocity. computeBranch reads + // final_vel (set correctly by tpSetupBlend9D) and produces the right profile. + if (tc->term_cond == TC_TERM_COND_TANGENT && tc->shared_9d.profile.valid) { + double prof_exit = tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double desired_exit = atomicLoadDouble(&tc->shared_9d.final_vel) * current_feed_now; + if (prof_exit < 1e-6 && desired_exit > 1.0) { + int bv = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (!bv) { + computeBranch(tp, tc, current_feed_now); + } + } + } + + pending_branch_t *branch = &tc->shared_9d.branch; + + // 1. Check for successful merge (RT took the branch) + int branch_valid = __atomic_load_n(&branch->valid, __ATOMIC_ACQUIRE); + int branch_taken = __atomic_load_n(&branch->taken, __ATOMIC_ACQUIRE); + + if (branch_valid && branch_taken) { + // MERGE: branch is now canonical + double new_canonical = branch->feed_scale; + + // Adaptive horizon: success, decay toward base + g_adaptive_horizon.onTake(); + + tc->shared_9d.canonical_feed_scale = new_canonical; + + // For two-stage profiles, wait until brake is complete before fully clearing + // RT needs taken=1 to detect it's still in brake phase (via in_brake_phase check) + int has_brake = branch->has_brake; + int brake_done = (has_brake ? __atomic_load_n(&branch->brake_done, __ATOMIC_ACQUIRE) : 1); + + // Always clear valid (prevents new branches during execution) + __atomic_store_n(&branch->valid, 0, __ATOMIC_RELEASE); + + if (!has_brake || brake_done) { + // Single-stage or brake complete: fully clear + __atomic_store_n(&branch->taken, 0, __ATOMIC_RELEASE); + } + // else: two-stage in brake phase — keep taken=1 for RT's brake→main transition + + // Recompute downstream at the BRANCH feed, not the current slider. + // The branch exit velocity was computed at branch_feed — downstream + // must match to avoid velocity mismatch at the junction. If the + // slider has moved since the branch was created, manageBranches will + // detect the gap (g_committed_feed != slider) and create a new branch + // through the normal branch-merge cycle. + double merge_feed = new_canonical; // = branch->feed_scale + double merge_rapid = emcmotStatus ? emcmotStatus->rapid_scale : 1.0; + // Invalidate all downstream segments and recompute as many as time allows. + invalidateNextNSegments(tp, INT_MAX); + int done = recomputeDownstreamProfiles(tp, merge_feed, merge_rapid); + + // Update global tracking + g_last_feed_scale = new_canonical; + + // Cursor walk continues from where the synchronous recompute left off + g_recompute_cursor = done + 1; // +1 to skip active segment (index 0) + g_recompute_feed_scale = merge_feed; + g_recompute_rapid_scale = merge_rapid; + g_recompute_first_batch_done = false; + return; + } + + // Cleanup for completed two-stage profiles: clear taken once brake is done + if (!branch_valid && branch_taken && branch->has_brake) { + int brake_done = __atomic_load_n(&branch->brake_done, __ATOMIC_ACQUIRE); + if (brake_done) { + __atomic_store_n(&branch->taken, 0, __ATOMIC_RELEASE); + } + } + + // 2. Check for missed branch (RT past window, didn't take it) + double rt_elapsed = atomicLoadDouble(&tc->elapsed_time); + + if (branch_valid && !branch_taken && rt_elapsed > branch->window_end_time) { + // Branch missed - back off the horizon + g_adaptive_horizon.onMiss(); + __atomic_store_n(&branch->valid, 0, __ATOMIC_RELEASE); + // Fall through to potentially create new branch + } + + // 3. Check if new feed change needed + if (!emcmotStatus) return; + + // Use the per-segment-type feed scale, not the global net_feed_scale. + // net_feed_scale is computed per RT cycle based on the CURRENT motion type + // (traverse vs feed), so it reflects whatever RT is executing right now — + // not the segment we're branching on. This causes phantom feed changes + // when RT executes a traverse while we're branching on a feed move. + double current_feed = tpGetSegmentFeedScale(tc); + + // Compare against REQUESTED feed scale, not canonical (effective) feed scale. + // This is critical for the achievable feed cascade: + // - If segment was too short, effective_feed != requested_feed + // - canonical_feed_scale tracks effective (what was achieved) + // - requested_feed_scale tracks what user actually wants + // - We should keep trying to achieve requested until segment ends + // - Using canonical would cause infinite loop: effective != requested -> recompute -> same effective + double requested_feed = tc->shared_9d.requested_feed_scale; + + // If requested_feed is uninitialized (0), fall back to canonical + if (requested_feed < 0.001) { + requested_feed = canonical_feed; + } + + // Check if feed or velocity limit changed significantly + double current_vel_limit = getEffectiveVelLimit(tp, tc); + double current_max_vel = applyVLimit(tp, tc, current_vel_limit * current_feed); + + double profile_vel_limit = tc->shared_9d.profile.computed_vel_limit; + double profile_feed_scale = tc->shared_9d.profile.computed_feed_scale; + + double effective_profile_feed = (profile_feed_scale < 0.001) + ? requested_feed : profile_feed_scale; + double profile_max_vel = applyVLimit(tp, tc, + profile_vel_limit * effective_profile_feed); + + bool feed_changed = feedChangedSignificantly(current_feed, requested_feed); + double vel_diff = fabs(current_max_vel - profile_max_vel); + double vel_threshold = fmax(current_max_vel, profile_max_vel) * 0.005; + bool vel_changed = (vel_diff > vel_threshold) && (vel_diff > 0.1); + + if (!feed_changed && !vel_changed) return; + + // Feed change detected. + // If cursor walk is active, queue this change (next-in-line, freely overwritable). + if (g_recompute_cursor > 0) { + // Recovery from feed-hold: abort the hold cursor and restart fresh, + // just like program start. The hold cursor is walking at feed≈0, + // skipping segments with valid profiles. Those profiles are stale + // (computed at the pre-hold feed) and RT will hit them with wrong v0. + // By falling through to the normal feed-change path, we get + // invalidateNextNSegments + synchronous recomputeDownstreamProfiles + // before RT can advance into the stale segments. + if (g_recompute_feed_scale < 0.001 && current_feed > 0.001) { + g_recompute_cursor = 0; + g_next_feed_scale = -1.0; + g_next_rapid_scale = -1.0; + // Fall through to process recovery feed immediately + } else { + g_next_feed_scale = current_feed; + g_next_rapid_scale = emcmotStatus->rapid_scale; + return; + } + } + + // Cursor idle — start processing this feed change. + // Debounce check (only when starting new work, not for queueing) + double now_ms = etime_user() * 1000.0; + if (now_ms - g_last_replan_time_ms < g_handoff_config.feed_override_debounce_ms) { + return; + } + + double snap_feed = current_feed; + double snap_rapid = emcmotStatus->rapid_scale; + + // Two-phase feed hold: when user requests 0%, first decelerate to 0.1% + // using normal Ruckig (Phase 1), then engage real 0% to stop completely + // (Phase 2). Phase 1 is a normal Ruckig branch — no special treatment. + // Phase 2 only fires once the machine has actually decelerated to near the + // 0.1% target velocity, checked via predictStateAtTime (not profile feed + // label, which drops to 0.001 immediately on commit). + static const double MINIMUM_RUCKIG_FEED = 0.001; // 0.1% + if (snap_feed < 0.001) { + double elapsed = atomicLoadDouble(&tc->elapsed_time); + PredictedState probe = predictStateAtTime(tc, elapsed); + double vel_at_min_feed = tc->reqvel * MINIMUM_RUCKIG_FEED; + if (!probe.valid || probe.velocity > vel_at_min_feed) { + // Phase 1: still decelerating, use normal Ruckig at 0.1% + snap_feed = MINIMUM_RUCKIG_FEED; + } + // else: Phase 2 — deceleration complete, allow real 0% + } + + int commit_seg; + if (current_feed < 0.001) { + commit_seg = 1; + } else { + commit_seg = estimateCommitSegment(tp, current_feed); + } + + if (commit_seg <= 1) { + // Big active segment or short queue: branch on active (existing fast path) + branch_valid = __atomic_load_n(&branch->valid, __ATOMIC_ACQUIRE); + + if (branch_valid) { + int branch_taken = __atomic_load_n(&branch->taken, __ATOMIC_ACQUIRE); + + if (!branch_taken) { + // Branch pending but not yet taken by RT. Safe to overwrite + // with the newer feed value — the old branch's feed is stale + // since the user has moved the slider since it was created. + __atomic_store_n(&branch->valid, 0, __ATOMIC_RELEASE); + branch_valid = 0; + } else { + // RT already took this branch — it's being merged. + // Don't overwrite during merge. + } + } + + if (!branch_valid) { + double branch_t0 = etime_user(); + if (computeBranch(tp, tc, snap_feed)) { + g_last_replan_time_ms = now_ms; + g_commit_segment = 1; + // Update committed feed so optimizer doesn't overwrite + // our batch-written profiles with a stale feed value. + g_committed_feed = snap_feed; + g_committed_rapid = snap_rapid; + + // Immediately recompute downstream profiles at the branch feed. + // computeBranch() has set achieved_exit_vel, so + // recomputeDownstreamProfiles() seeds the correct entry + // velocity for the next segment (via lines 1798-1800). + // This closes the 5-10ms gap where nexttc had stale v[0]. + double branch_elapsed_us = (etime_user() - branch_t0) * 1e6; + double budget_us = g_handoff_config.servo_cycle_time_sec * 0.5e6; + + invalidateNextNSegments(tp, INT_MAX); + + if (branch_elapsed_us < budget_us) { + // Have budget: recompute synchronously + int done = recomputeDownstreamProfiles(tp, snap_feed, snap_rapid); + g_recompute_cursor = done + 1; + } else { + // No budget: cursor walk will handle it next cycle + g_recompute_cursor = 1; + } + + g_recompute_feed_scale = snap_feed; + g_recompute_rapid_scale = snap_rapid; + g_recompute_first_batch_done = false; + } + } + } else { + // Active nearly done: defer feed change until next active has enough + // remaining time for a safe branch. The slider position persists in + // emcmotStatus->net_feed_scale — manageBranches will re-detect the + // feed difference on the next cycle where estimateCommitSegment returns 1. + // Do NOT update g_last_replan_time_ms — avoids blocking retry via debounce. + } +} + +/** + * @brief Compute the velocity at which a stop profile crosses a segment boundary. + * + * Given a velocity-control stop profile (decelerating to zero) and the remaining + * distance on the segment, find the velocity/acceleration state at the point + * where displacement equals remaining distance. If the stopping distance fits + * within the remaining distance, spill_vel is 0 (no spill-over). + * + * Uses binary search on time to find the crossing point. + */ +static void computeSpillOver(const ruckig_profile_t *profile, + double remaining_dist, + double *spill_vel, + double *spill_acc) +{ + *spill_vel = 0.0; + *spill_acc = 0.0; + + if (!profile || !profile->valid || remaining_dist <= 0.0) return; + + // Sample at profile end to get total stopping displacement + double end_pos, end_vel, end_acc, end_jrk; + int ok = ruckigProfileSample(const_cast(profile), + profile->duration, &end_pos, &end_vel, &end_acc, &end_jrk); + if (ok != 0) return; + + // If stopping distance fits within segment, no spill-over + if (end_pos <= remaining_dist) return; + + // Binary search for time when displacement = remaining_dist + double t_lo = 0.0; + double t_hi = profile->duration; + for (int i = 0; i < 50; i++) { + double t_mid = (t_lo + t_hi) * 0.5; + double p, v, a, j; + ruckigProfileSample(const_cast(profile), t_mid, &p, &v, &a, &j); + if (p < remaining_dist) { + t_lo = t_mid; + } else { + t_hi = t_mid; + } + if (t_hi - t_lo < 1e-9) break; + } + + // Sample at the crossing time + double p, v, a, j; + ruckigProfileSample(const_cast(profile), t_hi, &p, &v, &a, &j); + *spill_vel = fmax(v, 0.0); + *spill_acc = a; +} + +/** + * @brief Request a stop branch for abort (called from task layer) + * + * Called by emcTrajAbort() in the task thread BEFORE the EMCMOT_ABORT + * command is processed by RT. This guarantees the stop branch is ready + * when tpAbort() runs in the next servo cycle. + * + * Uses the same computeBranch(tp, tc, 0.0) path as feed hold / pause, + * leveraging the existing branch/merge infrastructure for smooth stop. + * + * When the stopping distance exceeds the remaining distance on the active + * segment, the deceleration spills over into downstream segments. Each + * downstream segment gets a velocity-control stop profile that starts from + * the spill-over velocity, so the machine decelerates smoothly across + * segment boundaries — just like a train braking across track sections. + * + * @param tp Trajectory planner structure (shared memory) + * @return 0 on success, -1 on error + */ +extern "C" int tpRequestAbortBranch_9D(TP_STRUCT *tp) +{ + if (!tp) return -1; + + TC_STRUCT *tc = tcqItem_user(&tp->queue, 0); + if (!tc || !tc->active || !tc->shared_9d.profile.valid) { + // No active segment — RT will handle abort directly + // (empty queue or segment without profile) + return 0; + } + + // Clear any pending branch so we can write the stop branch + __atomic_store_n(&tc->shared_9d.branch.valid, 0, __ATOMIC_RELEASE); + __atomic_store_n(&tc->shared_9d.branch.taken, 0, __ATOMIC_RELEASE); + + // Compute stop branch for active segment (same as 0% feed hold). + // Downstream segments are handled below via spill-over logic and + // by the optimizer (which writes zero-motion profiles at feed < 0.001). + if (!computeBranch(tp, tc, 0.0)) { + return -1; + } + + // Prevent feed-override merge from overwriting our stop profiles. + // Both this flag and the merge check run in the same userspace thread, + // so no atomics needed — no race by construction. + tp->abort_profiles_written = 1; + + // Check if the stop profile spills over the active segment boundary. + // The stop branch is now in tc->shared_9d.branch.profile. + double remaining = tc->target - tc->shared_9d.branch.handoff_position; + double spill_vel, spill_acc; + computeSpillOver(&tc->shared_9d.branch.profile, remaining, + &spill_vel, &spill_acc); + + if (spill_vel < TP_VEL_EPSILON) { + // Stopping distance fits within active segment — no spill-over needed + return 0; + } + + // Spill-over: the active segment's stop profile can't stop in time. + // Write velocity-control stop profiles to downstream segments that + // continue the deceleration from the spill-over point. + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + double default_jerk = tc->maxjerk > 0 ? tc->maxjerk : g_handoff_config.default_max_jerk; + + double entry_vel = spill_vel; + double entry_acc = spill_acc; + + for (int i = 1; i < queue_len && entry_vel > TP_VEL_EPSILON; i++) { + TC_STRUCT *next = tcqItem_user(queue, i); + if (!next || next->target < 1e-9) break; + + double vel_limit = getEffectiveVelLimit(tp, next); + double max_jrk = next->maxjerk > 0 ? next->maxjerk : default_jerk; + + try { + ruckig::Ruckig<1> otg(g_handoff_config.servo_cycle_time_sec); + ruckig::InputParameter<1> input; + ruckig::Trajectory<1> traj; + + + input.control_interface = ruckig::ControlInterface::Velocity; + input.current_position = {0.0}; + input.current_velocity = {entry_vel}; + input.current_acceleration = {entry_acc}; + input.target_velocity = {0.0}; + input.target_acceleration = {0.0}; + input.max_velocity = {vel_limit}; + input.max_acceleration = {next->maxaccel}; + input.max_jerk = {max_jrk}; + + auto result = otg.calculate(input, traj); + if (result != ruckig::Result::Working && result != ruckig::Result::Finished) { + break; + } + + ruckig_profile_t stop_profile; + memset(&stop_profile, 0, sizeof(stop_profile)); + copyRuckigProfile(traj, &stop_profile); + if (!stop_profile.valid || profileHasNegativeVelocity(&stop_profile)) { + break; + } + stop_profile.computed_feed_scale = 0.0; + stop_profile.computed_vel_limit = vel_limit; + stop_profile.computed_vLimit = tp->vLimit; + + // Check if this stop profile also spills over this segment + computeSpillOver(&stop_profile, next->target, + &entry_vel, &entry_acc); + + // Write the stop profile to this downstream segment + __atomic_store_n(&next->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + next->shared_9d.profile = stop_profile; + __atomic_thread_fence(__ATOMIC_SEQ_CST); + __atomic_store_n(&next->shared_9d.profile.valid, 1, __ATOMIC_RELEASE); + atomicStoreInt((int*)&next->shared_9d.optimization_state, TC_PLAN_FINALIZED); + + } catch (...) { + break; + } + } + + return 0; +} + +// Forward declaration (defined later in file) +int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimization_depth); + +/** + * @brief Ensure profiles are computed when buffer runs critically low + * + * Called periodically to check if buffer is thin and there are segments + * without valid profiles. If so, triggers optimization for those segments. + * + * This is the adaptive throttling mechanism - when buffer runs thin, + * userspace speeds up by computing more profiles ahead of time. + * + * @param tp Trajectory planner structure + */ +static void ensureProfilesOnLowBuffer(TP_STRUCT *tp) +{ + if (!tp) return; + + // Rate limit this check to avoid overhead + // NOTE: local static — persists across program runs! (potential non-determinism) + static double last_check_ms = 0; + double now_ms = etime_user() * 1000.0; + if (now_ms - last_check_ms < 20.0) { // Check at most every 20ms + return; + } + last_check_ms = now_ms; + + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + if (queue_len < 2) return; + + int depth = queue_len; + if (depth > MAX_LOOKAHEAD_DEPTH) depth = MAX_LOOKAHEAD_DEPTH; + + // === Phase 1: Urgent — missing profiles when buffer is critical === + double buffer_ms = calculateBufferTimeMs(tp); + if (buffer_ms < g_handoff_config.min_buffer_time_ms) { + bool needs_profiles = false; + for (int i = 1; i < queue_len; i++) { + TC_STRUCT *tc = tcqItem_user(queue, i); + if (tc && !tc->shared_9d.profile.valid) { + needs_profiles = true; + break; + } + } + + if (needs_profiles) { + rtapi_print_msg(RTAPI_MSG_DBG, + "Adaptive throttle: buffer=%.0fms < min=%.0fms, computing profiles for %d segments\n", + buffer_ms, g_handoff_config.min_buffer_time_ms, queue_len); + + int result = computeLimitingVelocities_9D(queue, depth, g_smoothing_data); + if (result != 0) return; + result = applyLimitingVelocities_9D(queue, g_smoothing_data, depth); + if (result != 0) return; + computeRuckigProfiles_9D(tp, queue, depth); + return; // Urgent work done, don't spend more time + } + } + + // === Phase 2: Convergence precomputation === + // Iterate backward+forward passes until all profiles stabilize. + // The time budget (half a servo cycle) is the dynamic limiter — + // no fixed iteration cap needed. Each iteration propagates the + // velocity cascade one step further; short-segment groups need + // as many iterations as there are chained short segments. + TC_STRUCT *active = tcqItem_user(queue, 0); + if (!active || !active->active || !active->shared_9d.profile.valid) return; + + double remaining_time_ms = (active->shared_9d.profile.duration - active->elapsed_time) * 1000.0; + if (remaining_time_ms < 10.0) return; // Too close to segment end, don't risk it + + // Snapshot exit velocities of ALL non-active segments to detect convergence. + // Use stack buffer for typical cases, heap for large queues. + int check_count = queue_len - 1; // Exclude active segment + if (check_count <= 0) return; + + double stack_buf[64]; + double *prev_exit_vels = (check_count <= 64) ? stack_buf : new double[check_count]; + + auto snapshotExitVels = [&]() { + for (int i = 0; i < check_count; i++) { + TC_STRUCT *tc = tcqItem_user(queue, i + 1); + if (!tc) { prev_exit_vels[i] = 0.0; continue; } + if (tc->shared_9d.profile.valid && tc->term_cond == TC_TERM_COND_TANGENT) { + prev_exit_vels[i] = profileExitVelUnscaled(&tc->shared_9d.profile); + } else { + prev_exit_vels[i] = 0.0; + } + } + }; + + auto checkConverged = [&]() -> bool { + for (int i = 0; i < check_count; i++) { + TC_STRUCT *tc = tcqItem_user(queue, i + 1); + if (!tc) continue; + double new_exit_vel = 0.0; + if (tc->shared_9d.profile.valid && tc->term_cond == TC_TERM_COND_TANGENT) { + new_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + } + if (fabs(new_exit_vel - prev_exit_vels[i]) > 0.1) return false; + } + return true; + }; + + // Time budget: half a servo cycle of wall time + double budget_us = g_handoff_config.servo_cycle_time_sec * 0.5e6; + double iter_start = etime_user(); + + // Iterate until converged or time budget exhausted + int conv_iters = 0; + bool conv_result = false; + for (;;) { + snapshotExitVels(); + + int result = computeLimitingVelocities_9D(queue, depth, g_smoothing_data); + if (result != 0) break; + result = applyLimitingVelocities_9D(queue, g_smoothing_data, depth); + if (result != 0) break; + computeRuckigProfiles_9D(tp, queue, depth); + conv_iters++; + + if (checkConverged()) { conv_result = true; break; } + + double elapsed_us = (etime_user() - iter_start) * 1e6; + if (elapsed_us >= budget_us) break; + } + + (void)conv_result; // Used only for convergence check above + + if (prev_exit_vels != stack_buf) delete[] prev_exit_vels; +} + +/** + * @brief Check for feed override changes (legacy wrapper) + * + * This function is kept for API compatibility. The actual branch/merge + * logic is now in manageBranches(). + * + * @deprecated Use manageBranches() directly + * @param tp Trajectory planner structure + */ +extern "C" void checkFeedOverride(TP_STRUCT *tp) +{ + // Abort profiles written — no feed-override activity allowed. + // This covers the window between tpRequestAbortBranch_9D() (userspace, + // sets abort_profiles_written=1) and tpAbort() (RT, sets tp->aborting=1). + // Once tp->aborting is visible, manageBranches() returns early anyway. + if (tp && tp->abort_profiles_written) { + g_recompute_cursor = 0; + return; + } + + // Forward to new branch/merge implementation + manageBranches(tp); + + // Incremental downstream profile recomputation + // After a feed change, walk through queued segments a few per tick, + // recomputing Ruckig profiles for the new feed scale. + // The immediate 3 segments are handled synchronously at merge/commit time; + // this cursor handles the rest incrementally. + if (g_recompute_cursor > 0 && tp) { + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len = tcqLen_user(queue); + + // Get default jerk + TC_STRUCT *active = tcqItem_user(queue, 0); + double default_jerk = (active && active->maxjerk > 0) + ? active->maxjerk : g_handoff_config.default_max_jerk; + if (default_jerk < 1.0) default_jerk = g_handoff_config.default_max_jerk; + + // Seed prev_exit_vel from segment before cursor. + // Use unscaled final_vel (consistent with loop body which scales at compute time). + // For active segment (cursor==1), use profile exit velocity un-scaled since + // the branch may have changed achievable exit from the backward pass value. + double prev_exit_vel = 0.0; + double prev_exit_feed_scale = 1.0; + if (g_recompute_cursor > 1) { + TC_STRUCT *prev = tcqItem_user(queue, g_recompute_cursor - 1); + if (prev && prev->term_cond == TC_TERM_COND_TANGENT) { + // Non-active segment: profile was computed by us, use its exit vel + if (prev->shared_9d.profile.valid) { + prev_exit_vel = profileExitVelUnscaled(&prev->shared_9d.profile); + prev_exit_feed_scale = prev->shared_9d.profile.computed_feed_scale; + } else { + prev_exit_vel = atomicLoadDouble(&prev->shared_9d.final_vel); + prev_exit_feed_scale = tpGetSnapshotFeedScale(prev, + g_recompute_feed_scale, g_recompute_rapid_scale); + } + } + } else if (g_recompute_cursor == 1 && active) { + if (active->term_cond == TC_TERM_COND_TANGENT && + active->shared_9d.profile.valid) { + // If a pending branch exists, its exit velocity is the future truth + int bv = __atomic_load_n(&active->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (bv && active->shared_9d.branch.profile.valid && + active->shared_9d.branch.profile.computed_feed_scale > 0.001) { + prev_exit_vel = profileExitVelUnscaled(&active->shared_9d.branch.profile); + prev_exit_feed_scale = active->shared_9d.branch.feed_scale; + } else { + prev_exit_vel = profileExitVelUnscaled(&active->shared_9d.profile); + prev_exit_feed_scale = active->shared_9d.profile.computed_feed_scale; + } + } + } + + int recomputed = 0; + int i = g_recompute_cursor; + for (; i < queue_len; i++) { + + TC_STRUCT *tc = tcqItem_user(queue, i); + if (!tc || tc->target < 1e-9) { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + continue; + } + + // Per-segment feed scale from snapshots (locked at trigger time) + double feed_scale = tpGetSnapshotFeedScale(tc, + g_recompute_feed_scale, g_recompute_rapid_scale); + + // Skip if profile already matches target feed + double profile_feed = tc->shared_9d.profile.computed_feed_scale; + if (tc->shared_9d.profile.valid && + fabs(profile_feed - feed_scale) < 0.005) { + // Feed matches — but check if entry velocity also matches + double expected_v0 = prev_exit_vel * prev_exit_feed_scale; + double profile_v0 = tc->shared_9d.profile.v[0]; + double v0_delta = fabs(profile_v0 - expected_v0); + if (v0_delta < 0.5) { + // Both feed and entry velocity match — genuinely up-to-date + if (tc->term_cond == TC_TERM_COND_TANGENT) { + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + prev_exit_feed_scale = tc->shared_9d.profile.computed_feed_scale; + } else { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + } + continue; + } + // Fall through to recompute — entry velocity is stale + } + + double v_entry = prev_exit_vel; + double v_exit = (tc->term_cond == TC_TERM_COND_TANGENT) + ? atomicLoadDouble(&tc->shared_9d.final_vel) : 0.0; + prev_exit_vel = v_exit; + // prev_exit_feed_scale set below after profile compute + + // Get previous segment's kink_vel for entry cap check + TC_STRUCT *prev_tc = (i > 0) ? tcqItem_user(queue, i - 1) : NULL; + double prev_kink = (prev_tc && prev_tc->kink_vel > 0) ? prev_tc->kink_vel : -1.0; + + atomicStoreDouble(&tc->shared_9d.entry_vel, v_entry); + + double vel_limit = getEffectiveVelLimit(tp, tc); + double max_jrk = tc->maxjerk > 0 ? tc->maxjerk : default_jerk; + + if (feed_scale < 0.001) { + // Don't clobber a valid non-hold profile with a feed-hold + // profile. The committed-feed mechanism ensures this pass + // sees feed_scale≈0 while the reactive path (branch / + // cursor walk) will restore motion at the new feed. + // If committed-feed ever drifts, this guard is the + // last line of defence against erasing good profiles. + if (tc->shared_9d.profile.valid && + tc->shared_9d.profile.computed_feed_scale > 0.001) { + if (tc->term_cond == TC_TERM_COND_TANGENT) { + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + } else { + prev_exit_vel = 0.0; + } + prev_exit_feed_scale = tc->shared_9d.profile.computed_feed_scale; + continue; + } + createFeedHoldProfile(tc, vel_limit, tp->vLimit, "computeRuckig_backward"); + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + } else { + double max_vel = applyVLimit(tp, tc, vel_limit * feed_scale); + double max_acc = tcGetTangentialMaxAccel_9D_user(tc); + // Restore absolute velocity using previous segment's feed scale + double scaled_v_entry = fmin(v_entry * prev_exit_feed_scale, max_vel); + + // Cap entry velocity by previous segment's kink limit + if (prev_kink > 0) + scaled_v_entry = fmin(scaled_v_entry, prev_kink); + + double scaled_v_exit_pre = fmin(v_exit * feed_scale, max_vel); + double scaled_v_exit = applyKinkVelCap(scaled_v_exit_pre, v_exit, max_vel, tc->kink_vel); + double desired_fvel_for_profile = scaled_v_exit; + + // Reachability cap + { + double v_fwd = jerkLimitedMaxEntryVelocity( + scaled_v_entry, tc->target, max_acc, max_jrk); + double v_bwd = jerkLimitedMaxEntryVelocity( + scaled_v_exit, tc->target, max_acc, max_jrk); + scaled_v_exit = fmin(scaled_v_exit, v_fwd); + scaled_v_entry = fmin(scaled_v_entry, v_bwd); + } + + try { + RuckigProfileParams rp = {scaled_v_entry, scaled_v_exit, + max_vel, max_acc, max_jrk, tc->target, + feed_scale, vel_limit, tp->vLimit, desired_fvel_for_profile}; + + if (computeAndStoreProfile(tc, rp)) { + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + if (tc->term_cond == TC_TERM_COND_TANGENT) + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + prev_exit_feed_scale = feed_scale; + + // One-step backtrack: if this segment's entry was + // capped by backward reachability (v[0] < predecessor's + // v[end]), recompute the predecessor with a lower exit + // target so the chain is gap-free at this junction. + // The backward pass runs at feed=1.0 and doesn't know + // that at higher feed, reachability is tighter. + // Skip active segment — rewriting triggers STOPWATCH_RESET + // with velocity discontinuity worse than the chain gap. + TC_STRUCT *prev_seg = (i > 0) ? tcqItem_user(queue, i - 1) : NULL; + if (prev_seg && !prev_seg->active + && prev_seg->term_cond == TC_TERM_COND_TANGENT + && prev_seg->shared_9d.profile.valid) { + double prev_v_end = prev_seg->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double curr_v0 = tc->shared_9d.profile.v[0]; + if (prev_v_end > curr_v0 + 0.01) { + // Recompute predecessor with exit = curr_v0 + double p_entry = prev_seg->shared_9d.profile.v[0]; + double p_vel_limit = getEffectiveVelLimit(tp, prev_seg); + double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * feed_scale); + double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg); + double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk; + RuckigProfileParams rp_prev = {p_entry, curr_v0, + p_max_vel, p_max_acc, p_max_jrk, prev_seg->target, + feed_scale, p_vel_limit, tp->vLimit, curr_v0}; + if (computeAndStoreProfile(prev_seg, rp_prev)) { + atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state, + TC_PLAN_FINALIZED); + } + } + } + } else { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + rtapi_print_msg(RTAPI_MSG_ERR, "Cursor recompute fail seg=%d", + tc->id); + } + } catch (std::exception &e) { + __atomic_store_n(&tc->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + rtapi_print_msg(RTAPI_MSG_ERR, "Cursor recompute exception seg=%d: %s", + tc->id, e.what()); + } + } + + recomputed++; + } + + if (recomputed > 0) { + g_recompute_first_batch_done = true; + // Update throughput estimate (EMA, alpha=0.3) + g_segments_per_tick = 0.7 * g_segments_per_tick + 0.3 * recomputed; + } + + // Advance or finish + if (i >= queue_len) { + g_recompute_cursor = 0; // Done — walked entire queue + // All segments are now consistently at the cursor walk's feed. + g_committed_feed = g_recompute_feed_scale; + g_committed_rapid = g_recompute_rapid_scale; + + // Promote queued feed change if present + if (g_next_feed_scale >= 0) { + double next_feed = g_next_feed_scale; + double next_rapid = g_next_rapid_scale; + g_next_feed_scale = -1.0; + g_next_rapid_scale = -1.0; + + // Only start new cursor walk if queued feed differs from committed + if (fabs(next_feed - g_committed_feed) > 0.005 || + fabs(next_rapid - g_committed_rapid) > 0.005) { + int commit_seg = estimateCommitSegment(tp, next_feed); + if (commit_seg <= 1) { + // Big active segment: let manageBranches handle it next cycle + } else { + // Defer: manageBranches will re-detect from net_feed_scale + } + } + } + } else { + g_recompute_cursor = i; // Resume here next tick + } + } + + // Adaptive throttling: ensure profiles computed when buffer is thin + ensureProfilesOnLowBuffer(tp); + +} + +/** + * @brief Compute Ruckig trajectory profiles for all segments + * + * Forward pass through queue: for each segment, compute Ruckig trajectory + * using entry velocity from previous segment and exit velocity from + * backward pass optimization. + * + * Phase 4 TODO (Blending): The backward pass currently sets final_vel as a + * static kink velocity at each junction. With blending, junctions are replaced + * by blend regions whose entry velocity depends on RT's actual position/speed. + * The backward pass should constrain final_vel to the blend entry velocity + * computed by the blend solver, which already accounts for achievable velocity. + * The forward pass (reading back actual Ruckig exit velocities) will still be + * needed — the same pattern used by manageBranches for every segment should + * apply uniformly, including the first segment. + * + * @param queue Queue of TC segments + * @param optimization_depth Number of segments to process + * @return 0 on success + */ +int computeRuckigProfiles_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, int optimization_depth) +{ + if (!tp || !queue || optimization_depth <= 0) return 0; + + // Get default jerk from first segment + // tc->maxjerk is set from INI [JOINT_N] MAX_JERK, transformed through Jacobian + // in userspace_kinematics.cc computeJointSpaceSegment() + TC_STRUCT *first_tc = tcqBack_user(queue, -(optimization_depth - 1)); + // Use tc->maxjerk if valid, otherwise fall back to config default + double default_jerk = first_tc ? first_tc->maxjerk : g_handoff_config.default_max_jerk; + if (default_jerk < 1.0) default_jerk = g_handoff_config.default_max_jerk; + + // Use committed feed, not live feed. + // The committed feed is only updated when a cursor walk completes a full pass, + // meaning all downstream segments have been consistently recomputed. + // This prevents overwriting profiles that the reactive path + // (recomputeDownstreamProfiles / cursor walk) is actively updating. + // During a feed transition, this function defers to the reactive path. + double live_feed = emcmotStatus ? emcmotStatus->feed_scale : 1.0; + double live_rapid = emcmotStatus ? emcmotStatus->rapid_scale : 1.0; + if (g_committed_feed < 0) { + // First call: initialize from live + g_committed_feed = live_feed; + g_committed_rapid = live_rapid; + } + double pass_feed_scale = g_committed_feed; + double pass_rapid_scale = g_committed_rapid; + + // Forward pass: oldest to newest (from -(depth-1) to 0) + // Entry velocity for segment k comes from exit velocity of segment k-1 + double prev_exit_vel = 0.0; + bool prev_exit_vel_known = false; + // Feed scale used to unscale prev_exit_vel. Needed to restore absolute + // velocity at feed/rapid boundaries where the arriving segment's feed_scale + // differs from the departing segment's. + double prev_exit_feed_scale = 1.0; + + // Seed prev_exit_vel from the segment just before the optimization window. + // Without this, the oldest segment in the window always gets v_entry=0 + // because no predecessor within the window sets prev_exit_vel. + { + TC_STRUCT *pre_window = tcqBack_user(queue, -optimization_depth); + { + static int seed_dbg = 0; + if (seed_dbg < 10) { + seed_dbg++; + rtapi_print_msg(RTAPI_MSG_ERR, + "SEED_DBG: pre_window=%p depth=%d term=%d pvalid=%d pfeed=%.3f " + "pv_exit=%.3f fv=%.3f type=%d id=%d\n", + pre_window, optimization_depth, + pre_window ? pre_window->term_cond : -1, + pre_window ? pre_window->shared_9d.profile.valid : 0, + pre_window ? pre_window->shared_9d.profile.computed_feed_scale : 0.0, + pre_window ? profileExitVelUnscaled(&pre_window->shared_9d.profile) : -1.0, + pre_window ? atomicLoadDouble(&pre_window->shared_9d.final_vel) : -1.0, + pre_window ? pre_window->motion_type : -1, + pre_window ? pre_window->id : -1); + } + } + if (pre_window && pre_window->term_cond == TC_TERM_COND_TANGENT + && pre_window->shared_9d.profile.valid + && pre_window->shared_9d.profile.computed_feed_scale > 0.001) { + prev_exit_vel = profileExitVelUnscaled(&pre_window->shared_9d.profile); + // If the stored profile has v_exit≈0 but final_vel is non-zero, + // the profile was poisoned by Fix 4 before a blend was created. + // Use final_vel (set by tpSetupBlend9D at enqueue time) instead. + if (prev_exit_vel < 1e-6) { + double fv = atomicLoadDouble(&pre_window->shared_9d.final_vel); + if (fv > 1e-6) + prev_exit_vel = fv; + } + prev_exit_feed_scale = pre_window->shared_9d.profile.computed_feed_scale; + prev_exit_vel_known = true; + } + } + + // Profile swaps are safe because RT detects generation-counter changes + // and performs a stopwatch reset (position_base = progress, elapsed_time = 0). + // No frozen zone needed — all segments can be freely recomputed. + + for (int k = optimization_depth - 1; k >= 0; k--) { + TC_STRUCT *tc = tcqBack_user(queue, -k); + if (!tc || tc->target < 1e-9) { + prev_exit_vel = 0.0; // After dwell/zero-length, next segment starts from rest + prev_exit_feed_scale = 1.0; + prev_exit_vel_known = true; + continue; + } + + // Skip active segment — its profile is managed by the branch/merge + // system which accounts for position_base. Overwriting here with + // target_position=tc->target would ignore position_base, causing + // the profile to overshoot the segment end. + if (tc->active) { + if (tc->term_cond == TC_TERM_COND_TANGENT && tc->shared_9d.profile.valid) { + // If a pending branch exists, its exit velocity reflects the + // new feed — use it instead of the stale main profile. + int bv = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + if (bv && tc->shared_9d.branch.profile.valid && + tc->shared_9d.branch.profile.computed_feed_scale > 0.001) { + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.branch.profile); + prev_exit_feed_scale = tc->shared_9d.branch.feed_scale; + } else { + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + prev_exit_feed_scale = tc->shared_9d.profile.computed_feed_scale; + } + } else { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + } + prev_exit_vel_known = true; + continue; + } + + // Check if profile needs (re)computation + // Recompute if: + // 1. Profile not valid + // 2. Marked for recomputation (TC_PLAN_UNTOUCHED) + // 3. Profile's feed scale doesn't match current feed scale (race condition fix) + TCPlanningState opt_state = (TCPlanningState)__atomic_load_n( + (int*)&tc->shared_9d.optimization_state, __ATOMIC_ACQUIRE); + + bool needs_recompute = false; + bool is_first_profile = !tc->shared_9d.profile.valid; + + if (!tc->shared_9d.profile.valid) { + needs_recompute = true; + } else if (opt_state == TC_PLAN_UNTOUCHED) { + needs_recompute = true; + } + + // Also check for feed scale mismatch (handles race condition where + // segments are invalidated by index but RT shifts queue indices). + // Use pass-level snapshot for consistency across all segments. + // + // SKIP this check when a cursor walk is active — the cursor walk is + // in the process of recomputing all segments at the new feed. + // Overwriting here would fight the cursor walk with stale committed_feed. + double seg_feed = tpGetSnapshotFeedScale(tc, pass_feed_scale, pass_rapid_scale); + if (g_recompute_cursor == 0 && !needs_recompute && tc->shared_9d.profile.valid) { + double profile_feed = tc->shared_9d.profile.computed_feed_scale; + if (fabs(profile_feed - seg_feed) > 0.005) { + needs_recompute = true; + } + } + + // Check if final velocity changed since last profile computation + // (e.g., kink velocity was set by a newly added segment) + // Compare the current desired exit velocity against what we asked for + // when the profile was last computed. This avoids infinite recomputation + // when Ruckig can't physically reach the desired velocity (reachability + // limited by segment length) — the achieved != desired gap is permanent, + // but if the desired hasn't changed, recomputing won't help. + if (!needs_recompute && tc->shared_9d.profile.valid) { + double desired_final_vel = atomicLoadDouble(&tc->shared_9d.final_vel); + double vel_limit_here = getEffectiveVelLimit(tp, tc); + double max_vel_here = applyVLimit(tp, tc, vel_limit_here * seg_feed); + double scaled_desired = fmin(desired_final_vel * seg_feed, max_vel_here); + scaled_desired = applyKinkVelCap(scaled_desired, desired_final_vel, max_vel_here, tc->kink_vel); + double prev_desired = tc->shared_9d.profile.computed_desired_fvel; + if (fabs(scaled_desired - prev_desired) > 1e-6) { + needs_recompute = true; + } + } + + // Check if entry velocity changed since last profile computation + // (e.g., backward pass reduced predecessor's exit velocity) + // Only check when prev_exit_vel comes from a real source (not the + // 0.0 default at the start of the optimization window) + if (!needs_recompute && tc->shared_9d.profile.valid && prev_exit_vel_known) { + double stored_entry = atomicLoadDouble(&tc->shared_9d.entry_vel); + if (fabs(prev_exit_vel - stored_entry) > 1e-6) { + needs_recompute = true; + } + } + + if (!needs_recompute) { + // Profile valid and up-to-date — read actual exit velocity from profile + if (tc->term_cond == TC_TERM_COND_TANGENT && tc->shared_9d.profile.valid) { + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + prev_exit_feed_scale = tc->shared_9d.profile.computed_feed_scale; + } else { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + } + prev_exit_vel_known = true; + continue; + } + + // Get velocities from backward pass + // v_entry = exit velocity of previous segment (propagated forward) + // v_exit = this segment's final velocity (computed by backward pass) + // Only TANGENT segments (promoted G61) have non-zero exit velocity; + // STOP (G61.1) and unpromoted EXACT segments must decelerate to zero. + double v_entry = prev_exit_vel; + double v_exit = (tc->term_cond == TC_TERM_COND_TANGENT) + ? atomicLoadDouble(&tc->shared_9d.final_vel) : 0.0; + + // --- Fix 4: Pessimistic initial profiles --- + // When building the FIRST profile for a segment, use exit velocity 0 + // regardless of what the backward pass says. The backward pass may + // not have converged yet (it runs iteratively, raising exit velocities + // over multiple optimizer cycles). If we use an unconverged optimistic + // value, the profile allows high-speed traversal — but then when the + // backward pass corrects downward, the replacement profile arrives + // while RT is already executing the stale one, causing jerk spikes. + // + // By starting pessimistic (decel to 0), the first profile is safe. + // Subsequent recomputes raise the exit velocity as the backward pass + // converges. These recomputes happen while the segment is still far + // from execution (outside the frozen zone), so the profile swap is safe. + // + // Exception 1: TC_BEZIER blend segments have pre-computed exit + // velocities from tpSetupBlend9D — deterministic, no convergence needed. + // + // Exception 2: Segments whose successor is a TC_BEZIER blend also + // have their final_vel set by tpSetupBlend9D at enqueue time. + // Forcing v_exit=0 here would poison the blend's v_entry (profiled + // with v0=0 while actual junction velocity is v_plan). + { + TC_STRUCT *next_seg = (k > 0) ? tcqBack_user(queue, -(k-1)) : NULL; + bool next_is_blend = (next_seg && next_seg->motion_type == TC_BEZIER); + if (tc->motion_type == TC_BEZIER || next_is_blend) + is_first_profile = false; + } + if (is_first_profile && v_exit > 0.0) { + v_exit = 0.0; + // Debug: log Fix 4 pessimistic override + { + static int fix4_dbg_count = 0; + if (fix4_dbg_count < 30) { + fix4_dbg_count++; + rtapi_print_msg(RTAPI_MSG_ERR, + "FIX4_DBG[id=%d]: first_profile v_exit forced 0 (was %.3f) type=%d target=%.3f\n", + tc->id, atomicLoadDouble(&tc->shared_9d.final_vel), tc->motion_type, tc->target); + } + } + } + + // prev_exit_vel is updated AFTER Ruckig compute (below) to use + // the actual achievable exit velocity, not the backward-pass ideal. + + // Store entry velocity for RT layer + atomicStoreDouble(&tc->shared_9d.entry_vel, v_entry); + + // Get per-segment feed scale from the pass-level snapshot. + // Using the same snapshot as the mismatch check above ensures + // the profile we write matches the feed_scale we decided on. + double feed_scale = tpGetSnapshotFeedScale(tc, pass_feed_scale, pass_rapid_scale); + + // Get base velocity limit (reqvel clamped to maxvel) + // vLimit is applied AFTER feed scaling (it's an absolute cap) + double vel_limit = getEffectiveVelLimit(tp, tc); + + // Handle feed hold (0% override) specially: create a "stay at start" profile + // This is a minimal valid profile that keeps position at 0 indefinitely + // RT will detect feed hold via computed_feed_scale and wait for feed restore + if (feed_scale < 0.001) { + // Don't clobber a valid non-hold profile with a feed-hold + // profile. The committed-feed mechanism ensures this pass + // sees feed_scale≈0 while the reactive path (branch / + // cursor walk) will restore motion at the new feed. + // If committed-feed ever drifts, this guard is the + // last line of defence against erasing good profiles. + if (tc->shared_9d.profile.valid && + tc->shared_9d.profile.computed_feed_scale > 0.001) { + prev_exit_vel = (tc->term_cond == TC_TERM_COND_TANGENT) + ? profileExitVelUnscaled(&tc->shared_9d.profile) : 0.0; + prev_exit_feed_scale = tc->shared_9d.profile.computed_feed_scale; + prev_exit_vel_known = true; + continue; + } + createFeedHoldProfile(tc, vel_limit, tp->vLimit, "computeRuckig_forward"); + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + } else { + // Normal profile computation + + // Kinematic limits + // Apply vLimit AFTER feed scaling (vLimit is absolute, not scaled) + double max_vel = applyVLimit(tp, tc, vel_limit * feed_scale); + double max_acc = tcGetTangentialMaxAccel_9D_user(tc); + double max_jrk = tc->maxjerk > 0 ? tc->maxjerk : default_jerk; + + // Apply feed override to entry/exit velocities. + // shared_9d.final_vel stores UNSCALED (absolute) velocities — + // consistent with kink_vel and future blend_vel. + // Feed override is applied here, once, in the Ruckig profile. + // Clamp to max_vel to avoid Ruckig ErrorInvalidInput (-100). + // + // Restore absolute velocity using the PREVIOUS segment's feed scale + // (the one profileExitVelUnscaled divided by). At feed/rapid + // boundaries (e.g. G1→G0), feed_scale ≠ prev_exit_feed_scale, + // and using feed_scale would halve/double the entry velocity. + double scaled_v_entry = fmin(v_entry * prev_exit_feed_scale, max_vel); + + // Cap entry velocity by previous segment's kink limit + { + TC_STRUCT *prev_tc_kink = (k < optimization_depth - 1) ? tcqBack_user(queue, -(k+1)) : NULL; + double prev_kink = (prev_tc_kink && prev_tc_kink->kink_vel > 0) ? prev_tc_kink->kink_vel : -1.0; + if (prev_kink > 0) + scaled_v_entry = fmin(scaled_v_entry, prev_kink); + } + + double scaled_v_exit = fmin(v_exit * feed_scale, max_vel); + scaled_v_exit = applyKinkVelCap(scaled_v_exit, v_exit, max_vel, tc->kink_vel); + + // Record the desired exit velocity (pre-reachability cap) for + // convergence detection — see RECOMP_FVEL check above. + double desired_fvel_for_profile = scaled_v_exit; + + // Reachability cap: ensure exit velocity is physically achievable + // from entry velocity within segment distance. By time-reversal + // symmetry, jerkLimitedMaxEntryVelocity(v, d, a, j) gives the + // max velocity at the other end — works for both accel and decel. + // Without this cap, Ruckig returns Working (overshooting profiles). + { + double v_fwd = jerkLimitedMaxEntryVelocity( + scaled_v_entry, tc->target, max_acc, max_jrk); + double v_bwd = jerkLimitedMaxEntryVelocity( + scaled_v_exit, tc->target, max_acc, max_jrk); + scaled_v_exit = fmin(scaled_v_exit, v_fwd); + scaled_v_entry = fmin(scaled_v_entry, v_bwd); + } + + try { + RuckigProfileParams rp = {scaled_v_entry, scaled_v_exit, + max_vel, max_acc, max_jrk, tc->target, + feed_scale, vel_limit, tp->vLimit, desired_fvel_for_profile}; + + if (computeAndStoreProfile(tc, rp)) { + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_FINALIZED); + + // Propagate actual achievable exit velocity (un-scaled) + prev_exit_vel = profileExitVelUnscaled(&tc->shared_9d.profile); + prev_exit_feed_scale = feed_scale; + prev_exit_vel_known = true; + + // Debug: log profile params for blend segments and neighbors + { + static int blend_fwd_dbg = 0; + // Check if this or the next segment is a bezier + TC_STRUCT *next_tc = (k > 0) ? tcqBack_user(queue, -(k-1)) : NULL; + bool is_blend_neighbor = (next_tc && next_tc->motion_type == TC_BEZIER); + if ((tc->motion_type == TC_BEZIER || is_blend_neighbor) && blend_fwd_dbg < 50) { + blend_fwd_dbg++; + rtapi_print_msg(RTAPI_MSG_ERR, + "BLEND_FWD[id=%d type=%d]: v_in=%.3f v_out=%.3f " + "sc_in=%.3f sc_out=%.3f first=%d maxv=%.3f " + "target=%.4f pv0=%.3f pvf=%.3f prev_exit=%.3f\n", + tc->id, tc->motion_type, v_entry, v_exit, + scaled_v_entry, scaled_v_exit, is_first_profile, + max_vel, tc->target, + tc->shared_9d.profile.v[0], + tc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES], + prev_exit_vel); + } + } + + // One-step backtrack: fix backward reachability gap + // Skip active segment — rewriting triggers STOPWATCH_RESET + // with velocity discontinuity worse than the chain gap. + TC_STRUCT *prev_seg = (k < optimization_depth - 1) + ? tcqBack_user(queue, -(k+1)) : NULL; + if (prev_seg && !prev_seg->active + && prev_seg->term_cond == TC_TERM_COND_TANGENT + && prev_seg->shared_9d.profile.valid) { + double prev_v_end = prev_seg->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double curr_v0 = tc->shared_9d.profile.v[0]; + if (prev_v_end > curr_v0 + 0.01) { + double p_entry = prev_seg->shared_9d.profile.v[0]; + double p_vel_limit = getEffectiveVelLimit(tp, prev_seg); + double p_feed = prev_seg->shared_9d.profile.computed_feed_scale; + if (p_feed < 0.001) p_feed = feed_scale; + double p_max_vel = applyVLimit(tp, prev_seg, p_vel_limit * p_feed); + double p_max_acc = tcGetTangentialMaxAccel_9D_user(prev_seg); + double p_max_jrk = prev_seg->maxjerk > 0 ? prev_seg->maxjerk : default_jerk; + RuckigProfileParams rp_prev = {p_entry, curr_v0, + p_max_vel, p_max_acc, p_max_jrk, prev_seg->target, + p_feed, p_vel_limit, tp->vLimit, curr_v0}; + if (computeAndStoreProfile(prev_seg, rp_prev)) { + atomicStoreInt((int*)&prev_seg->shared_9d.optimization_state, + TC_PLAN_FINALIZED); + } + } + } + } else { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + prev_exit_vel_known = true; + rtapi_print_msg(RTAPI_MSG_ERR, "Ruckig fail seg=%d v[%.1f->%.1f] " + "max_v=%.2f len=%.3f term=%d", + tc->id, v_entry, v_exit, + max_vel, tc->target, tc->term_cond); + } + } catch (std::exception &e) { + __atomic_store_n(&tc->shared_9d.profile.valid, 0, __ATOMIC_RELEASE); + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + prev_exit_vel_known = true; + rtapi_print_msg(RTAPI_MSG_ERR, "Ruckig exception seg=%d: %s", tc->id, e.what()); + } + } + + // Feed hold: next segment starts from rest + if (feed_scale < 0.001) { + prev_exit_vel = 0.0; + prev_exit_feed_scale = 1.0; + prev_exit_vel_known = true; + } + } + + // --- Backward fixup pass --- + // The forward pass computes each segment's profile independently: it scales + // v_exit by feed_scale and applies reachability caps within the segment. + // But it doesn't check whether the SUCCESSOR can accept the exit velocity. + // + // At feed_scale=1.0 this is fine — the backward pass already ensures + // consistency at unscaled velocities. At feed_scale>1.0, scaling can push + // a predecessor's exit beyond what the successor's reachability allows + // (short segment + low exit kink = can't decelerate fast enough). + // + // This fixup walks backward: for each (A, B) pair, compute the max entry + // velocity B can accept given B's profile exit velocity, length, and + // kinematic limits. If A's profile exit exceeds that, recompute A with + // the capped exit. + for (int k = 0; k < optimization_depth - 1; k++) { + TC_STRUCT *tc_B = tcqBack_user(queue, -k); + TC_STRUCT *tc_A = tcqBack_user(queue, -(k + 1)); + if (!tc_A || !tc_B) continue; + if (tc_A->active) continue; // Active segment managed by branch/merge + if (!tc_A->shared_9d.profile.valid || !tc_B->shared_9d.profile.valid) continue; + if (tc_A->term_cond != TC_TERM_COND_TANGENT) continue; // Only blending segments + + double A_v_exit = tc_A->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double B_v_entry = tc_B->shared_9d.profile.v[0]; + + // If A's exit already matches B's entry, no fixup needed + if (A_v_exit <= B_v_entry + 0.01) continue; + + // Compute max entry velocity B can accept + double B_v_exit_scaled = tc_B->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + double B_acc = tcGetTangentialMaxAccel_9D_user(tc_B); + double B_jrk = tc_B->maxjerk > 0 ? tc_B->maxjerk : default_jerk; + double B_max_entry = jerkLimitedMaxEntryVelocity( + B_v_exit_scaled, tc_B->target, B_acc, B_jrk); + + // Also cap by B's max_vel (can't enter faster than segment allows) + double B_feed = tc_B->shared_9d.profile.computed_feed_scale; + double B_vel_limit = getEffectiveVelLimit(tp, tc_B); + double B_max_vel = applyVLimit(tp, tc_B, B_vel_limit * B_feed); + B_max_entry = fmin(B_max_entry, B_max_vel); + + // Also cap by kink at the junction (stored on A) + if (tc_A->kink_vel > 0) + B_max_entry = fmin(B_max_entry, tc_A->kink_vel); + + if (A_v_exit <= B_max_entry + 0.01) continue; + + // A's exit exceeds what B can accept — recompute A with capped exit + double A_feed = tc_A->shared_9d.profile.computed_feed_scale; + if (A_feed < 0.001) continue; + + double A_vel_limit = getEffectiveVelLimit(tp, tc_A); + double A_max_vel = applyVLimit(tp, tc_A, A_vel_limit * A_feed); + double A_acc = tcGetTangentialMaxAccel_9D_user(tc_A); + double A_jrk = tc_A->maxjerk > 0 ? tc_A->maxjerk : default_jerk; + + // Reconstruct A's entry from its profile + double A_v_entry_scaled = tc_A->shared_9d.profile.v[0]; + + // Capped exit velocity for A + double capped_exit = B_max_entry; + + // Reachability: can A reach capped_exit from its entry within its length? + double A_v_fwd = jerkLimitedMaxEntryVelocity( + A_v_entry_scaled, tc_A->target, A_acc, A_jrk); + capped_exit = fmin(capped_exit, A_v_fwd); + + try { + RuckigProfileParams rp = {A_v_entry_scaled, capped_exit, + A_max_vel, A_acc, A_jrk, tc_A->target, + A_feed, A_vel_limit, tp->vLimit, capped_exit}; + computeAndStoreProfile(tc_A, rp); + } catch (...) { + // Leave A's profile as-is if Ruckig fails + } + } + + // FIXUP12 removed: computeChainExitCap and safe-endpoint chain extension + // handle downstream constraints, making per-index fixups redundant. + + return 0; +} + +/** + * @brief Apply computed limiting velocities back to queue + * + * Writes optimized velocities from SmoothingData back to + * TC_STRUCT elements using atomic operations. + */ +int applyLimitingVelocities_9D(TC_QUEUE_STRUCT *queue, + const SmoothingData &smoothing, + int optimization_depth) +{ + if (!queue) return -1; + if (optimization_depth <= 0) return 0; + + // Walk forward through queue and update velocities + for (int k = 0; k < optimization_depth && k < (int)smoothing.v_smooth.size(); ++k) { + // Use tcqBack to access from end of queue + TC_STRUCT *tc = tcqBack_user(queue, -k); + if (!tc) break; + + // Update final velocity using atomic operations + double v_new = smoothing.v_smooth[k]; + + // For TANGENT segments (promoted by kink computation), the kink velocity + // is an upper bound set by tc->finalvel. The backward pass may compute + // a lower value (it should), but must not exceed the kink velocity. + // Also ensure we don't overwrite a kink velocity with 0 from a stale + // backward pass that didn't yet see the promotion. + if (tc->term_cond == TC_TERM_COND_TANGENT && tc->finalvel > 0.0) { + // Clamp smoothed velocity to kink limit, but don't reduce below + // what kink computation set if backward pass returned 0 (stale) + if (v_new < 1e-6) { + v_new = tc->finalvel; // Backward pass was stale, use kink vel + } else { + v_new = fmin(v_new, tc->finalvel); + } + } + + // Write to shared_9d structure atomically + atomicStoreDouble(&tc->shared_9d.final_vel, v_new); + atomicStoreDouble(&tc->shared_9d.final_vel_limit, v_new); + + // Update planning state atomically + // BUT preserve TC_PLAN_UNTOUCHED - that means "needs profile recomputation" + TCPlanningState current_state = (TCPlanningState)__atomic_load_n( + (int*)&tc->shared_9d.optimization_state, __ATOMIC_ACQUIRE); + if (current_state != TC_PLAN_UNTOUCHED) { + atomicStoreInt((int*)&tc->shared_9d.optimization_state, TC_PLAN_SMOOTHED); + } + + // CRITICAL FIX: Update finalvel for segment handoff + // RT uses tc->finalvel in tpCompleteSegment() when term_cond == TC_TERM_COND_TANGENT + // to set nexttc->currentvel. If this doesn't match shared_9d.final_vel, + // we get velocity discontinuities causing 10x acceleration spikes. + // This field is used for: + // 1. Segment-to-segment handoff (tp.c:3974) + // 2. Abort detection (tp.c:3326) + // 3. Blend velocity calculations + tc->finalvel = v_new; + + // Also update target_vel for backward compatibility + // (RT layer may still read this for non-9D planners) + tc->target_vel = v_new; + } + + return 0; +} + +//============================================================================ +// PUBLIC API (C-COMPATIBLE) +//============================================================================ + +/** + * @brief Main optimization entry point + * + * Runs lookahead optimization on queued motion segments. + * This is called from userspace context (NOT RT). + */ +extern "C" int tpOptimizePlannedMotions_9D(TP_STRUCT * const tp, int optimization_depth) +{ + if (!tp) return -1; + + // Branch/Merge architecture for feed override handling + // Handles: merge taken branches, discard missed branches, create new branches + manageBranches(tp); + + // Get queue and validate it's properly initialized + TC_QUEUE_STRUCT *queue = &tp->queue; + + // Safety check: ensure queue is valid and initialized + if (queue->size <= 0) return 0; // Queue size not set + + int queue_len = tcqLen_user(queue); + + // Need at least 1 segment to optimize + if (queue_len < 1) return 0; + + // Limit optimization depth to queue length + int depth = optimization_depth; + if (depth > queue_len) { + depth = queue_len; + } + if (depth > MAX_LOOKAHEAD_DEPTH) { + depth = MAX_LOOKAHEAD_DEPTH; + } + + // Step 1: Compute limiting velocities (backward pass) + int result = computeLimitingVelocities_9D(queue, depth, g_smoothing_data); + if (result != 0) return result; + + // Step 2: Apply velocities back to queue (peak smoothing skipped — Ruckig handles jerk-aware smoothing) + result = applyLimitingVelocities_9D(queue, g_smoothing_data, depth); + if (result != 0) return result; + + // Step 3: Compute Ruckig trajectories for segments + // Forward pass: use optimized velocities as entry/exit constraints + result = computeRuckigProfiles_9D(tp, queue, depth); + if (result != 0) return result; + + return 0; +} + +/** + * @brief Finalize segment and enqueue with optimization + * + * Finalizes a trajectory segment and adds it to the queue, + * then triggers optimization if queue depth sufficient. + */ +extern "C" int tpFinalizeAndEnqueue_9D(TP_STRUCT * const tp, TC_STRUCT * const tc) +{ + if (!tp || !tc) return -1; + + // TODO: Implement segment finalization + // This is a stub - segments are finalized in tpAddLine_9D + + // The function should: + // 1. Finalize segment parameters + // 2. Add to queue using tcqPut() + // 3. Trigger optimization if queue depth >= optimization_depth + + return 0; +} + +/** + * @brief Clear planning state (e.g., on abort/reset) + */ +extern "C" int tpClearPlanning_9D(TP_STRUCT * const tp) +{ + if (!tp) return -1; + + // Clear smoothing data + g_smoothing_data = SmoothingData(); + + // Reset all persistent feed override / recomputation state + // Without this, state from the previous program leaks into the next one, + // causing non-deterministic behavior between identical runs. + g_last_feed_scale = 1.0; + g_last_replan_time_ms = 0.0; + g_committed_feed = -1.0; + g_committed_rapid = -1.0; + g_recompute_cursor = 0; + g_recompute_feed_scale = 1.0; + g_recompute_rapid_scale = 1.0; + g_recompute_first_batch_done = false; + g_next_feed_scale = -1.0; + g_next_rapid_scale = -1.0; + g_commit_segment = 1; + g_segments_per_tick = 3.0; + g_adaptive_horizon = AdaptiveHorizon(); + + return 0; +} diff --git a/src/emc/motion_planning/motion_planning_9d.hh b/src/emc/motion_planning/motion_planning_9d.hh new file mode 100644 index 00000000000..d27eb0a3aef --- /dev/null +++ b/src/emc/motion_planning/motion_planning_9d.hh @@ -0,0 +1,529 @@ +/******************************************************************** +* Description: motion_planning_9d.hh +* Tormach 9D trajectory planner - Userspace planning layer +* +* Ported from Tormach LinuxCNC implementation +* Provides lookahead optimization for 9-axis coordinated motion +* +* Author: Tormach (original), Port by LinuxCNC community +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +* ARCHITECTURE: +* This is the USERSPACE planning layer that runs optimization +* algorithms with lookahead. It uses C++ with dynamic allocation +* (std::vector) which is FORBIDDEN in RT context. +* +* Communication with RT layer is via atomic operations only. +* RT layer (tp.c) only READS pre-computed velocities. +* +********************************************************************/ +#ifndef MOTION_PLANNING_9D_HH +#define MOTION_PLANNING_9D_HH + +#include "posemath.h" +#include "tc_types.h" +#include "tp_types.h" + +#include +#include + +// Type alias for velocity/distance vectors +using SmoothingVector = std::vector; + +/** + * @struct SmoothingData + * @brief Lookahead buffer for velocity optimization + * + * Caches velocity, distance, and time data for segments in the + * lookahead window. Used by backward velocity pass and peak smoothing. + * + * This structure uses C++ std::vector for dynamic allocation. + * IT IS ONLY USED IN USERSPACE - never accessed from RT context. + */ +struct SmoothingData { + // Default constructor creates empty vectors. + // computeLimitingVelocities_9D pushes the head dummy element. + SmoothingData() = default; + + /** + * Pre-allocate capacity to avoid reallocation during optimization + * @param capacity Maximum expected lookahead depth + */ + void reserve(size_t capacity) { + ignore.reserve(capacity); + ds.reserve(capacity); + v_smooth.reserve(capacity); + t.reserve(capacity); + t_orig.reserve(capacity); + } + + // Core optimization data (always present) + std::vector ignore; // Ignore rapid moves in smoothing + SmoothingVector ds; // Distance samples + SmoothingVector v_smooth; // Smoothed velocity profile + SmoothingVector t; // Time at each sample + SmoothingVector t_orig; // Original time values +}; + + +//============================================================================ +// C-COMPATIBLE INTERFACE +// These functions are called from C code (tp.c) in the RT layer +//============================================================================ + +//============================================================================ +// PREDICTIVE HANDOFF CONFIGURATION +//============================================================================ + +/** + * @struct PredictiveHandoffConfig + * @brief Configuration parameters for branch/merge feed override system + * + * These parameters control timing and buffer management for + * real-time trajectory replanning (feed override, etc.). + * + * Branch/Merge Timing: + * handoff_horizon_ms: How far ahead to place the handoff point + * branch_window_ms: Size of the window RT has to take the branch + * + * Example: horizon=100ms, window=50ms + * - Branch handoff at elapsed_time + 100ms + * - RT must take it between [100ms, 150ms) + * - If RT reaches 150ms without taking, branch is discarded + */ +struct PredictiveHandoffConfig { + // Branch/Merge timing (milliseconds) + double handoff_horizon_ms; // How far ahead to predict (must exceed worst-case latency) + double branch_window_ms; // Window size for RT to take branch + double min_buffer_time_ms; // Alarm if buffer drops below this + double target_buffer_time_ms; // Optimizer aims to maintain this + double max_buffer_time_ms; // Stop adding segments above this + double feed_override_debounce_ms; // Minimum time between branch computations (ignores rapid changes) + + // System parameters (from INI/motion config) + double servo_cycle_time_sec; // Servo thread period in seconds (from [EMCMOT] SERVO_PERIOD) + double default_max_jerk; // Fallback jerk limit (from [TRAJ] MAX_LINEAR_JERK) + + // Default constructor + PredictiveHandoffConfig() : + handoff_horizon_ms(100.0), + branch_window_ms(50.0), + min_buffer_time_ms(100.0), + target_buffer_time_ms(200.0), + max_buffer_time_ms(500.0), + feed_override_debounce_ms(50.0), + servo_cycle_time_sec(0.001), // 1ms default (conservative) + default_max_jerk(1e9) // Match initraj.cc default + {} +}; + +/** + * @struct PredictedState + * @brief Predicted kinematic state at a future time + */ +struct PredictedState { + double position; // Absolute position along segment + double velocity; + double acceleration; + double jerk; + bool valid; + + PredictedState() : position(0), velocity(0), acceleration(0), jerk(0), valid(false) {} +}; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Add linear move to shared memory queue (userspace planning) + * + * This is the userspace version of tpAddLine for the 9D dual-layer architecture. + * It creates a TC_STRUCT, writes it to shared memory, and triggers optimization. + * + * @param tp Trajectory planner structure (from usrmotGetEmcmotInternal()->coord_tp) + * @param end End position + * @param type Motion type (EMC_MOTION_TYPE_FEED, etc.) + * @param vel Requested velocity + * @param ini_maxvel Maximum velocity + * @param acc Acceleration + * @param enables Feed/spindle scale enable bits (FS_ENABLED, SS_ENABLED, etc.) + * @param tag State tag for tracking + * @return 0 on success, error code otherwise + */ +int tpAddLine_9D( + TP_STRUCT * const tp, + EmcPose end, + int type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + struct state_tag_t const &tag); + +/** + * @brief Add circular arc segment to planner queue (9D userspace version) + * + * Userspace function that initializes arc geometry, computes joint-space + * segment via userspace kinematics (if enabled), and enqueues the segment. + * + * @param tp Trajectory planner structure (shared memory) + * @param end End position of the arc + * @param center Arc center point + * @param normal Arc plane normal vector + * @param turn Number of full turns (+ for CCW, - for CW looking along normal) + * @param type Canon motion type + * @param vel Requested velocity + * @param ini_maxvel Maximum velocity from INI + * @param acc Acceleration limit + * @param enables Feed/spindle scale enable bits + * @param tag State tag for tracking + * @return 0 on success, error code otherwise + */ +int tpAddCircle_9D( + TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, + int type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + struct state_tag_t const &tag); + +/** + * @brief Main optimization entry point + * + * Runs lookahead optimization on queued motion segments. + * Called from userspace context (NOT RT). + * + * @param tp Trajectory planner structure + * @param optimization_depth Number of segments to look ahead + * @return 0 on success, error code otherwise + */ +int tpOptimizePlannedMotions_9D(TP_STRUCT * const tp, int optimization_depth); + +/** + * @brief Finalize segment and enqueue with optimization + * + * Finalizes a trajectory segment and adds it to the queue, + * then triggers optimization if queue depth sufficient. + * + * @param tp Trajectory planner structure + * @param tc Trajectory component to finalize + * @return 0 on success, error code otherwise + */ +int tpFinalizeAndEnqueue_9D(TP_STRUCT * const tp, TC_STRUCT * const tc); + +/** + * @brief Clear planning state (e.g., on abort/reset) + * + * @param tp Trajectory planner structure + * @return 0 on success + */ +int tpClearPlanning_9D(TP_STRUCT * const tp); + +//============================================================================ +// PREDICTIVE HANDOFF FUNCTIONS (C-compatible) +//============================================================================ + +/** + * @brief Set predictive handoff configuration parameters + * + * Called from initraj.cc after parsing INI file. All timing values are in milliseconds + * except servo_cycle_time_sec which is in seconds. + * + * @param handoff_horizon_ms How far ahead to predict handoff point + * @param branch_window_ms Window size for RT to take branch + * @param min_buffer_time_ms Alarm if buffer drops below this + * @param target_buffer_time_ms Optimizer aims to maintain this + * @param max_buffer_time_ms Stop adding segments above this + * @param feed_override_debounce_ms Minimum time between branch computations + * @param servo_cycle_time_sec Servo thread period in seconds + * @param default_max_jerk Fallback jerk limit (in user units, typically mm/s³) + */ +void setHandoffConfig(double handoff_horizon_ms, + double branch_window_ms, + double min_buffer_time_ms, + double target_buffer_time_ms, + double max_buffer_time_ms, + double feed_override_debounce_ms, + double servo_cycle_time_sec, + double default_max_jerk); + +/** + * @brief Calculate how much buffered motion time remains + * + * Sum of remaining time in active segment + all queued segments. + * + * @param tp Trajectory planner structure + * @return Buffer time in milliseconds + */ +double calculateBufferTimeMs(TP_STRUCT *tp); + +/** + * @brief Get the configured default max jerk (from handoff config) + * @return Jerk limit in user units/s³ (e.g. mm/s³) + */ +double getDefaultMaxJerk(); + +/** + * @brief Manage branches for feed override using branch/merge architecture + * + * Called each iteration of the optimization loop. Handles: + * 1. Merging taken branches (branch.taken=1 -> canonical) + * 2. Discarding missed branches (elapsed > window_end) + * 3. Creating new branches when feed scale changes + * + * @param tp Trajectory planner structure + */ +void manageBranches(TP_STRUCT *tp); + +/** + * @brief Request a stop branch for abort (called from task layer) + * + * Called by emcTrajAbort() before sending EMCMOT_ABORT to RT. + * Computes a smooth stop branch using existing Ruckig infrastructure. + * + * @param tp Trajectory planner structure (shared memory) + * @return 0 on success, -1 on error + */ +int tpRequestAbortBranch_9D(TP_STRUCT *tp); + +/** + * @brief Check for feed override changes (legacy wrapper) + * + * This function is kept for API compatibility with existing callers. + * It simply forwards to manageBranches(). + * + * @deprecated Use manageBranches() directly + * @param tp Trajectory planner structure + */ +void checkFeedOverride(TP_STRUCT *tp); + +/** + * @brief Get minimum buffer time threshold (for critical alarms) + * @return Minimum buffer time in milliseconds + */ +double getBufferMinTimeMs(void); + +/** + * @brief Get target buffer time threshold (for adaptive optimization) + * @return Target buffer time in milliseconds + */ +double getBufferTargetTimeMs(void); + +/** + * @brief Initialize predictive handoff system for feed override handling + * + * Should be called after setHandoffConfig() has been used to set parameters. + * If setHandoffConfig() was not called, defaults are used. + * + * @return 0 on success + */ +int initPredictiveHandoff(void); + +#ifdef __cplusplus +} +#endif + + +//============================================================================ +// C++ INTERNAL FUNCTIONS +// These are only called from within the planning layer (C++ code) +//============================================================================ + +#ifdef __cplusplus + +/** + * @brief Compute limiting velocities using backward propagation + * + * Walks backward through the lookahead window and computes the + * maximum safe final velocity for each segment based on kinematics. + * + * This is the core "rising tide" optimization algorithm. + * + * @param queue Segment queue + * @param optimization_depth Number of segments to optimize + * @param smoothing Output smoothing data + * @return 0 on success + */ +int computeLimitingVelocities_9D(TC_QUEUE_STRUCT *queue, + int optimization_depth, + SmoothingData &smoothing); + +/** + * @brief Apply peak smoothing to velocity profile + * + * Flattens local velocity peaks to improve motion smoothness. + * Searches for peaks and troughs within time window and clamps + * peaks to minimum of bracketing edges. + * + * @param smoothing Smoothing data with velocity profile + * @param optimization_depth Number of segments + * @param t_window Time window for peak detection + * @return 0 on success + */ +int tpDoPeakSmoothing_9D(SmoothingData &smoothing, + int optimization_depth, + double t_window); + +/** + * @brief Compute optimal velocity at segment boundary + * + * Applies three constraints to determine safe velocity: + * 1. Backward kinematic limit (can we reach final velocity?) + * 2. Current segment velocity limit (geometry + feed override) + * 3. Previous segment velocity limit (with kink handling) + * + * @param tc Current segment + * @param prev_tc Previous segment + * @param v_f_this Final velocity target for current segment + * @param opt_step Optimization step index (for debugging) + * @return Optimal velocity (minimum of all constraints) + */ +double tpComputeOptimalVelocity_9D(TC_STRUCT const * const tc, + TC_STRUCT const * const prev_tc, + double v_f_this, + int opt_step); + +/** + * @brief Apply computed limiting velocities back to queue + * + * Writes optimized velocities from SmoothingData back to + * TC_STRUCT elements in queue using atomic operations. + * + * @param queue Segment queue + * @param smoothing Smoothing data with optimized velocities + * @param optimization_depth Number of segments + * @return 0 on success + */ +int applyLimitingVelocities_9D(TC_QUEUE_STRUCT *queue, + const SmoothingData &smoothing, + int optimization_depth); + +//============================================================================ +// PREDICTIVE HANDOFF INTERNAL FUNCTIONS (C++ only) +//============================================================================ + +/** + * @brief Predict kinematic state at a future time + * + * Samples the current Ruckig profile at the specified elapsed time. + * + * @param tc Trajectory component + * @param target_elapsed_time Time to predict state at + * @return Predicted state (check .valid flag) + */ +PredictedState predictStateAtTime(TC_STRUCT *tc, double target_elapsed_time); + +/** + * @brief Commit a branch for RT to take + * + * Writes branch data and sets the branch.valid flag. + * RT will take this branch when handoff_time is reached (if before window_end). + * + * @param shared Shared optimization data + * @param main_profile Main trajectory (position control, ends at ~0 velocity) + * @param brake_profile Optional brake trajectory (velocity control), NULL if not needed + * @param brake_target_vel Expected final velocity of brake profile (0 if no brake) + * @param brake_end_position Position where brake ends and main begins + * @param handoff_time RT elapsed_time when branch handoff should occur + * @param handoff_position Position at handoff (for merge reconciliation) + * @param feed_scale Feed scale this branch was computed for + * @param window_end_time Deadline - branch is stale if RT past this + * @return true if committed, false if previous branch still pending (valid && !taken) + */ +bool commitBranch(shared_optimization_data_9d_t *shared, + const ruckig_profile_t *main_profile, + const ruckig_profile_t *brake_profile, + double brake_target_vel, + double brake_end_position, + double handoff_time, + double handoff_position, + double feed_scale, + double window_end_time, + double expected_exit_vel = 0.0); + +/** + * @brief Compute a branch trajectory for feed override change + * + * Computes new Ruckig trajectory starting from predicted state + * at handoff horizon, with new feed scale applied. Uses achievable + * feed cascade: if segment is too short for requested feed, applies + * what's achievable and passes remainder to next segment. + * + * Achievable Feed Cascade: + * - Calculates minimum exit velocity achievable without overshoot + * - If requested feed isn't achievable, uses achievable portion + * - Stores achieved_exit_vel for cascade to next segment + * - Never overshoots - physically safe for CNC applications + * + * Phase 4 TODO (Blending Integration): + * - With segment blending, exit velocity becomes non-zero + * - achieved_exit_vel becomes entry velocity constraint for next segment + * - This eliminates jerk ramp-up/ramp-down overhead at boundaries + * - Short segment sequences will converge to target feed much faster + * - See also: computeRuckigProfiles_9D for Phase 4 entry velocity handling + * + * @param tp Trajectory planner structure + * @param tc Active trajectory component + * @param new_feed_scale New feed scale (0.0 to 1.0+) + * @return true if branch was successfully computed and committed + */ +bool computeBranch(TP_STRUCT *tp, TC_STRUCT *tc, double new_feed_scale); + +/** + * @brief Invalidate next N segments for cascade re-optimization + * + * Marks segments for re-optimization after feed override change. + * Only invalidates next N segments to prevent starvation. + * + * Phase 4 TODO: When blending is implemented, this should also + * propagate achieved_exit_vel from current segment to next segment's + * entry velocity constraint. + * + * @param tp Trajectory planner structure + * @param n Number of segments to invalidate + */ +void invalidateNextNSegments(TP_STRUCT *tp, int n, int start_index = 1); + +//============================================================================ +// PHASE 4 PLANNING: SEGMENT BLENDING FOR FEED OVERRIDE +//============================================================================ +// +// Current State: +// - Feed override uses achievable feed cascade +// - Each segment stops at end (target_velocity = 0) +// - Cascade through short segments is inefficient due to jerk overhead +// +// Phase 4 Goals: +// - Enable non-zero exit velocities with segment blending +// - achieved_exit_vel from segment N becomes entry velocity for segment N+1 +// - Eliminates repeated jerk ramp-up/ramp-down at segment boundaries +// - Short segment sequences converge to target feed in O(1) distance +// instead of O(n) where n = number of segments +// +// Implementation Steps: +// 1. computeRuckigProfiles_9D: Read prev segment's achieved_exit_vel as +// entry velocity constraint (instead of always starting from 0) +// 2. computeBranch: Set target_velocity = tc->finalvel * new_feed_scale +// (non-zero) when blending is enabled +// 3. RT handoff: Ensure velocity continuity when taking branch +// 4. Segment transition: Pass exit velocity to next segment atomically +// +// Key Constraint: +// - Exit velocity must be consumable by next segment (path continuity) +// - If next segment is too short or has sharp angle, must reduce exit vel +// - This is already handled by existing lookahead optimization +// +//============================================================================ + +#endif // __cplusplus + +#endif // MOTION_PLANNING_9D_HH diff --git a/src/emc/motion_planning/motion_planning_9d_stubs.c b/src/emc/motion_planning/motion_planning_9d_stubs.c new file mode 100644 index 00000000000..9f4e5592dc5 --- /dev/null +++ b/src/emc/motion_planning/motion_planning_9d_stubs.c @@ -0,0 +1,13 @@ +/** + * @file motion_planning_9d_stubs.c + * @brief Stubs for RT module symbols needed by TP code + * + * NOTE: Most stubs have been removed now that tp.c is compiled into the + * userspace library (providing the real globals like emcmotStatus). + * This file is kept for any remaining stubs that may be needed. + */ + +#include +#include "motion.h" + +/* emcmotStatus and other globals are now provided by tp.c */ diff --git a/src/emc/motion_planning/motion_planning_9d_userspace.cc b/src/emc/motion_planning/motion_planning_9d_userspace.cc new file mode 100644 index 00000000000..7e2933c0f68 --- /dev/null +++ b/src/emc/motion_planning/motion_planning_9d_userspace.cc @@ -0,0 +1,1161 @@ +/** + * @file motion_planning_9d_userspace.cc + * @brief Userspace motion planning functions for 9D dual-layer architecture + * + * This file contains the userspace planning functions (tpAddLine_9D, tpAddCircle_9D) + * that write segments to the shared memory queue and trigger optimization. + * + * Includes blend creation: when adjacent segments can be blended, a Bezier9 + * blend segment is inserted between them for velocity continuity. + */ + +#include "motion_planning_9d.hh" +#include "userspace_kinematics.hh" +#include // memset +#include // clock_gettime for timing +#include // acos, sin, sqrt, fmin +#include "rtapi.h" // rtapi_print_msg for error reporting + +// blend_sizing.h has extern "C" guards +#include "blend_sizing.h" + +// C headers need extern "C" when included from C++ +extern "C" { +#include "motion.h" // emcmot_status_t, emcmot_config_t (needed before tp.h) +#include "tp.h" +#include "tc.h" +#include "tc_types.h" +#include "emcpos.h" +#include "posemath.h" +#include "blendmath.h" // pmCircleEffectiveMinRadius, findIntersectionAngle +#include "atomic_9d.h" // atomicStoreDouble +} + +// External queue functions (from motion_planning_9d.cc - will be made public) +extern "C" { + extern int tcqPut_user(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc); + extern int tcqLen_user(TC_QUEUE_STRUCT const * const tcq); + + // Access to global pointers (defined in usrmotintf.cc, initialized by milltask) + extern struct emcmot_struct_t *emcmotStruct; + extern struct emcmot_internal_t *emcmotInternal; + extern emcmot_config_t *emcmotConfig; +} + +/** + * @brief Initialize TC_STRUCT with basic parameters + * + * IMPORTANT: Must set indexer_jnum = -1 (no indexer) otherwise + * tpActivateSegment() will wait forever for rotary unlock. + */ +static int tcInit_9D(TC_STRUCT * const tc, + int motion_type, + int canon_motion_type, + double cycle_time) +{ + if (!tc) return -1; + + memset(tc, 0, sizeof(TC_STRUCT)); + + tc->motion_type = motion_type; + tc->canon_motion_type = canon_motion_type; + tc->cycle_time = cycle_time; + tc->active_depth = 1; + + /* Critical: -1 means no rotary indexer. If 0, RT thinks joint 0 is + * a locking indexer and waits forever for unlock (TP_ERR_WAITING). */ + tc->indexer_jnum = -1; + + /* Also set id to -1 like original tcInit() does */ + tc->id = -1; + + /* Default tangential accel ratio (memset zeroed it). + * Lines use 1.0 (no centripetal); arcs compute proper ratio in tpAddCircle_9D. */ + tc->acc_ratio_tan = 1.0; + + return 0; +} + +/** + * @brief Set motion parameters (velocity, acceleration) + */ +static int tcSetupMotion_9D(TC_STRUCT * const tc, + double vel, + double ini_maxvel, + double acc) +{ + if (!tc) return -1; + + tc->reqvel = vel; + tc->maxvel = ini_maxvel; // Use maxvel, not maxvel_geom (LinuxCNC difference) + tc->maxaccel = acc; + + return 0; +} + +/** + * @brief Validate shared memory queue state + * + * Performs comprehensive validation to detect uninitialized shared memory: + * - NULL pointer check + * - Magic number verification + * - queue_ready flag check + * - Queue size range validation + * - Atomic index bounds checks + * - Syncs goalPos to currentPos when queue is empty + * + * @param tp Trajectory planner structure + * @param out_queue_len Output: current queue length + * @param out_current_end Output: current end index + * @param func_name Function name for error messages (e.g., "tpAddLine_9D") + * @return 0 on success, -1 on error (with rtapi_print_msg already called) + */ +static int validateQueueState_9D(TP_STRUCT *tp, int *out_queue_len, int *out_current_end, const char *func_name) +{ + // CRITICAL: Safety check - shared memory must be initialized + if (!tp) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - tp pointer is NULL\n", func_name); + return -1; + } + + // Comprehensive validation to detect uninitialized shared memory + // Check magic number (set by tpInit()) + if (tp->magic != TP_MAGIC) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - magic number mismatch (got 0x%08x, expected 0x%08x)\n", + func_name, tp->magic, TP_MAGIC); + return -1; + } + + // Check queue ready flag (set after queue allocation) + if (!tp->queue_ready) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - queue_ready flag not set (value=%d)\n", func_name, tp->queue_ready); + return -1; + } + + TC_QUEUE_STRUCT *queue = &tp->queue; + + // Validate queue size (reasonable range check) + // LinuxCNC allows queue sizes from 32 up to several thousand + if (queue->size <= 0 || queue->size > 10000) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - invalid queue size (%d)\n", func_name, queue->size); + return -1; + } + + // Validate atomic indices are within valid range + // This catches garbage values from uninitialized memory + int current_start = __atomic_load_n(&queue->start_atomic, __ATOMIC_ACQUIRE); + int current_end = __atomic_load_n(&queue->end_atomic, __ATOMIC_ACQUIRE); + + if (current_start < 0 || current_start >= queue->size) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - start index out of bounds (%d, size=%d)\n", + func_name, current_start, queue->size); + return -1; + } + + if (current_end < 0 || current_end >= queue->size) { + rtapi_print_msg(RTAPI_MSG_ERR, "%s: FAIL - end index out of bounds (%d, size=%d)\n", + func_name, current_end, queue->size); + return -1; + } + + // CRITICAL FIX: Sync goalPos to currentPos when queue is empty. + // This ensures the first segment of a new program/run computes from + // the actual current machine position, not stale goalPos from a previous run. + // (Following Tormach's tpResetAtModeChange pattern, but in userspace) + int queue_len = (current_end - current_start + queue->size) % queue->size; + if (queue_len == 0) { + tp->goalPos = tp->currentPos; + } + + if (out_queue_len) { + *out_queue_len = queue_len; + } + if (out_current_end) { + *out_current_end = current_end; + } + + return 0; +} + +/** + * @brief Trigger adaptive optimization based on buffer time + * + * Adjusts optimization depth based on current buffer level: + * - Below min_buffer: optimize all queued segments (full depth) + * - Below target_buffer: optimize 16 segments + * - Above target_buffer: optimize 8 segments + * + * @param tp Trajectory planner structure + */ +static void triggerAdaptiveOptimization_9D(TP_STRUCT *tp) +{ + TC_QUEUE_STRUCT *queue = &tp->queue; + int queue_len_opt = tcqLen_user(queue); + + if (queue_len_opt >= 1) { + double buffer_ms = calculateBufferTimeMs(tp); + double min_buffer = getBufferMinTimeMs(); + double target_buffer = getBufferTargetTimeMs(); + + int opt_depth; + if (buffer_ms < min_buffer) { + opt_depth = queue_len_opt; + } else if (buffer_ms < target_buffer) { + opt_depth = 16; + } else { + opt_depth = 8; + } + + if (opt_depth > queue_len_opt) { + opt_depth = queue_len_opt; + } + + tpOptimizePlannedMotions_9D(tp, opt_depth); + } +} + +/** + * @brief Compute line geometry and length (using LinuxCNC's pmLine9Init) + */ +static int computeLineLengthAndTarget_9D(TC_STRUCT * tc, + EmcPose const *start, + EmcPose const *end) +{ + if (!tc || !start || !end) return -1; + + // Initialize PmLine9 structure (LinuxCNC version takes EmcPose, not PmVector) + pmLine9Init(&tc->coords.line, start, end); + + // Compute length (LinuxCNC uses pmLine9Target, not pmLine9Length) + tc->target = tc->nominal_length = pmLine9Target(&tc->coords.line); + + return 0; +} + +/** + * @brief Compute 9D tangent rate vector at the start or end of a segment + * + * Returns a 9-element vector representing the rate of axis position change + * per unit path velocity: axis_vel[a] = v_path * tangent9[a] + * + * For XYZ: unit tangent direction (magnitude 1 for xyz-dominant moves) + * For ABC/UVW: scaled by (subspace_length / segment_target) since these + * subspaces are interpolated proportionally to the dominant subspace. + * + * @param tc Segment to compute tangent for + * @param at_end true for end tangent, false for start tangent + * @param out Output 9-element tangent rate vector + * @return 0 on success, -1 on error + */ +static int tcGetTangentRate9D(TC_STRUCT const * const tc, int at_end, double out[9]) +{ + for (int i = 0; i < 9; i++) out[i] = 0.0; + + if (tc->target < 1e-15) return -1; + double inv_target = 1.0 / tc->target; + + PmCartesian xyz_tan; + + switch (tc->motion_type) { + case TC_LINEAR: { + // XYZ tangent: unit direction vector + xyz_tan = tc->coords.line.xyz.uVec; + // ABC/UVW: linear subspaces scaled by their proportion of path length + double abc_scale = tc->coords.line.abc.tmag_zero ? 0.0 + : tc->coords.line.abc.tmag * inv_target; + double uvw_scale = tc->coords.line.uvw.tmag_zero ? 0.0 + : tc->coords.line.uvw.tmag * inv_target; + out[0] = xyz_tan.x; out[1] = xyz_tan.y; out[2] = xyz_tan.z; + out[3] = tc->coords.line.abc.uVec.x * abc_scale; + out[4] = tc->coords.line.abc.uVec.y * abc_scale; + out[5] = tc->coords.line.abc.uVec.z * abc_scale; + out[6] = tc->coords.line.uvw.uVec.x * uvw_scale; + out[7] = tc->coords.line.uvw.uVec.y * uvw_scale; + out[8] = tc->coords.line.uvw.uVec.z * uvw_scale; + // For xyz-zero moves (abc or uvw dominant), re-normalize + if (tc->coords.line.xyz.tmag_zero) { + out[0] = out[1] = out[2] = 0.0; + if (!tc->coords.line.uvw.tmag_zero) { + out[6] = tc->coords.line.uvw.uVec.x; + out[7] = tc->coords.line.uvw.uVec.y; + out[8] = tc->coords.line.uvw.uVec.z; + double abc_uvw_scale = tc->coords.line.abc.tmag_zero ? 0.0 + : tc->coords.line.abc.tmag / tc->coords.line.uvw.tmag; + out[3] = tc->coords.line.abc.uVec.x * abc_uvw_scale; + out[4] = tc->coords.line.abc.uVec.y * abc_uvw_scale; + out[5] = tc->coords.line.abc.uVec.z * abc_uvw_scale; + } else { + // Pure ABC move + out[3] = tc->coords.line.abc.uVec.x; + out[4] = tc->coords.line.abc.uVec.y; + out[5] = tc->coords.line.abc.uVec.z; + } + } + break; + } + case TC_CIRCULAR: { + // XYZ tangent varies along the arc + double angle = at_end ? tc->coords.circle.xyz.angle : 0.0; + pmCircleTangentVector(&tc->coords.circle.xyz, angle, &xyz_tan); + // ABC/UVW: linear subspaces, constant rate + double abc_scale = tc->coords.circle.abc.tmag_zero ? 0.0 + : tc->coords.circle.abc.tmag * inv_target; + double uvw_scale = tc->coords.circle.uvw.tmag_zero ? 0.0 + : tc->coords.circle.uvw.tmag * inv_target; + out[0] = xyz_tan.x; out[1] = xyz_tan.y; out[2] = xyz_tan.z; + out[3] = tc->coords.circle.abc.uVec.x * abc_scale; + out[4] = tc->coords.circle.abc.uVec.y * abc_scale; + out[5] = tc->coords.circle.abc.uVec.z * abc_scale; + out[6] = tc->coords.circle.uvw.uVec.x * uvw_scale; + out[7] = tc->coords.circle.uvw.uVec.y * uvw_scale; + out[8] = tc->coords.circle.uvw.uVec.z * uvw_scale; + break; + } + case TC_BEZIER: { + // Bezier blend: get tangent at start or end + PmCartesian xyz_tan = {0,0,0}, abc_tan = {0,0,0}, uvw_tan = {0,0,0}; + double progress = at_end ? tc->coords.bezier.total_length : 0.0; + bezier9Tangent(&tc->coords.bezier, progress, &xyz_tan, &abc_tan, &uvw_tan); + out[0] = xyz_tan.x; out[1] = xyz_tan.y; out[2] = xyz_tan.z; + out[3] = abc_tan.x; out[4] = abc_tan.y; out[5] = abc_tan.z; + out[6] = uvw_tan.x; out[7] = uvw_tan.y; out[8] = uvw_tan.z; + break; + } + default: + return -1; + } + return 0; +} + +/** + * @brief Compute centripetal acceleration vector per unit v² at segment endpoint + * + * Returns a 3-element XYZ vector such that: a_centripetal = v² * result + * The magnitude is 1/R_effective and direction is toward the center of curvature. + * For lines (zero curvature), returns zero vector. + * ABC/UVW subspaces are linearly interpolated and have no centripetal component. + * + * Uses rTan·cos + rPerp·sin directly (not pmCirclePoint) to avoid + * spiral and helix contamination of the centripetal direction. + * + * @param tc Segment + * @param at_end true for end of segment, false for start + * @param out Output 3-element vector (XYZ centripetal accel per v²) + * @return 0 on success, -1 if segment type not supported + */ +static int tcGetCentripetalAccelPerV2(TC_STRUCT const * const tc, + int at_end, double out[3]) +{ + out[0] = out[1] = out[2] = 0.0; + + switch (tc->motion_type) { + case TC_LINEAR: + // Zero curvature — no centripetal acceleration + return 0; + + case TC_CIRCULAR: { + PmCircle const *circ = &tc->coords.circle.xyz; + double R_eff = pmCircleEffectiveMinRadius(circ); + if (R_eff < 1e-9) return -1; // degenerate arc + + double angle = at_end ? circ->angle : 0.0; + + // Compute in-plane radial vector at the endpoint + // radial = rTan*cos(angle) + rPerp*sin(angle) (points outward) + PmCartesian par, perp, centripetal; + pmCartScalMult(&circ->rTan, cos(angle), &par); + pmCartScalMult(&circ->rPerp, sin(angle), &perp); + pmCartCartAdd(&par, &perp, ¢ripetal); + // Centripetal direction is toward center (negate radial) + pmCartNegEq(¢ripetal); + pmCartUnitEq(¢ripetal); + + double inv_R = 1.0 / R_eff; + out[0] = centripetal.x * inv_R; + out[1] = centripetal.y * inv_R; + out[2] = centripetal.z * inv_R; + return 0; + } + + case TC_SPHERICAL: { + SphericalArc const *arc = &tc->coords.arc.xyz; + double R = arc->radius; + if (R < 1e-9) return -1; + + // Use cached start/end points directly (no need for arcPoint) + PmCartesian const *point = at_end ? &arc->end : &arc->start; + + // Centripetal: toward center + PmCartesian centripetal; + pmCartCartSub(&arc->center, point, ¢ripetal); + pmCartUnitEq(¢ripetal); + + double inv_R = 1.0 / R; + out[0] = centripetal.x * inv_R; + out[1] = centripetal.y * inv_R; + out[2] = centripetal.z * inv_R; + return 0; + } + + case TC_BEZIER: { + // Bezier blend: use precomputed max curvature + double R = tc->coords.bezier.min_radius; + if (R < 1e-9) return -1; + + // Get centripetal direction at endpoint from tangent cross second derivative + // Simplified: use tangent direction rotated 90° (approximate for small blends) + PmCartesian xyz_tan; + double progress = at_end ? tc->coords.bezier.total_length : 0.0; + bezier9Tangent(&tc->coords.bezier, progress, &xyz_tan, NULL, NULL); + + // Centripetal is perpendicular to tangent, magnitude 1/R + // For blend velocity limiting, the magnitude matters more than direction + double inv_R = 1.0 / R; + out[0] = inv_R; // Conservative: use max curvature on primary axis + out[1] = 0.0; + out[2] = 0.0; + return 0; + } + + default: + // TC_DWELL, TC_RIGIDTAP: not supported + return -1; + } +} + +/** + * @brief Get last element in the userspace queue + * + * Returns a pointer to the most recently enqueued TC_STRUCT, + * or NULL if the queue is empty. + */ +static TC_STRUCT * tcqLast_user(TC_QUEUE_STRUCT * const tcq) +{ + if (!tcq) return NULL; + + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + int len = (current_end - current_start + tcq->size) % tcq->size; + if (len == 0) return NULL; + + int idx = (current_end - 1 + tcq->size) % tcq->size; + return &(tcq->queue[idx]); +} + +/** + * @brief Attempt to create a blend between adjacent segments + * + * Called from tpAddLine_9D and tpAddCircle_9D after the current segment + * is fully built but BEFORE it is enqueued. If blending succeeds: + * - prev_tc is trimmed from its end + * - A Bezier9 blend segment is enqueued + * - tc is trimmed from its start + * - prev_tc and blend get TC_TERM_COND_TANGENT with planned blend velocity + * + * If blending fails, returns 0 so caller can fall back to kink velocity. + * + * @param tp Trajectory planner + * @param prev_tc Previous segment (already in queue, modified in place) + * @param tc Current segment (NOT yet enqueued, modified in place) + * @return 1 on blend created, 0 on no blend (fallback to kink), -1 on error + */ +static int tpSetupBlend9D(TP_STRUCT *tp, TC_STRUCT *prev_tc, TC_STRUCT *tc) +{ + if (!tp || !prev_tc || !tc) return 0; + + // Guard: skip RIGIDTAP and DWELL — they cannot participate in blends + if (prev_tc->motion_type == TC_RIGIDTAP || prev_tc->motion_type == TC_DWELL || + tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_DWELL) { + return 0; + } + + // Guard: skip if prev_tc is already a blend segment + if (prev_tc->motion_type == TC_BEZIER) { + return 0; + } + + // Guard: skip if either segment is too short for a meaningful blend + // (e.g. near-zero-length move like "g1 x0 y0" when already at origin) + if (prev_tc->target < 0.1 || tc->target < 0.1) { + return 0; + } + + // G61.1 (CANON_EXACT_STOP → TC_TERM_COND_STOP): + // Strictest mode — always decel to zero, no blending. + if (tp->termCond == TC_TERM_COND_STOP) { + return 0; + } + + // G61 (EXACT): use kink velocity, no blend geometry + if (tp->termCond == TC_TERM_COND_EXACT) { + return 0; + } + + // Only G64 (PARABOLIC) mode creates blend segments + // Compute blend tolerance from G64 P value + double tolerance; + int res = tcFindBlendTolerance9(prev_tc, tc, &tolerance); + if (res != TP_ERR_OK || tolerance <= 0.0) { + return 0; + } + + // Build per-axis velocity and acceleration bounds + // Use minimum of both segments' limits + AxisBounds9 vel_bounds, acc_bounds; + memset(&vel_bounds, 0, sizeof(vel_bounds)); + memset(&acc_bounds, 0, sizeof(acc_bounds)); + + double v_min = fmin(prev_tc->maxvel, tc->maxvel); + double a_min = fmin(prev_tc->maxaccel, tc->maxaccel); + vel_bounds.xyz.x = vel_bounds.xyz.y = vel_bounds.xyz.z = v_min; + acc_bounds.xyz.x = acc_bounds.xyz.y = acc_bounds.xyz.z = a_min; + + // Maximum feed scale for velocity planning (accounts for possible feed override) + double max_feed_scale = 1.0; + if (emcmotConfig) { + max_feed_scale = emcmotConfig->maxFeedScale; + } + + // Run blend optimizer + BlendSolution9 solution; + res = optimizeBlendSize9(prev_tc, tc, tolerance, max_feed_scale, + &vel_bounds, &acc_bounds, &solution); + if (res != TP_ERR_OK || solution.status != BLEND9_OK) { + return 0; // Blend failed — fall back to kink velocity + } + + // Trim previous segment from its end + res = trimSegment9(prev_tc, solution.trim_prev, 0); + if (res != TP_ERR_OK) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetupBlend9D: failed to trim prev_tc\n"); + return -1; + } + + // Trim current segment from its start + res = trimSegment9(tc, solution.trim_tc, 1); + if (res != TP_ERR_OK) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetupBlend9D: failed to trim tc\n"); + return -1; + } + + // Create blend segment + TC_STRUCT blend_tc; + res = createBlendSegment9(prev_tc, tc, &solution, &blend_tc, tp->cycleTime); + if (res != TP_ERR_OK) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetupBlend9D: failed to create blend segment\n"); + return -1; + } + + // Junction velocity = v_plan from the blend optimizer. + // The optimizer already ensures v_plan respects the centripetal acceleration + // limit (v² / R_min ≤ a_normal) via bezier9AccLimit. The quintic Bezier + // provides G2 continuity: B''(0) = B''(1) = 0, so curvature is zero at both + // endpoints. This eliminates the centripetal acceleration discontinuity that + // cubic (G1-only) blends suffered from — no jerk spike at segment boundaries. + double v_plan = solution.params.v_plan; + + // Per-joint curvature-rate jerk limit (hard cap, like kink_vel). + // Centripetal jerk = v³ · dκ/ds must stay within per-joint jerk budgets. + // Uses per-joint limits from kinematics module (not trajectory-level maxjerk). + { + using motion_planning::g_userspace_kins_planner; + int num_joints = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getNumJoints() : 9; + if (num_joints > 9) num_joints = 9; + double dkds = solution.bezier.max_dkappa_ds; + if (dkds > BEZIER9_CURVATURE_EPSILON) { + double v_jerk_cap = 1e9; + for (int j = 0; j < num_joints; j++) { + double jlim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointJerkLimit(j) : 1e9; + if (jlim > 0 && jlim < 1e9) { + double v_j = cbrt(jlim / dkds); + if (v_j < v_jerk_cap) v_jerk_cap = v_j; + } + } + if (v_jerk_cap < v_plan) { + v_plan = v_jerk_cap; + blend_tc.maxvel = v_plan; + } + // Hard cap on blend exit (like kink_vel, ignores feed override) + blend_tc.kink_vel = v_jerk_cap; + // Hard cap on prev_tc exit → blend entry + if (prev_tc->kink_vel <= 0 || v_jerk_cap < prev_tc->kink_vel) + prev_tc->kink_vel = v_jerk_cap; + } + } + + double v_entry = v_plan; + double v_exit = v_plan; + + // Set up velocity continuity: + // prev_tc → blend at v_entry, blend → tc at v_exit + prev_tc->term_cond = TC_TERM_COND_TANGENT; + prev_tc->finalvel = v_entry; + atomicStoreDouble(&prev_tc->shared_9d.final_vel, v_entry); + atomicStoreDouble(&prev_tc->shared_9d.final_vel_limit, v_entry); + + // Mark prev_tc for Ruckig recomputation (was profiled with finalvel=0) + __atomic_store_n((int*)&prev_tc->shared_9d.optimization_state, + TC_PLAN_UNTOUCHED, __ATOMIC_RELEASE); + + // Blend segment: TANGENT exit to tc at v_exit + blend_tc.term_cond = TC_TERM_COND_TANGENT; + blend_tc.finalvel = v_exit; + atomicStoreDouble(&blend_tc.shared_9d.final_vel, v_exit); + atomicStoreDouble(&blend_tc.shared_9d.final_vel_limit, v_exit); + + // Set jerk limit on blend from parent segments + blend_tc.maxjerk = fmin(prev_tc->maxjerk, tc->maxjerk); + + // Enqueue blend segment + TC_QUEUE_STRUCT *queue = &tp->queue; + res = tcqPut_user(queue, &blend_tc); + if (res != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetupBlend9D: failed to enqueue blend segment\n"); + return -1; + } + + rtapi_print_msg(RTAPI_MSG_ERR, + "BLEND_DBG: created blend v_plan=%.3f kink_vel=%.3f len=%.4f trim_prev=%.4f trim_tc=%.4f " + "max_kappa=%.6f max_dkds=%.3f min_radius=%.3f prev_target=%.3f prev_tc_type=%d tc_type=%d\n", + v_plan, blend_tc.kink_vel, blend_tc.target, solution.trim_prev, solution.trim_tc, + solution.bezier.max_kappa, solution.bezier.max_dkappa_ds, solution.bezier.min_radius, + prev_tc->target, prev_tc->motion_type, tc->motion_type); + + // tc will be enqueued by caller as usual (after this function returns) + // Set tc's tolerance for the blend + tc->tolerance = tp->tolerance; + + return 1; // Blend created successfully +} + +/** + * @brief Compute kink velocity at junction between prev_tc and tc + * + * For G61 (Exact Path) mode: compute the maximum safe velocity at the + * junction such that per-joint jerk and acceleration limits are respected. + * + * At the junction, the velocity direction changes instantaneously (over one + * servo period dt). For each joint j, the direction change delta_u projected + * through the Jacobian gives the velocity impulse per unit path speed: + * + * joint_acc_j = v * |J[j] . delta_u| / dt (must be <= acc_limit[j]) + * joint_jerk_j = v * |J[j] . delta_u| / dt^2 (must be <= jerk_limit[j]) + * + * For trivkins (J = identity), this simplifies to per-axis limiting. + * The kink_vel is the minimum over all joints of both constraints. + * + * The result is an absolute (unscaled) velocity. Feed override is applied + * downstream at Ruckig profile computation time. + * + * @param tp Trajectory planner state + * @param queue Segment queue + * @param tc New segment being added (local, not yet enqueued) + * @param queue_len Current number of elements in queue + * @param current_end Current end index of queue (tc will be placed here) + */ +static void tpComputeKinkVelocity_9D(TP_STRUCT *tp, TC_QUEUE_STRUCT *queue, + TC_STRUCT *tc, int queue_len, int current_end) +{ + // Only compute for EXACT mode (G61) + if (tp->termCond != TC_TERM_COND_EXACT) return; + + // Need at least 1 element in queue (the previous segment) + if (queue_len < 1) return; + + // Get previous segment from queue (last enqueued element) + int prev_idx = (current_end - 1 + queue->size) % queue->size; + TC_STRUCT *prev_tc = &queue->queue[prev_idx]; + + // Skip if either segment can't participate in kink computation + if (prev_tc->motion_type == TC_RIGIDTAP || prev_tc->motion_type == TC_DWELL) return; + if (tc->motion_type == TC_RIGIDTAP || tc->motion_type == TC_DWELL) return; + + // Compute 9D tangent rate vectors at the junction + double u_prev[9], u_next[9]; + if (tcGetTangentRate9D(prev_tc, 1, u_prev) != 0) return; + if (tcGetTangentRate9D(tc, 0, u_next) != 0) return; + + // Compute 9D direction-change vector + double delta_u[9]; + double delta_mag_sq = 0.0; + for (int a = 0; a < 9; a++) { + delta_u[a] = u_next[a] - u_prev[a]; + delta_mag_sq += delta_u[a] * delta_u[a]; + } + + double vel_cap = fmin(prev_tc->maxvel, tc->maxvel); + double dt = tp->cycleTime; + // Physical limit starts unconstrained — only jerk/accel physics determine it. + // vel_cap (programmed feed) is applied separately for the backward pass. + double kink_vel = 1e9; + + using motion_planning::g_userspace_kins_planner; + int num_joints = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getNumJoints() : 9; + if (num_joints > 9) num_joints = 9; + + // Compute Jacobian at junction for non-identity kinematics + // Used by both Part A (direction-change) and Part B (centripetal) + double J[9][9] = {{0}}; + bool have_jacobian = false; + if (g_userspace_kins_planner.isEnabled() && + !g_userspace_kins_planner.isIdentity()) { + EmcPose junction; + memset(&junction, 0, sizeof(junction)); + if (tc->motion_type == TC_LINEAR) { + junction.tran = tc->coords.line.xyz.start; + junction.a = tc->coords.line.abc.start.x; + junction.b = tc->coords.line.abc.start.y; + junction.c = tc->coords.line.abc.start.z; + junction.u = tc->coords.line.uvw.start.x; + junction.v = tc->coords.line.uvw.start.y; + junction.w = tc->coords.line.uvw.start.z; + } else if (tc->motion_type == TC_CIRCULAR) { + pmCirclePoint(&tc->coords.circle.xyz, 0.0, &junction.tran); + junction.a = tc->coords.circle.abc.start.x; + junction.b = tc->coords.circle.abc.start.y; + junction.c = tc->coords.circle.abc.start.z; + junction.u = tc->coords.circle.uvw.start.x; + junction.v = tc->coords.circle.uvw.start.y; + junction.w = tc->coords.circle.uvw.start.z; + } + have_jacobian = g_userspace_kins_planner.computeJacobian(junction, J); + } + + // --- Part A: Direction-change limiting --- + // For non-trivkins: joint velocity impulse = v * sum_a(|J[j][a]| * |delta_u[a]|) / dt + // For trivkins (J = identity): simplifies to v * |delta_u[j]| / dt + double dt_sq = dt * dt; + if (delta_mag_sq >= 1e-12) { + for (int j = 0; j < num_joints; j++) { + double projection; + if (have_jacobian) { + // Jacobian projection: sum_a(|J[j][a]| * |delta_u[a]|) + projection = 0.0; + for (int a = 0; a < 9; a++) { + projection += fabs(J[j][a]) * fabs(delta_u[a]); + } + } else { + projection = fabs(delta_u[j]); // trivkins: J = identity + } + if (projection < 1e-15) continue; + + double jerk_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointJerkLimit(j) : 1e9; + if (jerk_lim > 0 && jerk_lim < 1e9) { + double v_jerk = jerk_lim * dt_sq / projection; + if (v_jerk < kink_vel) kink_vel = v_jerk; + } + + double acc_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointAccLimit(j) : 1e9; + if (acc_lim > 0 && acc_lim < 1e9) { + double v_acc = acc_lim * dt / projection; + if (v_acc < kink_vel) kink_vel = v_acc; + } + } + } + + // --- Part B: Centripetal jerk limiting --- + // Even when tangent-continuous (delta_u ~ 0), curvature can change at + // the junction (line->arc, arc->line, arc->arc with different R or center). + // Centripetal acceleration = v^2 * kappa * n_hat, so when kappa changes + // instantaneously: j_centripetal = v^2 * |delta(kappa*n)| / dt + // Constraint is quadratic in v: v <= sqrt(jerk_limit * dt / |delta_ac|) + { + double ac_prev[3], ac_next[3]; + int have_prev = tcGetCentripetalAccelPerV2(prev_tc, 1, ac_prev); + int have_next = tcGetCentripetalAccelPerV2(tc, 0, ac_next); + + if (have_prev == 0 && have_next == 0) { + // Compute per-joint centripetal projections via Jacobian + // ac_prev/ac_next are 3-element XYZ vectors (world space) + // Joint centripetal = sum_a(|J[j][a]| * |ac[a]|) for a=0..2 + for (int j = 0; j < num_joints; j++) { + double delta_proj, max_proj; + if (have_jacobian) { + double dp = 0.0, mp = 0.0; + for (int a = 0; a < 3; a++) { + double jabs = fabs(J[j][a]); + dp += jabs * fabs(ac_next[a] - ac_prev[a]); + mp += jabs * fmax(fabs(ac_prev[a]), fabs(ac_next[a])); + } + delta_proj = dp; + max_proj = mp; + } else { + if (j >= 3) continue; // trivkins: XYZ only + delta_proj = fabs(ac_next[j] - ac_prev[j]); + max_proj = fmax(fabs(ac_prev[j]), fabs(ac_next[j])); + } + + // Curvature-change jerk: v^2 * delta_proj / dt <= jerk_limit + if (delta_proj > 1e-15) { + double jerk_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointJerkLimit(j) : 1e9; + if (jerk_lim > 0 && jerk_lim < 1e9) { + double v_curv_jerk = sqrt(jerk_lim * dt / delta_proj); + if (v_curv_jerk < kink_vel) kink_vel = v_curv_jerk; + } + } + + // Per-joint centripetal acceleration: v^2 * max_proj <= acc_limit + if (max_proj > 1e-15) { + double acc_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointAccLimit(j) : 1e9; + if (acc_lim > 0 && acc_lim < 1e9) { + double v_curv_acc = sqrt(acc_lim / max_proj); + if (v_curv_acc < kink_vel) kink_vel = v_curv_acc; + } + } + } + } + } + + // --- Part C: Virtual arc curvature for line-line junctions --- + // Consecutive small line segments approximate a smooth curve. Compute the + // discrete curvature from three points (start of prev, junction, end of tc) + // and apply per-joint jerk and centripetal acceleration limits. + // This caps kink_vel at the physically safe velocity for the approximated + // curve's curvature, preventing kink_ratio from exceeding geometric limits. + // Ported from planner 0/1 (tp.c:2266-2337) with Jacobian extension. + if (prev_tc->motion_type == TC_LINEAR && tc->motion_type == TC_LINEAR) { + PmCartesian P0 = prev_tc->coords.line.xyz.start; + PmCartesian P1 = prev_tc->coords.line.xyz.end; // junction point + PmCartesian P2 = tc->coords.line.xyz.end; + + PmCartesian A, B; + pmCartCartSub(&P1, &P0, &A); + pmCartCartSub(&P2, &P1, &B); + + double A_mag, B_mag; + pmCartMag(&A, &A_mag); + pmCartMag(&B, &B_mag); + + if (A_mag > 1e-12 && B_mag > 1e-12) { + // kappa = 2*(t_out - t_in) / (|A| + |B|) + // Equals the curvature of the circumscribed circle through P0, P1, P2. + PmCartesian t_in, t_out, delta_t, curv_vec; + pmCartScalMult(&A, 1.0 / A_mag, &t_in); + pmCartScalMult(&B, 1.0 / B_mag, &t_out); + pmCartCartSub(&t_out, &t_in, &delta_t); + pmCartScalMult(&delta_t, 2.0 / (A_mag + B_mag), &curv_vec); + + double curv_xyz[3] = {curv_vec.x, curv_vec.y, curv_vec.z}; + + for (int j = 0; j < num_joints; j++) { + double curv_proj; + if (have_jacobian) { + curv_proj = 0.0; + for (int a = 0; a < 3; a++) + curv_proj += fabs(J[j][a]) * fabs(curv_xyz[a]); + } else { + if (j >= 3) continue; // trivkins: XYZ curvature only + curv_proj = fabs(curv_xyz[j]); + } + if (curv_proj < 1e-15) continue; + + // Curvature-change jerk: v^2 * curv_proj / dt <= jerk_limit + double jerk_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointJerkLimit(j) : 1e9; + if (jerk_lim > 0 && jerk_lim < 1e9) { + double v_curv_jerk = sqrt(jerk_lim * dt / curv_proj); + if (v_curv_jerk < kink_vel) kink_vel = v_curv_jerk; + } + + // Centripetal acceleration: v^2 * curv_proj <= acc_limit + double acc_lim = g_userspace_kins_planner.isEnabled() + ? g_userspace_kins_planner.getJointAccLimit(j) : 1e9; + if (acc_lim > 0 && acc_lim < 1e9) { + double v_curv_acc = sqrt(acc_lim / curv_proj); + if (v_curv_acc < kink_vel) kink_vel = v_curv_acc; + } + } + } + } + + // Floor at a small positive value to avoid numerical issues + if (kink_vel < 1e-6) kink_vel = 1e-6; + + // kink_vel is the physical limit (from jerk/accel constraints). + // Store it in tc->kink_vel as a hard cap NOT scaled by feed override. + // The backward pass velocity is capped to the programmed feed (vel_cap) + // since the backward pass operates in unscaled space (feed_scale=1.0). + tcSetKinkProperties(prev_tc, tc, kink_vel, 0.0); + + double backward_pass_vel = fmin(vel_cap, kink_vel); + + // Promote prev_tc to TANGENT to allow non-zero exit velocity + prev_tc->term_cond = TC_TERM_COND_TANGENT; + prev_tc->finalvel = backward_pass_vel; + atomicStoreDouble(&prev_tc->shared_9d.final_vel, backward_pass_vel); + atomicStoreDouble(&prev_tc->shared_9d.final_vel_limit, backward_pass_vel); + + // Mark prev_tc for Ruckig recomputation (was profiled with finalvel=0) + __atomic_store_n((int*)&prev_tc->shared_9d.optimization_state, + TC_PLAN_UNTOUCHED, __ATOMIC_RELEASE); +} + +/** + * @brief Add linear move to shared memory queue (userspace planning) + * + * - Initialize TC_STRUCT + * - Set geometry + * - Write to queue + * - Trigger optimization + * - NO blending (requires blend geometry implementation) + */ +extern "C" int tpAddLine_9D( + TP_STRUCT * const tp, + EmcPose end_pose, + int type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + struct state_tag_t const &tag) +{ + int queue_len, current_end; + if (validateQueueState_9D(tp, &queue_len, ¤t_end, "tpAddLine_9D") != 0) { + return -1; + } + + // First segment of a new program: reset userspace planning state. + // tpClear() runs in RT context where tpClearPlanning_9D is unavailable, + // so we detect program-start here by an empty queue. + if (queue_len == 0) { + tpClearPlanning_9D(tp); + } + + TC_QUEUE_STRUCT *queue = &tp->queue; + + // Initialize new TC_STRUCT for the line segment + TC_STRUCT tc; + tcInit_9D(&tc, TC_LINEAR, type, tp->cycleTime); + + // CRITICAL: Set enables for feed/spindle scaling (FS_ENABLED, SS_ENABLED, etc.) + // Without this, feed override won't work! + tc.enables = enables; + + // Set motion parameters + tcSetupMotion_9D(&tc, vel, ini_maxvel, acc); + + // Set state tag for tracking + tc.tag = tag; + tc.id = tag.fields[0]; // Simple ID assignment + + // Compute line geometry (goalPos is EmcPose in LinuxCNC) + int result = computeLineLengthAndTarget_9D(&tc, &tp->goalPos, &end_pose); + if (result != 0) { + return -1; + } + + // Zero-length move (e.g. G0Z0 when already at Z0): treat as no-op success. + // Update goalPos to match command endpoint (handles FP rounding) and return + // success so the task controller advances without falling through to the + // RT path (which would call tpSetId/tpAddLine on the shared queue). + if (tc.target < 1e-6) { + tp->goalPos = end_pose; + return 0; + } + + // Set termination condition from G-code mode (G61/G61.1/G64): + // G61.1 (STOP): decelerate to zero at every junction + // G61 (EXACT): exact path with kink velocity + // G64 (PARABOLIC): continuous blending via Bezier blend segments + tc.term_cond = tp->termCond; + if (tc.term_cond == TC_TERM_COND_PARABOLIC) { + // G64: will be promoted to TANGENT if blend succeeds, else STOP + tc.term_cond = TC_TERM_COND_STOP; + } + tc.tolerance = tp->tolerance; + tc.finalvel = 0.0; // Default: decelerate to zero + + // Attempt blend with previous segment (G64 mode) or kink velocity (G61 mode). + // tpSetupBlend9D handles G64: creates Bezier blend, trims both segments. + // tpComputeKinkVelocity_9D handles G61: computes junction velocity limit. + { + TC_STRUCT *prev_tc = tcqLast_user(queue); + if (prev_tc) { + int blend_result = tpSetupBlend9D(tp, prev_tc, &tc); + if (blend_result <= 0) { + // No blend created — compute kink velocity as fallback + tpComputeKinkVelocity_9D(tp, queue, &tc, queue_len, current_end); + } + } + } + + // Compute joint-space segment if userspace kinematics enabled + // This populates tc.joint_space with start/end joint positions and + // velocity/acceleration limits derived from Jacobian analysis. + if (motion_planning::userspace_kins_is_enabled()) { + if (motion_planning::userspace_kins_compute_joint_segment(&tp->goalPos, &end_pose, &tc) != 0) { + tc.joint_space.valid = 0; + } + } else { + tc.joint_space.valid = 0; + } + + // Write segment to shared memory queue + result = tcqPut_user(queue, &tc); + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpAddLine_9D: FAIL - tcqPut_user failed (queue full?)\n"); + return -1; // Queue full or error + } + + // Update goal position for next segment (use LinuxCNC's tcGetEndpoint) + tcGetEndpoint(&tc, &tp->goalPos); + + // Trigger optimization for all queued segments + // Even single segments need profile computation + triggerAdaptiveOptimization_9D(tp); + + return 0; +} + +/** + * @brief Add circular arc move to shared memory queue (userspace planning) + * + * - Initialize TC_STRUCT for circular motion + * - Set arc geometry using pmCircle9Init + * - Write to queue + * - Trigger optimization + * - NO blending (requires blend geometry implementation) + */ +extern "C" int tpAddCircle_9D( + TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, + int type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + struct state_tag_t const &tag) +{ + int queue_len, current_end; + if (validateQueueState_9D(tp, &queue_len, ¤t_end, "tpAddCircle_9D") != 0) { + return -1; + } + + // First segment of a new program: reset userspace planning state. + if (queue_len == 0) { + tpClearPlanning_9D(tp); + } + + TC_QUEUE_STRUCT *queue = &tp->queue; + + // Initialize new TC_STRUCT for the circular segment + TC_STRUCT tc; + tcInit_9D(&tc, TC_CIRCULAR, type, tp->cycleTime); + + // CRITICAL: Set enables for feed/spindle scaling + tc.enables = enables; + + // Set motion parameters + tcSetupMotion_9D(&tc, vel, ini_maxvel, acc); + + // Set state tag for tracking + tc.tag = tag; + tc.id = tag.fields[0]; + + // Initialize circle geometry using pmCircle9Init + int res_init = pmCircle9Init(&tc.coords.circle, + &tp->goalPos, + &end, + ¢er, + &normal, + turn); + + if (res_init != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpAddCircle_9D: FAIL - pmCircle9Init failed (%d)\n", res_init); + return -1; + } + + // Compute target as arc length + tc.target = tc.nominal_length = pmCircle9Target(&tc.coords.circle); + + // Zero-length arc: treat as no-op success (same logic as zero-length line). + if (tc.target < 1e-6) { + tp->goalPos = end; + return 0; + } + + // Centripetal jerk velocity limit: v³/R² ≤ jerk_limit → v ≤ cbrt(jerk * R²) + // On a circular arc, the centripetal acceleration vector rotates at rate v/R, + // producing world-space jerk of magnitude v³/R². Cap velocity so this + // doesn't exceed the configured jerk limit. + { + double radius = pmCircleEffectiveMinRadius(&tc.coords.circle.xyz); + if (radius > TP_POS_EPSILON) { + double jerk_limit = getDefaultMaxJerk(); + double v_max_cj = cbrt(jerk_limit * radius * radius); + if (v_max_cj < tc.maxvel) { + tc.maxvel = v_max_cj; + } + if (tc.reqvel > tc.maxvel) { + tc.reqvel = tc.maxvel; + } + + // Compute acc_ratio_tan: fraction of accel budget available for tangential use. + // Centripetal accel = v²/R; tangential budget = sqrt(a_max² - a_centripetal²) + double a_max = tc.maxaccel; + double a_n = tc.maxvel * tc.maxvel / radius; + if (a_n < a_max) { + tc.acc_ratio_tan = sqrt(1.0 - (a_n * a_n) / (a_max * a_max)); + } else { + // Centripetal alone saturates accel budget — cap velocity further + tc.maxvel = sqrt(a_max * radius); + if (tc.reqvel > tc.maxvel) tc.reqvel = tc.maxvel; + tc.acc_ratio_tan = 0.0; + } + + } + } + + // Set termination condition from G-code mode (same as tpAddLine_9D) + tc.term_cond = tp->termCond; + if (tc.term_cond == TC_TERM_COND_PARABOLIC) { + tc.term_cond = TC_TERM_COND_STOP; + } + tc.tolerance = tp->tolerance; + tc.finalvel = 0.0; // Default: decelerate to zero + + // Attempt blend with previous segment (G64) or kink velocity (G61) + { + TC_STRUCT *prev_tc = tcqLast_user(queue); + if (prev_tc) { + int blend_result = tpSetupBlend9D(tp, prev_tc, &tc); + if (blend_result <= 0) { + tpComputeKinkVelocity_9D(tp, queue, &tc, queue_len, current_end); + } + } + } + + // Compute joint-space segment if userspace kinematics enabled + if (motion_planning::userspace_kins_is_enabled()) { + if (motion_planning::userspace_kins_compute_joint_segment(&tp->goalPos, &end, &tc) != 0) { + tc.joint_space.valid = 0; + } + } else { + tc.joint_space.valid = 0; + } + + // Write segment to shared memory queue + int result = tcqPut_user(queue, &tc); + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpAddCircle_9D: FAIL - tcqPut_user failed (queue full?)\n"); + return -1; + } + + // Update goal position for next segment + tcGetEndpoint(&tc, &tp->goalPos); + + // Trigger optimization for all queued segments + triggerAdaptiveOptimization_9D(tp); + + return 0; +} diff --git a/src/emc/motion_planning/path_sampler.cc b/src/emc/motion_planning/path_sampler.cc new file mode 100644 index 00000000000..8a36921adc8 --- /dev/null +++ b/src/emc/motion_planning/path_sampler.cc @@ -0,0 +1,279 @@ +/******************************************************************** + * Description: path_sampler.cc + * Path sampling implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "path_sampler.hh" +#include +#include +#include + +extern "C" { +#include "blendmath.h" // For pmCircleAngleFromProgress, pmCirclePoint +#include "tc.h" // For pmCircle9Target, pmCartLinePoint +#include "bezier9.h" // For bezier9Point, bezier9Length +} + +namespace motion_planning { + +PathSampler::PathSampler() + : kins_ctx_(nullptr), + initialized_(false) { +} + +PathSampler::~PathSampler() { + // Note: kins_ctx_ is owned externally, don't free it +} + +bool PathSampler::init(KinematicsUserContext* kins_ctx, + const PathSamplerConfig& config) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + config_ = config; + + // For trivkins, don't need to compute Jacobian + if (kinematicsUserIsIdentity(kins_ctx_)) { + config_.compute_jacobian = false; + } + + initialized_ = true; + return true; +} + +void PathSampler::interpolatePose(const EmcPose& start, + const EmcPose& end, + double t, + EmcPose& out) { + // Linear interpolation for all 9 axes + out.tran.x = start.tran.x + t * (end.tran.x - start.tran.x); + out.tran.y = start.tran.y + t * (end.tran.y - start.tran.y); + out.tran.z = start.tran.z + t * (end.tran.z - start.tran.z); + out.a = start.a + t * (end.a - start.a); + out.b = start.b + t * (end.b - start.b); + out.c = start.c + t * (end.c - start.c); + out.u = start.u + t * (end.u - start.u); + out.v = start.v + t * (end.v - start.v); + out.w = start.w + t * (end.w - start.w); +} + +double PathSampler::poseDistance(const EmcPose& p1, const EmcPose& p2) { + // Euclidean distance in 9D space + // Weight rotary axes differently (convert degrees to equivalent linear) + const double rotary_scale = 1.0; // mm per degree (adjustable) + + double dx = p2.tran.x - p1.tran.x; + double dy = p2.tran.y - p1.tran.y; + double dz = p2.tran.z - p1.tran.z; + double da = (p2.a - p1.a) * rotary_scale; + double db = (p2.b - p1.b) * rotary_scale; + double dc = (p2.c - p1.c) * rotary_scale; + double du = p2.u - p1.u; + double dv = p2.v - p1.v; + double dw = p2.w - p1.w; + + return std::sqrt(dx*dx + dy*dy + dz*dz + + da*da + db*db + dc*dc + + du*du + dv*dv + dw*dw); +} + +bool PathSampler::computeJoints(const EmcPose& world_pos, PathSample& sample) { + if (!kins_ctx_) { + return false; + } + + // Copy world position + sample.world_pos = world_pos; + + // Compute inverse kinematics + if (kinematicsUserInverse(kins_ctx_, &world_pos, sample.joints) != 0) { + sample.valid = false; + return false; + } + + // Initialize limits to high values (will be set by joint limit calculator) + sample.max_vel = 1e9; + sample.max_acc = 1e9; + + // Zero Jacobian for now (computed by JacobianCalculator if needed) + std::memset(sample.jacobian, 0, sizeof(sample.jacobian)); + sample.condition_number = 1.0; // Identity for trivkins + + sample.valid = true; + return true; +} + +int PathSampler::sampleLine(const EmcPose& start, + const EmcPose& end, + std::vector& samples) { + if (!initialized_) { + return -1; + } + + samples.clear(); + + // Compute path length + double path_length = poseDistance(start, end); + + // Determine number of samples + int num_samples; + if (path_length < 1e-9) { + // Zero-length move: just sample endpoints + num_samples = 2; + } else { + // Calculate based on sample_distance + num_samples = static_cast(std::ceil(path_length / config_.sample_distance)) + 1; + } + + // Clamp to min/max + num_samples = std::max(num_samples, config_.min_samples); + num_samples = std::min(num_samples, config_.max_samples); + + // Reserve space + samples.reserve(num_samples); + + // Sample the path + for (int i = 0; i < num_samples; i++) { + double t = (num_samples > 1) ? + static_cast(i) / (num_samples - 1) : + 0.0; + + PathSample sample; + EmcPose pose; + interpolatePose(start, end, t, pose); + + if (!computeJoints(pose, sample)) { + // Kinematics failed at this point + sample.valid = false; + } + + sample.distance_from_start = t * path_length; + samples.push_back(sample); + } + + return static_cast(samples.size()); +} + +int PathSampler::sampleCircle(const EmcPose& start, + const EmcPose& end, + const PmCircle9& circle, + std::vector& samples) { + (void)start; // Arc geometry is fully contained in circle parameter + (void)end; + + if (!initialized_) { + return -1; + } + + samples.clear(); + + // Get arc length from circle geometry + double arc_length = pmCircle9Target(&circle); + + // Determine number of samples based on arc length + int num_samples; + if (arc_length < 1e-9) { + num_samples = 2; + } else { + num_samples = static_cast(std::ceil(arc_length / config_.sample_distance)) + 1; + } + + // Clamp to min/max + num_samples = std::max(num_samples, config_.min_samples); + num_samples = std::min(num_samples, config_.max_samples); + + samples.reserve(num_samples); + + // Sample along the arc + for (int i = 0; i < num_samples; i++) { + double progress = (num_samples > 1) ? + static_cast(i) / (num_samples - 1) * arc_length : + 0.0; + + // Get angle from progress (handles spiral arcs) + double angle = 0.0; + pmCircleAngleFromProgress(&circle.xyz, &circle.fit, progress, &angle); + + // Get XYZ position along arc (does sin/cos) + PmCartesian xyz; + pmCirclePoint(&circle.xyz, angle, &xyz); + + // Interpolate ABC and UVW linearly + double t = (arc_length > 1e-9) ? progress / arc_length : 0.0; + PmCartesian abc, uvw; + pmCartLinePoint(&circle.abc, t * circle.abc.tmag, &abc); + pmCartLinePoint(&circle.uvw, t * circle.uvw.tmag, &uvw); + + // Build EmcPose from components + EmcPose pose; + pose.tran = xyz; + pose.a = abc.x; + pose.b = abc.y; + pose.c = abc.z; + pose.u = uvw.x; + pose.v = uvw.y; + pose.w = uvw.z; + + PathSample sample; + if (!computeJoints(pose, sample)) { + sample.valid = false; + } + + sample.distance_from_start = progress; + samples.push_back(sample); + } + + return static_cast(samples.size()); +} + +int PathSampler::sampleBezier(const Bezier9& bezier, + std::vector& samples) { + if (!initialized_) { + return -1; + } + + samples.clear(); + + double length = bezier9Length(&bezier); + + int num_samples; + if (length < 1e-9) { + num_samples = 2; + } else { + num_samples = static_cast(std::ceil(length / config_.sample_distance)) + 1; + } + + num_samples = std::max(num_samples, config_.min_samples); + num_samples = std::min(num_samples, config_.max_samples); + + samples.reserve(num_samples); + + for (int i = 0; i < num_samples; i++) { + double progress = (num_samples > 1) ? + static_cast(i) / (num_samples - 1) * length : + 0.0; + + EmcPose pose; + bezier9Point(&bezier, progress, &pose); + + PathSample sample; + if (!computeJoints(pose, sample)) { + sample.valid = false; + } + + sample.distance_from_start = progress; + samples.push_back(sample); + } + + return static_cast(samples.size()); +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/path_sampler.hh b/src/emc/motion_planning/path_sampler.hh new file mode 100644 index 00000000000..fc1a558a7ef --- /dev/null +++ b/src/emc/motion_planning/path_sampler.hh @@ -0,0 +1,161 @@ +/******************************************************************** + * Description: path_sampler.hh + * Path sampling for userspace kinematics trajectory planning + * + * Samples paths in world coordinates for kinematic analysis. + * Each sample point is used to compute Jacobian and joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef PATH_SAMPLER_HH +#define PATH_SAMPLER_HH + +#include + +// emcpos.h includes posemath.h which has C++ function overloads +#include "emcpos.h" + +extern "C" { +#include "kinematics_userspace/kinematics_user.h" +#include "tc_types.h" // For PmCircle9 +} + +namespace motion_planning { + +/** + * A single sample point along a path + */ +struct PathSample { + EmcPose world_pos; // World coordinates at this sample + double joints[KINEMATICS_USER_MAX_JOINTS]; // Joint positions + double max_vel; // Max velocity at this point (from joint limits) + double max_acc; // Max acceleration at this point + double distance_from_start; // Distance along path from segment start + double jacobian[9][9]; // Jacobian at this point (for non-trivkins) + double condition_number; // Jacobian condition number (singularity detection) + bool valid; // True if sample is valid (kin solved, limits ok) +}; + +/** + * Configuration for path sampling + */ +struct PathSamplerConfig { + int min_samples; // Minimum samples per segment (default: 10) + int max_samples; // Maximum samples per segment (default: 1000) + double sample_distance; // Target distance between samples (mm, default: 0.1) + bool compute_jacobian; // Whether to compute Jacobian (false for trivkins) + + PathSamplerConfig() : + min_samples(10), + max_samples(1000), + sample_distance(0.1), + compute_jacobian(false) {} +}; + +/** + * Path sampler class + * + * Samples paths in world coordinates and computes joint positions. + * Currently supports linear moves with trivkins. + */ +class PathSampler { +public: + PathSampler(); + ~PathSampler(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @param config Sampler configuration + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx, + const PathSamplerConfig& config = PathSamplerConfig()); + + /** + * Sample a linear path (G1 move) + * + * @param start Start position in world coordinates + * @param end End position in world coordinates + * @param samples Output vector of samples (cleared first) + * @return Number of samples generated, or -1 on error + */ + int sampleLine(const EmcPose& start, + const EmcPose& end, + std::vector& samples); + + /** + * Sample a circular arc (G2/G3) + * + * @param start Start position in world coordinates + * @param end End position in world coordinates + * @param circle PmCircle9 arc geometry (initialized by pmCircle9Init) + * @param samples Output vector of samples (cleared first) + * @return Number of samples generated, or -1 on error + */ + int sampleCircle(const EmcPose& start, + const EmcPose& end, + const PmCircle9& circle, + std::vector& samples); + + /** + * Sample a Bezier blend curve + * + * @param bezier Bezier9 blend curve geometry + * @param samples Output vector of samples (cleared first) + * @return Number of samples generated, or -1 on error + */ + int sampleBezier(const Bezier9& bezier, + std::vector& samples); + + /** + * Get the current configuration + */ + const PathSamplerConfig& getConfig() const { return config_; } + + /** + * Set configuration + */ + void setConfig(const PathSamplerConfig& config) { config_ = config; } + +private: + /** + * Compute joint positions for a world pose + * + * @param world_pos World coordinates + * @param sample Sample to fill in + * @return true on success + */ + bool computeJoints(const EmcPose& world_pos, PathSample& sample); + + /** + * Linear interpolation between two poses + * + * @param start Start pose + * @param end End pose + * @param t Interpolation parameter [0, 1] + * @param out Output pose + */ + static void interpolatePose(const EmcPose& start, + const EmcPose& end, + double t, + EmcPose& out); + + /** + * Compute distance between two poses + */ + static double poseDistance(const EmcPose& p1, const EmcPose& p2); + + KinematicsUserContext* kins_ctx_; + PathSamplerConfig config_; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // PATH_SAMPLER_HH diff --git a/src/emc/motion_planning/smoothing_data.hh b/src/emc/motion_planning/smoothing_data.hh new file mode 100644 index 00000000000..a51b6b17720 --- /dev/null +++ b/src/emc/motion_planning/smoothing_data.hh @@ -0,0 +1,26 @@ +/******************************************************************** +* Description: smoothing_data.hh +* Helper types for lookahead buffer and velocity smoothing +* +* Author: Tormach (original), Port by LinuxCNC community +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +********************************************************************/ +#ifndef SMOOTHING_DATA_HH +#define SMOOTHING_DATA_HH + +#include + +// Maximum lookahead depth (safety limit for pre-allocation) +#define MAX_LOOKAHEAD_DEPTH 200 + +// Type alias for consistency with Tormach implementation +using SmoothingVector = std::vector; + +// SmoothingData is defined in motion_planning_9d.hh +// This header just provides the type alias for use in implementation files + +#endif // SMOOTHING_DATA_HH diff --git a/src/emc/motion_planning/userspace_kinematics.cc b/src/emc/motion_planning/userspace_kinematics.cc new file mode 100644 index 00000000000..47143d46417 --- /dev/null +++ b/src/emc/motion_planning/userspace_kinematics.cc @@ -0,0 +1,466 @@ +/******************************************************************** + * Description: userspace_kinematics.cc + * Userspace kinematics integration implementation + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "userspace_kinematics.hh" +#include +#include +#include +#include + +extern "C" { +#include "blendmath.h" // For pmCircleEffectiveMinRadius +} + +// Include inihal for access to current joint limits (reflects HAL pin changes) +// The old_inihal_data structure is updated by inihal.cc when HAL pins change +// (ini.N.max_velocity, ini.N.max_limit, etc.) +#include "inihal.hh" +extern value_inihal_data old_inihal_data; + +namespace motion_planning { + +// Global userspace kinematics planner instance +UserspaceKinematicsPlanner g_userspace_kins_planner; + +UserspaceKinematicsPlanner::UserspaceKinematicsPlanner() + : initialized_(false), + enabled_(false), + kins_ctx_(nullptr) { +} + +UserspaceKinematicsPlanner::~UserspaceKinematicsPlanner() { + if (kins_ctx_) { + kinematicsUserFree(kins_ctx_); + kins_ctx_ = nullptr; + } +} + +bool UserspaceKinematicsPlanner::init(const UserspaceKinematicsConfig& config) { + config_ = config; + + if (!config.enabled) { + enabled_ = false; + initialized_ = true; + return true; + } + + // Initialize userspace kinematics + kins_ctx_ = kinematicsUserInit(config.kinematics_type.c_str(), + config.num_joints, + config.coordinates.c_str()); + if (!kins_ctx_) { + // Kinematics type not supported + enabled_ = false; + initialized_ = false; + return false; + } + + // Initialize path sampler + PathSamplerConfig sampler_config; + sampler_config.min_samples = 10; + sampler_config.max_samples = 100; + sampler_config.sample_distance = 0.1; + sampler_config.compute_jacobian = !kinematicsUserIsIdentity(kins_ctx_); + + if (!path_sampler_.init(kins_ctx_, sampler_config)) { + enabled_ = false; + initialized_ = false; + return false; + } + + // Initialize Jacobian calculator + if (!jacobian_calc_.init(kins_ctx_)) { + enabled_ = false; + initialized_ = false; + return false; + } + + // Initialize joint limit calculator + if (!limit_calc_.init(config.num_joints)) { + enabled_ = false; + initialized_ = false; + return false; + } + + enabled_ = true; + initialized_ = true; + return true; +} + +bool UserspaceKinematicsPlanner::setJointLimits(int joint, + double vel_limit, + double acc_limit, + double min_pos, + double max_pos) { + if (!initialized_) { + return false; + } + + JointLimitConfig limits; + limits.vel_limit = vel_limit; + limits.acc_limit = acc_limit; + limits.min_pos_limit = min_pos; + limits.max_pos_limit = max_pos; + limits.jerk_limit = 1e9; // Jerk limits deferred + + return limit_calc_.setJointLimits(joint, limits); +} + +bool UserspaceKinematicsPlanner::isIdentity() const { + if (!kins_ctx_) { + return true; + } + return kinematicsUserIsIdentity(kins_ctx_) != 0; +} + +bool UserspaceKinematicsPlanner::computeJointSpaceSegment(const EmcPose& start, + const EmcPose& end, + TC_STRUCT* tc) { + if (!enabled_ || !tc) { + return false; + } + + // Refresh joint limits from inihal data before computing + // This ensures we use current limits, including any runtime changes + // via HAL pins (ini.N.max_limit, ini.N.max_velocity, etc.) + // The old_inihal_data structure is updated by inihal.cc when HAL pins change. + { + double vel_limits[KINEMATICS_USER_MAX_JOINTS]; + double acc_limits[KINEMATICS_USER_MAX_JOINTS]; + double min_pos[KINEMATICS_USER_MAX_JOINTS]; + double max_pos[KINEMATICS_USER_MAX_JOINTS]; + double jerk_limits[KINEMATICS_USER_MAX_JOINTS]; + + for (int j = 0; j < config_.num_joints && j < KINEMATICS_USER_MAX_JOINTS; j++) { + vel_limits[j] = old_inihal_data.joint_max_velocity[j]; + acc_limits[j] = old_inihal_data.joint_max_acceleration[j]; + min_pos[j] = old_inihal_data.joint_min_limit[j]; + max_pos[j] = old_inihal_data.joint_max_limit[j]; + jerk_limits[j] = old_inihal_data.joint_jerk[j]; + } + + limit_calc_.updateAllLimits(vel_limits, acc_limits, min_pos, max_pos, jerk_limits); + } + + // Sample the path (line or circle based on motion type) + std::vector samples; + int num_samples; + if (tc->motion_type == TC_CIRCULAR) { + num_samples = path_sampler_.sampleCircle(start, end, tc->coords.circle, samples); + } else if (tc->motion_type == TC_BEZIER) { + num_samples = path_sampler_.sampleBezier(tc->coords.bezier, samples); + } else { + num_samples = path_sampler_.sampleLine(start, end, samples); + } + if (num_samples < 2) { + return false; + } + + // Compute joint positions at start and end + double joints_start[KINEMATICS_USER_MAX_JOINTS]; + double joints_end[KINEMATICS_USER_MAX_JOINTS]; + + if (kinematicsUserInverse(kins_ctx_, &start, joints_start) != 0) { + return false; + } + if (kinematicsUserInverse(kins_ctx_, &end, joints_end) != 0) { + return false; + } + + // Compute velocity and acceleration limits + // First compute trivkins-style limits (direct joint limits from moving joints) + // Then for non-trivkins, also compute Jacobian-based limits + // Use the more restrictive of the two, but with trivkins limits as a floor + + // Step 1: Compute trivkins-style limits (works for all kinematics) + // Only apply limits from moving joints to prevent stationary + // rotary axes from limiting linear moves + double trivkins_vel_limit = 1e9; + double trivkins_acc_limit = 1e9; + double trivkins_jerk_limit = 1e9; + + bool joint_moving[KINEMATICS_USER_MAX_JOINTS] = {false}; + for (int j = 0; j < config_.num_joints; j++) { + double delta = fabs(joints_end[j] - joints_start[j]); + joint_moving[j] = (delta > 1e-9); + } + + for (int j = 0; j < config_.num_joints; j++) { + if (joint_moving[j]) { + if (limit_calc_.getJointVelLimit(j) < trivkins_vel_limit) { + trivkins_vel_limit = limit_calc_.getJointVelLimit(j); + } + if (limit_calc_.getJointAccLimit(j) < trivkins_acc_limit) { + trivkins_acc_limit = limit_calc_.getJointAccLimit(j); + } + if (limit_calc_.getJointJerkLimit(j) < trivkins_jerk_limit) { + trivkins_jerk_limit = limit_calc_.getJointJerkLimit(j); + } + } + } + + // Fallback if no joints moving + if (trivkins_vel_limit > 1e8) trivkins_vel_limit = tc->maxvel; + if (trivkins_acc_limit > 1e8) trivkins_acc_limit = tc->maxaccel; + if (trivkins_jerk_limit > 1e8) trivkins_jerk_limit = tc->maxjerk; + + // Step 2: For non-trivkins, compute Jacobian-based limits + double jacobian_vel_limit = 1e9; + double jacobian_acc_limit = 1e9; + double jacobian_jerk_limit = 1e9; + double max_condition_number = 1.0; + + if (!isIdentity()) { + // Precompute chord tangent: exact for lines, fallback for circles + // tangent[a] = d(world_axis[a]) / d(path_param) + // For rotary-dominated moves, rotary components can be >> 1.0 + double chord_tangent[9] = {0}; + if (tc->target > 1e-12) { + chord_tangent[AXIS_X] = (end.tran.x - start.tran.x) / tc->target; + chord_tangent[AXIS_Y] = (end.tran.y - start.tran.y) / tc->target; + chord_tangent[AXIS_Z] = (end.tran.z - start.tran.z) / tc->target; + chord_tangent[AXIS_A] = (end.a - start.a) / tc->target; + chord_tangent[AXIS_B] = (end.b - start.b) / tc->target; + chord_tangent[AXIS_C] = (end.c - start.c) / tc->target; + chord_tangent[AXIS_U] = (end.u - start.u) / tc->target; + chord_tangent[AXIS_V] = (end.v - start.v) / tc->target; + chord_tangent[AXIS_W] = (end.w - start.w) / tc->target; + } + + bool is_circular = (tc->motion_type == TC_CIRCULAR); + + // Non-trivkins path: use Jacobian to compute world-space limits + // Sample the path and find the most restrictive limits + for (size_t i = 0; i < samples.size(); i++) { + const auto& sample = samples[i]; + + // Compute Jacobian at this sample point + double J[9][9]; + if (!jacobian_calc_.compute(sample.world_pos, J)) { + continue; + } + + // Compute path tangent at this sample point + // Lines: constant chord tangent (exact) + // Circles: finite differences on sample positions (XYZ tangent rotates) + double tangent[9]; + if (is_circular && samples.size() >= 3) { + size_t i_prev = (i > 0) ? i - 1 : 0; + size_t i_next = (i < samples.size() - 1) ? i + 1 : samples.size() - 1; + double ds = samples[i_next].distance_from_start + - samples[i_prev].distance_from_start; + if (ds > 1e-15) { + const EmcPose& pp = samples[i_prev].world_pos; + const EmcPose& pn = samples[i_next].world_pos; + tangent[AXIS_X] = (pn.tran.x - pp.tran.x) / ds; + tangent[AXIS_Y] = (pn.tran.y - pp.tran.y) / ds; + tangent[AXIS_Z] = (pn.tran.z - pp.tran.z) / ds; + tangent[AXIS_A] = (pn.a - pp.a) / ds; + tangent[AXIS_B] = (pn.b - pp.b) / ds; + tangent[AXIS_C] = (pn.c - pp.c) / ds; + tangent[AXIS_U] = (pn.u - pp.u) / ds; + tangent[AXIS_V] = (pn.v - pp.v) / ds; + tangent[AXIS_W] = (pn.w - pp.w) / ds; + } else { + std::memcpy(tangent, chord_tangent, sizeof(tangent)); + } + } else { + std::memcpy(tangent, chord_tangent, sizeof(tangent)); + } + + // Compute world-space limits using path tangent at this sample + JointLimitResult result; + if (!limit_calc_.computeForTangent(J, sample.joints, tangent, + result, + config_.singularity_threshold)) { + continue; + } + + // Track minimum limits across all samples + if (result.max_world_vel < jacobian_vel_limit) { + jacobian_vel_limit = result.max_world_vel; + } + if (result.max_world_acc < jacobian_acc_limit) { + jacobian_acc_limit = result.max_world_acc; + } + if (result.max_world_jerk < jacobian_jerk_limit) { + jacobian_jerk_limit = result.max_world_jerk; + } + + // Track worst-case condition number (for diagnostics) + if (result.condition_number > max_condition_number) { + max_condition_number = result.condition_number; + } + } + } + + // Step 3: Combine limits + // For trivkins: use trivkins limits directly + // For non-trivkins: use Jacobian limits but floor at trivkins limits + // This ensures we never go slower than the old working code + double min_vel_limit; + double min_acc_limit; + double min_jerk_limit; + + if (isIdentity()) { + min_vel_limit = trivkins_vel_limit; + min_acc_limit = trivkins_acc_limit; + min_jerk_limit = trivkins_jerk_limit; + } else { + // Use the more restrictive of trivkins and Jacobian limits + // Jacobian accounts for kinematic coupling (e.g. 5axiskins pivot) + // that amplifies Cartesian motion into joint-space motion + min_vel_limit = std::min(trivkins_vel_limit, jacobian_vel_limit); + min_acc_limit = std::min(trivkins_acc_limit, jacobian_acc_limit); + min_jerk_limit = std::min(trivkins_jerk_limit, jacobian_jerk_limit); + } + + // Step 4: Apply centripetal velocity limit for circular motion + // v_max = sqrt(a * r) to keep centripetal acceleration within limits + if (tc->motion_type == TC_CIRCULAR) { + double radius = pmCircleEffectiveMinRadius(&tc->coords.circle.xyz); + if (radius > 1e-9) { + double v_centripetal = std::sqrt(min_acc_limit * radius); + if (v_centripetal < min_vel_limit) { + min_vel_limit = v_centripetal; + } + } + } + + // Populate JointSpaceSegment + JointSpaceSegment* js = &tc->joint_space; + js->num_joints = config_.num_joints; + + for (int j = 0; j < config_.num_joints; j++) { + js->start[j] = joints_start[j]; + js->end[j] = joints_end[j]; + } + for (int j = config_.num_joints; j < JOINT_SPACE_MAX_JOINTS; j++) { + js->start[j] = 0.0; + js->end[j] = 0.0; + } + + js->vel_limit_start = min_vel_limit; + js->vel_limit_end = min_vel_limit; + js->acc_limit = min_acc_limit; + js->valid = 1; + + // Update TC limits based on joint limits + // The backward pass will use these as additional constraints + if (min_vel_limit < tc->maxvel) { + tc->maxvel = min_vel_limit; + } + if (min_acc_limit < tc->maxaccel) { + tc->maxaccel = min_acc_limit; + } + if (min_jerk_limit < tc->maxjerk || tc->maxjerk <= 0) { + tc->maxjerk = min_jerk_limit; + } + + // Ensure reqvel doesn't exceed maxvel + if (tc->reqvel > tc->maxvel) { + tc->reqvel = tc->maxvel; + } + + return true; +} + +bool UserspaceKinematicsPlanner::evaluateJointPositions(const TC_STRUCT* tc, + double progress, + double* joints) { + if (!tc || !joints) { + return false; + } + + const JointSpaceSegment* js = &tc->joint_space; + if (!js->valid) { + return false; + } + + // Clamp progress to [0, 1] + if (progress < 0.0) progress = 0.0; + if (progress > 1.0) progress = 1.0; + + // Linear interpolation + for (int j = 0; j < js->num_joints; j++) { + joints[j] = js->start[j] + progress * (js->end[j] - js->start[j]); + } + + // Zero unused joints + for (int j = js->num_joints; j < KINEMATICS_USER_MAX_JOINTS; j++) { + joints[j] = 0.0; + } + + return true; +} + +bool UserspaceKinematicsPlanner::computeJacobian(const EmcPose& pose, + double J[9][9]) const { + if (!enabled_ || !initialized_) return false; + // jacobian_calc_ is mutable-safe (no state change in compute) + return const_cast(jacobian_calc_).compute(pose, J); +} + +// C interface implementations + +extern "C" int userspace_kins_init(const char* kins_type, + int num_joints, + const char* coordinates) { + UserspaceKinematicsConfig config; + config.enabled = true; + config.kinematics_type = kins_type ? kins_type : "trivkins"; + config.num_joints = num_joints; + config.coordinates = coordinates ? coordinates : ""; + config.singularity_threshold = 100.0; + + if (!g_userspace_kins_planner.init(config)) { + return -1; + } + return 0; +} + +extern "C" int userspace_kins_set_joint_limits(int joint, + double vel_limit, + double acc_limit, + double min_pos, + double max_pos) { + if (!g_userspace_kins_planner.setJointLimits(joint, vel_limit, acc_limit, min_pos, max_pos)) { + return -1; + } + return 0; +} + +extern "C" int userspace_kins_is_enabled(void) { + return g_userspace_kins_planner.isEnabled() ? 1 : 0; +} + +extern "C" int userspace_kins_compute_joint_segment(const EmcPose* start, + const EmcPose* end, + TC_STRUCT* tc) { + if (!start || !end || !tc) { + return -1; + } + if (!g_userspace_kins_planner.computeJointSpaceSegment(*start, *end, tc)) { + return -1; + } + return 0; +} + +extern "C" int userspace_kins_evaluate_joints(const TC_STRUCT* tc, + double progress, + double* joints) { + if (!g_userspace_kins_planner.evaluateJointPositions(tc, progress, joints)) { + return -1; + } + return 0; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/userspace_kinematics.hh b/src/emc/motion_planning/userspace_kinematics.hh new file mode 100644 index 00000000000..42fbf8351db --- /dev/null +++ b/src/emc/motion_planning/userspace_kinematics.hh @@ -0,0 +1,218 @@ +/******************************************************************** + * Description: userspace_kinematics.hh + * Userspace kinematics integration for trajectory planning + * + * This module integrates the userspace kinematics components: + * - Path sampling + * - Jacobian calculation + * - Joint limit enforcement + * - Joint-space segment generation + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef USERSPACE_KINEMATICS_HH +#define USERSPACE_KINEMATICS_HH + +#include +#include "path_sampler.hh" +#include "jacobian.hh" +#include "joint_limits.hh" + +// tc_types.h and emcpos.h include posemath.h which has C++ overloads +#include "tc_types.h" +#include "emcpos.h" + +namespace motion_planning { + +/** + * Userspace kinematics planner configuration + */ +struct UserspaceKinematicsConfig { + bool enabled; // True if userspace kinematics path is active + std::string kinematics_type; // Kinematics type string (e.g., "trivkins") + int num_joints; // Number of joints + std::string coordinates; // Coordinate mapping (e.g., "XYZABC") + double singularity_threshold; // Condition number threshold for slowdown + + UserspaceKinematicsConfig() : + enabled(false), + kinematics_type("trivkins"), + num_joints(3), + coordinates("XYZ"), + singularity_threshold(100.0) {} +}; + +/** + * Userspace kinematics planner class + * + * Integrates path sampling, Jacobian calculation, and joint limits + * to compute joint-space segments from world-space moves. + */ +class UserspaceKinematicsPlanner { +public: + UserspaceKinematicsPlanner(); + ~UserspaceKinematicsPlanner(); + + /** + * Initialize the userspace kinematics planner + * + * @param config Configuration including kinematics type and joints + * @return true on success, false if kinematics not supported + */ + bool init(const UserspaceKinematicsConfig& config); + + /** + * Set joint limits from motion controller configuration + * + * @param joint Joint index + * @param vel_limit Maximum velocity + * @param acc_limit Maximum acceleration + * @param min_pos Minimum position (soft limit) + * @param max_pos Maximum position (soft limit) + * @return true on success + */ + bool setJointLimits(int joint, + double vel_limit, + double acc_limit, + double min_pos, + double max_pos); + + /** + * Compute joint-space segment for a linear move + * + * Samples the path, computes Jacobian at each sample, determines + * limiting velocity/acceleration from joint limits, and populates + * the JointSpaceSegment in the TC_STRUCT. + * + * @param start Start position in world coordinates + * @param end End position in world coordinates + * @param tc TC_STRUCT to populate with joint_space data + * @return true on success + */ + bool computeJointSpaceSegment(const EmcPose& start, + const EmcPose& end, + TC_STRUCT* tc); + + /** + * Evaluate joint positions at a given progress through segment + * + * Uses linear interpolation: + * joint[j] = start[j] + progress * (end[j] - start[j]) + * + * @param tc TC_STRUCT with joint_space data + * @param progress Progress through segment [0, 1] + * @param joints Output array of joint positions + * @return true on success + */ + bool evaluateJointPositions(const TC_STRUCT* tc, + double progress, + double* joints); + + /** + * Check if userspace kinematics planner is enabled and initialized + */ + bool isEnabled() const { return enabled_ && initialized_; } + + /** + * Check if using identity kinematics + */ + bool isIdentity() const; + + /** + * Get the current configuration + */ + const UserspaceKinematicsConfig& getConfig() const { return config_; } + + /** + * Get per-joint jerk limit (for direction-dependent kink velocity computation) + */ + double getJointJerkLimit(int joint) const { return limit_calc_.getJointJerkLimit(joint); } + + /** + * Get per-joint acceleration limit + */ + double getJointAccLimit(int joint) const { return limit_calc_.getJointAccLimit(joint); } + + /** + * Get number of joints + */ + int getNumJoints() const { return limit_calc_.getNumJoints(); } + + /** + * Compute Jacobian at a world pose + * + * @param pose World-space pose + * @param J Output 9x9 Jacobian matrix + * @return true on success + */ + bool computeJacobian(const EmcPose& pose, double J[9][9]) const; + +private: + UserspaceKinematicsConfig config_; + bool initialized_; + bool enabled_; + + KinematicsUserContext* kins_ctx_; + PathSampler path_sampler_; + JacobianCalculator jacobian_calc_; + JointLimitCalculator limit_calc_; +}; + +/** + * Global userspace kinematics planner instance + * + * This is automatically initialized when PLANNER_TYPE=2 in the INI file. + */ +extern UserspaceKinematicsPlanner g_userspace_kins_planner; + +/** + * Initialize userspace kinematics planner from INI configuration + * + * Called during motion controller initialization if userspace kinematics is enabled. + * + * @param kins_type Kinematics type string + * @param num_joints Number of joints + * @param coordinates Coordinate mapping string + * @return 0 on success, -1 on failure + */ +extern "C" int userspace_kins_init(const char* kins_type, + int num_joints, + const char* coordinates); + +/** + * Set joint limits for userspace kinematics planner + * + * Called after userspace_kins_init for each joint. + */ +extern "C" int userspace_kins_set_joint_limits(int joint, + double vel_limit, + double acc_limit, + double min_pos, + double max_pos); + +/** + * Check if userspace kinematics planner is enabled + */ +extern "C" int userspace_kins_is_enabled(void); + +/** + * Compute joint-space segment (C interface) + */ +extern "C" int userspace_kins_compute_joint_segment(const EmcPose* start, + const EmcPose* end, + TC_STRUCT* tc); + +/** + * Evaluate joint positions at progress (C interface) + */ +extern "C" int userspace_kins_evaluate_joints(const TC_STRUCT* tc, + double progress, + double* joints); + +} // namespace motion_planning + +#endif // USERSPACE_KINEMATICS_HH diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 3eba83a9c1d..9826c1fca8a 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -369,6 +369,9 @@ extern int emcTrajForward(); extern int emcTrajStep(); extern int emcTrajResume(); extern int emcTrajDelay(double delay); +extern int emcTrajQueueSpindleOn(int spindle, double speed, int direction); +extern int emcTrajQueueSpindleOff(int spindle); +extern int emcTrajQueueCoolant(int mist_on, int flood_on); extern int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, int indexer_jnum); extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN& @@ -454,6 +457,7 @@ int emcSetupArcBlends(int arcBlendEnable, double arcBlendRampFreq, double arcBlendTangentKinkRatio); int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit); +int emcSetEmulateLegacyMoveCommands(int emulate); int emcGetExternalOffsetApplied(void); EmcPose emcGetExternalOffsets(void); diff --git a/src/emc/task/Submakefile b/src/emc/task/Submakefile index fabd1cd4a12..f60bed51acd 100644 --- a/src/emc/task/Submakefile +++ b/src/emc/task/Submakefile @@ -29,7 +29,7 @@ USERSRCS += $(MILLTASKSRCS) ../bin/milltask: $(call TOOBJS, $(MILLTASKSRCS)) ../lib/librs274.so.0 ../lib/liblinuxcnc.a \ ../lib/libnml.so.0 ../lib/liblinuxcncini.so.0 ../lib/libposemath.so.0 \ ../lib/liblinuxcnchal.so.0 ../lib/libpyplugin.so.0 \ - ../lib/libtooldata.so.0 + ../lib/libtooldata.so.0 ../lib/libmotion_planning_9d.so.0 $(ECHO) Linking $(notdir $@) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 0df62f0d5e7..b593fea3875 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3000,6 +3000,14 @@ auto SPINDLE_SPEED_(int s, int dir, double speed) void START_SPINDLE_CLOCKWISE(int s, int wait_for_atspeed) { + // For planner_type 2: queue inline action instead of NML message + // This avoids flush_segments() which breaks velocity continuity + if (emcTrajQueueSpindleOn(s, canon.spindle[s].speed, 1) == 0) { + canon.spindle[s].dir = 1; + return; + } + + // Fallback: use NML message approach auto msg = SPINDLE_SPEED_<>(s, 1, 0); msg->wait_for_spindle_at_speed = wait_for_atspeed; interp_list.append(std::move(msg)); @@ -3007,6 +3015,13 @@ void START_SPINDLE_CLOCKWISE(int s, int wait_for_atspeed) void START_SPINDLE_COUNTERCLOCKWISE(int s, int wait_for_atspeed) { + // For planner_type 2: queue inline action + if (emcTrajQueueSpindleOn(s, canon.spindle[s].speed, -1) == 0) { + canon.spindle[s].dir = -1; + return; + } + + // Fallback: use NML message approach auto msg = SPINDLE_SPEED_<>(s, -1, 0); msg->wait_for_spindle_at_speed = wait_for_atspeed; interp_list.append(std::move(msg)); @@ -3014,11 +3029,27 @@ void START_SPINDLE_COUNTERCLOCKWISE(int s, int wait_for_atspeed) void SET_SPINDLE_SPEED(int s, double speed_rpm) { + // For planner_type 2: queue inline action if spindle is already running + if (canon.spindle[s].dir != 0) { + if (emcTrajQueueSpindleOn(s, fabs(speed_rpm), canon.spindle[s].dir) == 0) { + canon.spindle[s].speed = fabs(speed_rpm); + return; + } + } + + // Fallback: use NML message approach interp_list.append(SPINDLE_SPEED_(s, 0, speed_rpm)); } void STOP_SPINDLE_TURNING(int s) { + // For planner_type 2: queue inline action + if (emcTrajQueueSpindleOff(s) == 0) { + canon.spindle[s].dir = 0; + return; + } + + // Fallback: use NML message approach auto emc_spindle_off_msg = std::make_unique(); flush_segments(); @@ -3420,11 +3451,21 @@ void SIMPLE_COMMAND_() void FLOOD_OFF() { + // For planner_type 2: queue inline action + if (emcTrajQueueCoolant(0, 0) == 0) { + return; + } + // Fallback: use NML message approach SIMPLE_COMMAND_(); } void FLOOD_ON() { + // For planner_type 2: queue inline action + if (emcTrajQueueCoolant(0, 1) == 0) { + return; + } + // Fallback: use NML message approach SIMPLE_COMMAND_(); } @@ -3468,11 +3509,21 @@ void LOGCLOSE() { void MIST_OFF() { + // For planner_type 2: queue inline action + if (emcTrajQueueCoolant(0, 0) == 0) { + return; + } + // Fallback: use NML message approach SIMPLE_COMMAND_(); } void MIST_ON() { + // For planner_type 2: queue inline action + if (emcTrajQueueCoolant(1, 0) == 0) { + return; + } + // Fallback: use NML message approach SIMPLE_COMMAND_(); } diff --git a/src/emc/task/emctask.cc b/src/emc/task/emctask.cc index a17f604a448..3d0f7938e97 100644 --- a/src/emc/task/emctask.cc +++ b/src/emc/task/emctask.cc @@ -232,6 +232,9 @@ int emcTaskAbort() { emcMotionAbort(); + // REMOVED: Immediate FREE mode call (no longer needed) + // Was part of workaround for pausing flag bug, now fixed in tpCleanupAfterAbort_9D() + // clear out the pending command emcTaskCommand = 0; interp_list.clear(); @@ -288,16 +291,22 @@ int emcTaskSetMode(EMC_TASK_MODE mode) case EMC_TASK_MODE::MDI: // go to mdi mode - emcTrajSetMode(EMC_TRAJ_MODE::COORD); + // IMPORTANT: Abort BEFORE entering COORD mode to ensure clean state. + // If COORD is sent first, ABORT may arrive late (after segments are + // queued) and wipe fresh segments that haven't started moving yet. emcTaskAbort(); + emcTrajSetMode(EMC_TRAJ_MODE::COORD); emcTaskPlanSynch(); mdiOrAuto = EMC_TASK_MODE::MDI; break; case EMC_TASK_MODE::AUTO: // go to auto mode - emcTrajSetMode(EMC_TRAJ_MODE::COORD); + // IMPORTANT: Abort BEFORE entering COORD mode to ensure clean state. + // If COORD is sent first, ABORT may arrive late (after segments are + // queued) and wipe fresh segments that haven't started moving yet. emcTaskAbort(); + emcTrajSetMode(EMC_TRAJ_MODE::COORD); emcTaskPlanSynch(); mdiOrAuto = EMC_TASK_MODE::AUTO; break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 5029143ae42..8e1016575b3 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -20,6 +20,9 @@ #include "usrmotintf.h" // usrmotInit(), usrmotReadEmcmotStatus(), // etc. #include "motion.h" // emcmot_command_t,STATUS, etc. +#include "tp.h" // tpAddLine(), tpAddCircle(), tpSetId() for dual-layer arch +#include "motion_planning_9d.hh" // tpOptimizePlannedMotions_9D() for planner_type 2 +#include "userspace_kinematics.hh" // userspace_kins_is_enabled() for planner_type 2 guard #include "homing.h" #include "emc.hh" #include "emccfg.h" // EMC_INIFILE @@ -1176,6 +1179,17 @@ int emcTrajSetJerk(double jerk) int emcTrajPlannerType(int type) { + if (type == 2) { + // Planner type 2 requires userspace kinematics to be initialized at startup. + // If switching via HAL pin and it wasn't configured, reject with a warning. + if (!motion_planning::userspace_kins_is_enabled()) { + rcs_print_error("Cannot switch to PLANNER_TYPE 2: userspace kinematics not initialized.\n"); + rcs_print_error("PLANNER_TYPE 2 requires PLANNER_TYPE = 2 in the [TRAJ] section of the INI file.\n"); + rcs_print_error("Userspace kinematics cannot be initialized at runtime.\n"); + return -1; + } + } + emcmotCommand.command = EMCMOT_SET_PLANNER_TYPE; emcmotCommand.planner_type = type; @@ -1371,6 +1385,8 @@ int emcTrajInit() if (0 != usrmotInit("emc2_task")) { return -1; } + // Note: tpMotData() call removed - direct TP calls disabled until + // queue storage is moved to shared memory (see emcTrajLinearMove comment) } TrajConfig.Inited = 1; // initialize parameters from INI file @@ -1407,6 +1423,17 @@ int emcTrajDisable() int emcTrajAbort() { + // For planner_type 2: compute stop branch BEFORE sending abort command. + // This guarantees the stop branch is ready when RT processes the abort. + // Uses existing branch/merge infrastructure for smooth jerk-limited stop. + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (status && status->planner_type == 2) { + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + tpRequestAbortBranch_9D(tp); + } + } + emcmotCommand.command = EMCMOT_ABORT; return usrmotWriteEmcmotCommand(&emcmotCommand); @@ -1447,10 +1474,119 @@ int emcTrajResume() return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajDelay(double /*delay*/) +int emcTrajDelay(double delay) +{ + // Check if planner_type is 2 (9D dual-layer architecture) + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (status && status->planner_type == 2) { + // Use inline dwell segment for planner_type 2 + // This keeps dwell in the motion buffer, maintaining proper sync + // without forcing queue drain (which breaks velocity continuity) + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + int result = tpAddDwell(tp, delay, localEmcTrajTag); + if (result == 0) { + // Trigger optimization to compute profiles for segments after dwell + tpOptimizePlannedMotions_9D(tp, 16); + rtapi_print_msg(RTAPI_MSG_DBG, + "emcTrajDelay: added inline dwell %.3fs for planner_type 2\n", delay); + return 0; + } + // If tpAddDwell failed, fall through to default behavior + rtapi_print_msg(RTAPI_MSG_WARN, + "emcTrajDelay: tpAddDwell failed, falling back to task-layer delay\n"); + } + } + + /* For planner_type 0 and 1 (or fallback): done in task controller */ + return 0; +} + +/** + * Queue spindle ON action for planner_type 2 inline execution + * + * For planner_type 2 (9D architecture), spindle commands are queued as + * segment actions and fired by RT when the next segment activates. + * This maintains velocity continuity by not forcing queue drain. + * + * @param spindle Spindle number (0-based) + * @param speed Speed in RPM (positive) + * @param direction 1 for CW, -1 for CCW + * @return 0 on success, -1 if not using planner_type 2 + */ +int emcTrajQueueSpindleOn(int spindle, double speed, int direction) { - /* nothing need be done here - it's done in task controller */ + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (!status || status->planner_type != 2) { + return -1; // Not planner_type 2, use normal command path + } + + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (!tp) { + return -1; + } + segment_action_type_t action = (direction > 0) ? SEG_ACTION_SPINDLE_CW : SEG_ACTION_SPINDLE_CCW; + int result = tpSetSegmentAction(tp, action, spindle, speed); + if (result == 0) { + rtapi_print_msg(RTAPI_MSG_DBG, + "emcTrajQueueSpindleOn: queued spindle %d %s @ %.1f RPM\n", + spindle, (direction > 0) ? "CW" : "CCW", speed); + } + return result; +} + +/** + * Queue spindle OFF action for planner_type 2 inline execution + */ +int emcTrajQueueSpindleOff(int spindle) +{ + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (!status || status->planner_type != 2) { + return -1; + } + + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (!tp) { + return -1; + } + + int result = tpSetSegmentAction(tp, SEG_ACTION_SPINDLE_OFF, spindle, 0.0); + if (result == 0) { + rtapi_print_msg(RTAPI_MSG_DBG, + "emcTrajQueueSpindleOff: queued spindle %d OFF\n", spindle); + } + return result; +} + +/** + * Queue coolant action for planner_type 2 inline execution + */ +int emcTrajQueueCoolant(int mist_on, int flood_on) +{ + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (!status || status->planner_type != 2) { + return -1; + } + + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (!tp) { + return -1; + } + + // Queue appropriate actions based on requested state + if (mist_on) { + tpSetSegmentAction(tp, SEG_ACTION_COOLANT_MIST, 0, 0.0); + } + if (flood_on) { + tpSetSegmentAction(tp, SEG_ACTION_COOLANT_FLOOD, 0, 0.0); + } + if (!mist_on && !flood_on) { + tpSetSegmentAction(tp, SEG_ACTION_COOLANT_OFF, 0, 0.0); + } + + rtapi_print_msg(RTAPI_MSG_DBG, + "emcTrajQueueCoolant: mist=%d flood=%d\n", mist_on, flood_on); return 0; } @@ -1482,15 +1618,25 @@ int emcTrajSetSpindleSync(int spindle, double fpr, bool wait_for_index) int emcTrajSetTermCond(int cond, double tolerance) { + // For planner_type 2: write directly to shared memory so the value is + // immediately visible to tpAddLine_9D/tpAddCircle_9D (called from this + // same thread). Without this, the RT command queue introduces a race: + // moves get added before RT processes the SET_TERM_COND command. + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + tp->termCond = cond; + tp->tolerance = tolerance; + } + + // Also send RT command (needed for planner_type 0/1 and RT-side state) emcmotCommand.command = EMCMOT_SET_TERM_COND; - // Direct passthrough since TP can handle the distinction now emcmotCommand.termCond = cond; emcmotCommand.tolerance = tolerance; return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, +int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk, int indexer_jnum) { #ifdef ISNAN_TRAP @@ -1502,10 +1648,38 @@ int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxve } #endif - emcmotCommand.command = EMCMOT_SET_LINE; + // Check if planner_type is 2 (9D dual-layer architecture) + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (status && status->planner_type == 2) { + // Use direct userspace planning for 9D planner + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + // Get enables from status (FS_ENABLED, SS_ENABLED, etc.) + // This is critical for feed override to work! + unsigned char enables = status->enables_new; + int result = tpAddLine_9D(tp, end, type, vel, ini_maxvel, acc, enables, localEmcTrajTag); + + if (result == 0) { + // Success - motion queued in userspace + // Tell RT to skip re-queuing (but RT still executes from queue) + emcmotCommand.command = EMCMOT_SET_LINE; + emcmotCommand.pos = end; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.motion_type = type; + emcmotCommand.userspace_already_queued = 1; // Skip RT queuing + return usrmotWriteEmcmotCommand(&emcmotCommand); + } + + // If userspace planning failed, fall back to NML + rcs_print("9D userspace planning failed, falling back to RT queuing\n"); + } + // Fall through to NML path if TP pointer unavailable + } + // Default path: Use NML messaging for planner_type 0 and 1 (or fallback for type 2) + emcmotCommand.command = EMCMOT_SET_LINE; emcmotCommand.pos = end; - emcmotCommand.id = TrajConfig.MotionId; emcmotCommand.tag = localEmcTrajTag; emcmotCommand.motion_type = type; @@ -1514,6 +1688,7 @@ int emcTrajLinearMove(const EmcPose& end, int type, double vel, double ini_maxve emcmotCommand.acc = acc; emcmotCommand.ini_maxjerk = ini_maxjerk; emcmotCommand.turn = indexer_jnum; + emcmotCommand.userspace_already_queued = 0; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1532,27 +1707,65 @@ int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, } #endif - emcmotCommand.command = EMCMOT_SET_CIRCLE; + // Check if planner_type is 2 (9D dual-layer architecture) + emcmot_status_t *status = usrmotGetEmcmotStatus(); + if (status && status->planner_type == 2) { + // Use direct userspace planning for 9D planner + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + // Get enables from status (FS_ENABLED, SS_ENABLED, etc.) + unsigned char enables = status->enables_new; + + // Convert PM_CARTESIAN to PmCartesian for tpAddCircle_9D + PmCartesian pm_center = {center.x, center.y, center.z}; + PmCartesian pm_normal = {normal.x, normal.y, normal.z}; + + int result = tpAddCircle_9D(tp, end, pm_center, pm_normal, turn, type, + vel, ini_maxvel, acc, enables, localEmcTrajTag); + + if (result == 0) { + // Success - motion queued in userspace + // Tell RT to skip re-queuing (but RT still executes from queue) + emcmotCommand.command = EMCMOT_SET_CIRCLE; + emcmotCommand.pos = end; + emcmotCommand.motion_type = type; + emcmotCommand.center.x = center.x; + emcmotCommand.center.y = center.y; + emcmotCommand.center.z = center.z; + emcmotCommand.normal.x = normal.x; + emcmotCommand.normal.y = normal.y; + emcmotCommand.normal.z = normal.z; + emcmotCommand.turn = turn; + emcmotCommand.id = TrajConfig.MotionId; + emcmotCommand.tag = localEmcTrajTag; + emcmotCommand.userspace_already_queued = 1; // Skip RT queuing + return usrmotWriteEmcmotCommand(&emcmotCommand); + } + + // If userspace planning failed, fall back to NML + rcs_print("9D userspace arc planning failed, falling back to RT queuing\n"); + } + // Fall through to NML path if TP pointer unavailable + } + // Default path: Use NML messaging for planner_type 0 and 1 (or fallback for type 2) + emcmotCommand.command = EMCMOT_SET_CIRCLE; emcmotCommand.pos = end; emcmotCommand.motion_type = type; - emcmotCommand.center.x = center.x; emcmotCommand.center.y = center.y; emcmotCommand.center.z = center.z; - emcmotCommand.normal.x = normal.x; emcmotCommand.normal.y = normal.y; emcmotCommand.normal.z = normal.z; - emcmotCommand.turn = turn; emcmotCommand.id = TrajConfig.MotionId; emcmotCommand.tag = localEmcTrajTag; - emcmotCommand.vel = vel; emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.ini_maxjerk = ini_maxjerk; + emcmotCommand.userspace_already_queued = 0; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1598,6 +1811,7 @@ int emcTrajRigidTap(const EmcPose& pos, double vel, double ini_maxvel, double ac } #endif + // TODO: Direct TP calls disabled - see emcTrajLinearMove comment emcmotCommand.command = EMCMOT_RIGID_TAP; emcmotCommand.pos.tran = pos.tran; emcmotCommand.id = TrajConfig.MotionId; @@ -1724,6 +1938,17 @@ int emcTrajUpdate(EMC_TRAJ_STAT * stat) stat->maxVelocity = emcmotConfig.limitVel; } + // For 9D planner: check for feed override changes during motion + // This is called periodically so feed changes are detected even when + // no new segments are being added + emcmot_status_t *mot_status = usrmotGetEmcmotStatus(); + if (mot_status && mot_status->planner_type == 2) { + TP_STRUCT *tp = usrmotGetTPDataPtr(); + if (tp) { + checkFeedOverride(tp); + } + } + return 0; } @@ -2060,6 +2285,15 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) { return -1; } + + // ======================================================================== + // REMOVED: MDI idle detection workaround (no longer needed) + // + // The workaround was compensating for tp->pausing flag not being cleared + // on abort. That bug is now fixed in tpCleanupAfterAbort_9D() which sets + // pausing=0. Keeping the workaround causes busy loops during abort. + // ======================================================================== + new_config = 0; if (emcmotStatus.config_num != emcmotConfig.config_num) { if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) { @@ -2173,6 +2407,12 @@ int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit) { return usrmotWriteEmcmotCommand(&emcmotCommand); } +int emcSetEmulateLegacyMoveCommands(int emulate) { + emcmotCommand.command = EMCMOT_SET_EMULATE_LEGACY_MOVE_COMMANDS; + emcmotCommand.emulate_legacy_move_commands = emulate; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcGetExternalOffsetApplied(void) { return emcmotStatus.external_offsets_applied; } diff --git a/src/emc/tp/bezier9_rt.c b/src/emc/tp/bezier9_rt.c new file mode 100644 index 00000000000..e359c5b1333 --- /dev/null +++ b/src/emc/tp/bezier9_rt.c @@ -0,0 +1,252 @@ +/** + * bezier9_rt.c — RT-safe subset of Bezier9 evaluation functions + * + * This file provides the functions needed by the RT trajectory planner + * (tpmod.so) to evaluate pre-built Bezier9 curves during execution. + * Heavy initialization (Gauss-Legendre quadrature, curvature analysis) + * is done in userspace by bezier9.cc; RT only evaluates using the + * precomputed lookup tables stored in the Bezier9 struct. + * + * Functions provided: + * bezier9Point — position at arc-length parameter + * bezier9Tangent — unit tangent vectors at arc-length parameter + * bezier9AccLimit — velocity limit from cached curvature + * bezier9Length — total arc length accessor + * + * NOT provided (userspace only): + * bezier9Init, bezier9Curvature, bezier9MaxCurvature, bezier9Deviation + */ + +#include "posemath.h" +#include "bezier9.h" +#include "blendmath.h" /* BLEND_ACC_RATIO_NORMAL */ +#include "rtapi_math.h" +#include "tp_types.h" + +/** + * bezier5_eval — Evaluate quintic Bezier curve at parameter t + * + * B(t) = (1-t)^5 P0 + 5(1-t)^4 t P1 + 10(1-t)^3 t^2 P2 + * + 10(1-t)^2 t^3 P3 + 5(1-t) t^4 P4 + t^5 P5 + */ +static void bezier5_eval(PmCartesian const * const P, + double t, + PmCartesian * const out) +{ + double s = 1.0 - t; + double s2 = s * s; + double s3 = s2 * s; + double s4 = s3 * s; + double s5 = s4 * s; + double t2 = t * t; + double t3 = t2 * t; + double t4 = t3 * t; + double t5 = t4 * t; + + double b0 = s5; + double b1 = 5.0 * s4 * t; + double b2 = 10.0 * s3 * t2; + double b3 = 10.0 * s2 * t3; + double b4 = 5.0 * s * t4; + double b5 = t5; + + out->x = b0*P[0].x + b1*P[1].x + b2*P[2].x + b3*P[3].x + b4*P[4].x + b5*P[5].x; + out->y = b0*P[0].y + b1*P[1].y + b2*P[2].y + b3*P[3].y + b4*P[4].y + b5*P[5].y; + out->z = b0*P[0].z + b1*P[1].z + b2*P[2].z + b3*P[3].z + b4*P[4].z + b5*P[5].z; +} + +/** + * bezier5_deriv — Evaluate first derivative of quintic Bezier at t + * + * B'(t) = 5 * sum_{i=0}^{4} C(4,i) (1-t)^(4-i) t^i (P_{i+1} - P_i) + */ +static void bezier5_deriv(PmCartesian const * const P, + double t, + PmCartesian * const out) +{ + double s = 1.0 - t; + double s2 = s * s; + double s3 = s2 * s; + double s4 = s3 * s; + double t2 = t * t; + double t3 = t2 * t; + double t4 = t3 * t; + + double c0 = 5.0 * s4; + double c1 = 5.0 * 4.0 * s3 * t; + double c2 = 5.0 * 6.0 * s2 * t2; + double c3 = 5.0 * 4.0 * s * t3; + double c4 = 5.0 * t4; + + PmCartesian D[5]; + int i; + for (i = 0; i < 5; i++) { + D[i].x = P[i+1].x - P[i].x; + D[i].y = P[i+1].y - P[i].y; + D[i].z = P[i+1].z - P[i].z; + } + + out->x = c0*D[0].x + c1*D[1].x + c2*D[2].x + c3*D[3].x + c4*D[4].x; + out->y = c0*D[0].y + c1*D[1].y + c2*D[2].y + c3*D[3].y + c4*D[4].y; + out->z = c0*D[0].z + c1*D[1].z + c2*D[2].z + c3*D[3].z + c4*D[4].z; +} + +/** + * arc_length_to_t — Map arc length s to Bezier parameter t + * + * Uses binary search on precomputed lookup table, then linear interpolation. + */ +static double arc_length_to_t(Bezier9 const * const b, double s) +{ + if (s <= 0.0) return 0.0; + if (s >= b->total_length) return 1.0; + + int lo = 0; + int hi = BEZIER9_ARC_LENGTH_SAMPLES; + + while (hi - lo > 1) { + int mid = (lo + hi) / 2; + if (b->s_table[mid] < s) { + lo = mid; + } else { + hi = mid; + } + } + + double s0 = b->s_table[lo]; + double s1 = b->s_table[hi]; + double t0 = b->t_table[lo]; + double t1 = b->t_table[hi]; + + double ds = s1 - s0; + if (ds < BEZIER9_MIN_LENGTH) { + return t0; + } + + double alpha = (s - s0) / ds; + return t0 + alpha * (t1 - t0); +} + +/* ---------------------------------------------------------------- + * Public API — called from tc.c dispatch (tcGetPosReal, etc.) + * ---------------------------------------------------------------- */ + +int bezier9Point(Bezier9 const * const b, + double progress, + EmcPose * const out) +{ + if (!b || !out) { + return TP_ERR_MISSING_INPUT; + } + + double t = arc_length_to_t(b, progress); + + PmCartesian xyz, abc, uvw; + bezier5_eval(b->P, t, &xyz); + bezier5_eval(b->A, t, &abc); + bezier5_eval(b->U, t, &uvw); + + out->tran.x = xyz.x; + out->tran.y = xyz.y; + out->tran.z = xyz.z; + out->a = abc.x; + out->b = abc.y; + out->c = abc.z; + out->u = uvw.x; + out->v = uvw.y; + out->w = uvw.z; + + return TP_ERR_OK; +} + +int bezier9Tangent(Bezier9 const * const b, + double progress, + PmCartesian * const xyz_out, + PmCartesian * const abc_out, + PmCartesian * const uvw_out) +{ + if (!b) { + return TP_ERR_MISSING_INPUT; + } + + double t = arc_length_to_t(b, progress); + + PmCartesian dP, dA, dU; + bezier5_deriv(b->P, t, &dP); + bezier5_deriv(b->A, t, &dA); + bezier5_deriv(b->U, t, &dU); + + if (xyz_out) { + double mag = sqrt(dP.x * dP.x + dP.y * dP.y + dP.z * dP.z); + if (mag > BEZIER9_POS_EPSILON) { + xyz_out->x = dP.x / mag; + xyz_out->y = dP.y / mag; + xyz_out->z = dP.z / mag; + } else { + xyz_out->x = 0.0; + xyz_out->y = 0.0; + xyz_out->z = 0.0; + } + } + + if (abc_out) { + double mag = sqrt(dA.x * dA.x + dA.y * dA.y + dA.z * dA.z); + if (mag > BEZIER9_POS_EPSILON) { + abc_out->x = dA.x / mag; + abc_out->y = dA.y / mag; + abc_out->z = dA.z / mag; + } else { + abc_out->x = 0.0; + abc_out->y = 0.0; + abc_out->z = 0.0; + } + } + + if (uvw_out) { + double mag = sqrt(dU.x * dU.x + dU.y * dU.y + dU.z * dU.z); + if (mag > BEZIER9_POS_EPSILON) { + uvw_out->x = dU.x / mag; + uvw_out->y = dU.y / mag; + uvw_out->z = dU.z / mag; + } else { + uvw_out->x = 0.0; + uvw_out->y = 0.0; + uvw_out->z = 0.0; + } + } + + return TP_ERR_OK; +} + +double bezier9Length(Bezier9 const * const b) +{ + if (!b) { + return 0.0; + } + return b->total_length; +} + +double bezier9AccLimit(Bezier9 const * const b, + double v_req, + double a_max, + double j_max) +{ + (void)v_req; + + if (!b || a_max <= 0.0) { + return 0.0; + } + + double a_normal = BLEND_ACC_RATIO_NORMAL * a_max; + double v_limit = sqrt(a_normal * b->min_radius); + + if (j_max > 0.0 && b->max_dkappa_ds > BEZIER9_POS_EPSILON) { + double j_normal = BLEND_ACC_RATIO_NORMAL * j_max; + double v_jerk = cbrt(j_normal / b->max_dkappa_ds); + if (v_jerk < v_limit) { + v_limit = v_jerk; + } + } + + return v_limit; +} diff --git a/src/emc/tp/spherical_arc.h b/src/emc/tp/spherical_arc.h index cea336909b9..a1b15b8ad6d 100644 --- a/src/emc/tp/spherical_arc.h +++ b/src/emc/tp/spherical_arc.h @@ -15,6 +15,10 @@ #include "posemath.h" +#ifdef __cplusplus +extern "C" { +#endif + #define ARC_POS_EPSILON 1e-12 #define ARC_MIN_RADIUS 1e-12 #define ARC_MIN_ANGLE 1e-6 @@ -64,4 +68,9 @@ int arcConvexTest(PmCartesian const * const center, PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir); int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end); + +#ifdef __cplusplus +} +#endif + #endif diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 772a743dde3..63b83357741 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -25,6 +25,7 @@ #include "motion_types.h" #include "motion.h" #include "ruckig_wrapper.h" +#include "bezier9.h" //Debug output #include "tp_debug.h" @@ -84,11 +85,12 @@ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) { double a_scale = tcGetOverallMaxAccel(tc); - // Reduce allowed tangential acceleration in circular motions to stay + // Reduce allowed tangential acceleration in curved motions to stay // within overall limits (accounts for centripetal acceleration while - // moving along the circular path). - if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { - //Limit acceleration for circular arcs to allow for normal acceleration + // moving along the curved path). + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL || + tc->motion_type == TC_BEZIER) { + //Limit acceleration for arcs/beziers to allow for normal acceleration a_scale *= tc->acc_ratio_tan; } return a_scale; @@ -297,6 +299,9 @@ int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const case TC_CIRCULAR: pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); break; + case TC_BEZIER: + bezier9Tangent(&tc->coords.bezier, 0.0, out, NULL, NULL); + break; default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -320,6 +325,9 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou pmCircleTangentVector(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, out); break; + case TC_BEZIER: + bezier9Tangent(&tc->coords.bezier, tc->coords.bezier.total_length, out, NULL, NULL); + break; default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); return -1; @@ -375,6 +383,9 @@ int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * cons arcTangent(arc, out, at_end); } break; + case TC_BEZIER: + bezier9Tangent(&tc->coords.bezier, tc->progress, out, NULL, NULL); + break; default: rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); return -1; @@ -498,6 +509,15 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) abc = tc->coords.arc.abc; uvw = tc->coords.arc.uvw; break; + case TC_BEZIER: + bezier9Point(&tc->coords.bezier, progress, pos); + return TP_ERR_OK; + case TC_DWELL: + // Dwell holds position - return start point + xyz = tc->coords.line.xyz.start; + abc = tc->coords.line.abc.start; + uvw = tc->coords.line.uvw.start; + break; } if (res_fit == TP_ERR_OK) { @@ -736,7 +756,6 @@ int tcSetupMotion(TC_STRUCT * const tc, tc->maxaccel = acc; tc->maxjerk = ini_maxjerk; - tc->blend_maxjerk = ini_maxjerk; // default equals maxjerk, look-ahead adjusts as needed tc->maxvel = ini_maxvel; @@ -852,6 +871,28 @@ int tcUpdateArcLimits(TC_STRUCT * tc) radius = tc->coords.arc.xyz.radius; angle = tc->coords.arc.xyz.angle; break; + case TC_BEZIER: { + // Bezier blend: use precomputed curvature for velocity limiting + double v_limit = bezier9AccLimit(&tc->coords.bezier, + tc->reqvel, tc->maxaccel, + tc->maxjerk); + if (v_limit > 0.0 && v_limit < tc->maxvel) { + tc->maxvel = v_limit; + } + // Compute acc_ratio_tan from curvature + if (tc->coords.bezier.min_radius > TP_POS_EPSILON) { + double a_max_bez = tcGetOverallMaxAccel(tc); + double a_n = pmSq(tc->maxvel) / tc->coords.bezier.min_radius; + if (a_n < a_max_bez) { + tc->acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n / a_max_bez)); + } else { + tc->acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + } + } else { + tc->acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + } + return 0; + } default: return 1; // Not an arc, nothing to do } @@ -1104,4 +1145,197 @@ int tcClearFlags(TC_STRUCT * const tc) return TP_ERR_OK; } +/** + * Sample Ruckig profile at given time + * + * Evaluates the pre-computed Ruckig trajectory profile at time t. + * The profile consists of 7 phases with constant jerk in each phase. + * Within each phase, we integrate: + * a(dt) = a0 + j * dt + * v(dt) = v0 + a0*dt + 0.5*j*dt^2 + * p(dt) = p0 + v0*dt + 0.5*a0*dt^2 + (1/6)*j*dt^3 + * + * @param profile Pointer to ruckig_profile_t + * @param t Time since segment start (seconds) + * @param pos Output: position at time t + * @param vel Output: velocity at time t + * @param acc Output: acceleration at time t + * @param jerk Output: jerk at time t + * @return 0 on success, -1 if profile invalid + */ +int ruckigProfileSample(ruckig_profile_t const * const profile, + double t, + double *pos, + double *vel, + double *acc, + double *jerk) +{ + if (!profile || !profile->valid) { + return -1; + } + + // Clamp time to valid range + if (t < 0.0) t = 0.0; + if (t >= profile->duration) { + // At or past end - return final state with zero jerk + *pos = profile->p[RUCKIG_PROFILE_PHASES]; + *vel = profile->v[RUCKIG_PROFILE_PHASES]; + *acc = profile->a[RUCKIG_PROFILE_PHASES]; + *jerk = 0.0; + return 0; + } + + // Find which phase we're in + int phase = 0; + double t_phase_start = 0.0; + + for (int i = 0; i < RUCKIG_PROFILE_PHASES; i++) { + if (t < profile->t_sum[i]) { + phase = i; + break; + } + t_phase_start = profile->t_sum[i]; + phase = i + 1; + } + + // Clamp phase to valid range + if (phase >= RUCKIG_PROFILE_PHASES) { + phase = RUCKIG_PROFILE_PHASES - 1; + t_phase_start = (phase > 0) ? profile->t_sum[phase - 1] : 0.0; + } + + // Time within this phase + double dt = t - t_phase_start; + + // Get phase boundary conditions + double p0 = profile->p[phase]; + double v0 = profile->v[phase]; + double a0 = profile->a[phase]; + double j = profile->j[phase]; + + // Polynomial evaluation for phase-based profiles + // Constant jerk per phase is exact for Ruckig's native phases + *jerk = j; + *acc = a0 + j * dt; + *vel = v0 + a0 * dt + 0.5 * j * dt * dt; + *pos = p0 + v0 * dt + 0.5 * a0 * dt * dt + (1.0/6.0) * j * dt * dt * dt; + + return 0; +} + +/** + * RT-safe jerk-limited emergency stop. + * + * Computes one cycle of jerk-limited deceleration to bring velocity to zero. + * Called each RT cycle when abort/pause is active during Ruckig execution. + * + * Algorithm: sqrt-profile targeting + * - Far from stop: ramp up to max deceleration + * - Close to stop: use sqrt(2*j*v) to compute smooth approach + * - Clamp jerk to limits, prevent velocity overshoot through zero + * + * Known limitations: + * - Not time-optimal (proper 3-phase profile would be better) + * - Edge cases near v=0 use heuristic clamping + * - Does not pre-compute full stopping trajectory + * + * Future improvement: Have userspace compute stopping trajectory via + * Ruckig when abort detected, hand off to RT via predictive mechanism. + */ +void tcComputeJerkLimitedStop(double v0, double a0, + double j_max, double a_max, + double dt, + double *v_out, double *a_out, + double *j_out, double *dist_out) +{ + // Handle edge cases + if (j_max <= 0.0) j_max = 1e6; + if (a_max <= 0.0) a_max = 1e6; + if (dt <= 0.0) dt = 0.001; + + // Already stopped? + if (fabs(v0) < 1e-9 && fabs(a0) < 1e-9) { + *v_out = 0.0; + *a_out = 0.0; + *j_out = 0.0; + *dist_out = 0.0; + return; + } + + // Direction of motion + double sign_v = (v0 >= 0.0) ? 1.0 : -1.0; + double v_abs = fabs(v0); + + // Velocity that can be removed in one jerk phase (ramp down accel to 0): + // From a = j*t and v = 0.5*j*t^2, we get v = 0.5*a^2/j + double v_jerk_phase = 0.5 * a_max * a_max / j_max; + + double j_cmd; + double a_target; + + if (v_abs > v_jerk_phase * 2.0) { + // Far from stop: build up to max deceleration + a_target = -sign_v * a_max; + } else { + // Close to stop: use sqrt profile for smooth approach + // Target accel that will bring us to zero smoothly + a_target = -sign_v * sqrt(2.0 * j_max * v_abs); + if (fabs(a_target) > a_max) { + a_target = -sign_v * a_max; + } + } + + // Compute jerk needed to move toward target acceleration + double a_error = a_target - a0; + double j_needed = a_error / dt; + + // Clamp jerk to limits + if (j_needed > j_max) j_needed = j_max; + if (j_needed < -j_max) j_needed = -j_max; + j_cmd = j_needed; + + // Check if we'd overshoot zero velocity this cycle + double v_predicted = v0 + a0 * dt + 0.5 * j_cmd * dt * dt; + if ((v0 > 0 && v_predicted < 0) || (v0 < 0 && v_predicted > 0)) { + // Would cross zero - compute jerk to land exactly at v=0 + // v0 + a0*dt + 0.5*j*dt^2 = 0 + // j = -2*(v0 + a0*dt) / dt^2 + j_cmd = -2.0 * (v0 + a0 * dt) / (dt * dt); + if (j_cmd > j_max) j_cmd = j_max; + if (j_cmd < -j_max) j_cmd = -j_max; + + // Recompute with clamped jerk + v_predicted = v0 + a0 * dt + 0.5 * j_cmd * dt * dt; + if ((v0 > 0 && v_predicted < 0) || (v0 < 0 && v_predicted > 0)) { + v_predicted = 0.0; + } + } + + // Integrate one cycle + double a_new = a0 + j_cmd * dt; + double v_new = v0 + a0 * dt + 0.5 * j_cmd * dt * dt; + double d_new = v0 * dt + 0.5 * a0 * dt * dt + (1.0/6.0) * j_cmd * dt * dt * dt; + + // Clamp acceleration + if (a_new > a_max) a_new = a_max; + if (a_new < -a_max) a_new = -a_max; + + // Final velocity clamp - don't reverse + if ((v0 > 0 && v_new < 0) || (v0 < 0 && v_new > 0)) { + v_new = 0.0; + a_new = 0.0; + } + + // If velocity is zero, zero acceleration too + if (fabs(v_new) < 1e-9) { + v_new = 0.0; + a_new = 0.0; + } + + *v_out = v_new; + *a_out = a_new; + *j_out = j_cmd; + *dist_out = (d_new > 0) ? d_new : 0.0; // Don't go backwards +} + diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index f8859c5f55c..cfe8cb99971 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -22,6 +22,10 @@ #include "tc_types.h" #include "tp_types.h" +#ifdef __cplusplus +extern "C" { +#endif + double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale); @@ -118,10 +122,69 @@ int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); int tcClearFlags(TC_STRUCT * const tc); +/** + * Sample Ruckig profile at given time + * + * Evaluates the pre-computed Ruckig trajectory profile at time t. + * Uses polynomial integration: p(t) = p0 + v0*dt + 0.5*a0*dt^2 + (1/6)*j*dt^3 + * + * @param profile Pointer to ruckig_profile_t + * @param t Time since segment start (seconds) + * @param pos Output: position at time t + * @param vel Output: velocity at time t + * @param acc Output: acceleration at time t + * @param jerk Output: jerk at time t + * @return 0 on success, -1 if profile invalid + */ +int ruckigProfileSample(ruckig_profile_t const * const profile, + double t, + double *pos, + double *vel, + double *acc, + double *jerk); + +/** + * Compute one cycle of jerk-limited deceleration toward velocity = 0. + * + * RT-safe jerk-limited emergency stop implementation. + * + * When abort/pause is triggered during Ruckig profile execution, we can't + * follow the pre-computed profile (it would run to segment end). Instead, + * we compute jerk-limited deceleration cycle-by-cycle in RT to stop + * smoothly along the path. + * + * Current approach: Simple sqrt-profile targeting. Computes optimal + * deceleration each cycle to bring velocity to zero while respecting + * jerk and acceleration limits. + * + * Future refinement: Could use userspace to pre-compute a + * proper stopping trajectory via Ruckig when abort is detected, then + * hand off to RT via predictive handoff mechanism. + * + * @param v0 Current velocity (signed, positive = forward) + * @param a0 Current acceleration (signed) + * @param j_max Maximum jerk (positive value) + * @param a_max Maximum acceleration (positive value) + * @param dt Time step (cycle time, typically 1ms) + * @param v_out Output: new velocity after this cycle + * @param a_out Output: new acceleration after this cycle + * @param j_out Output: jerk applied this cycle + * @param dist_out Output: distance traveled this cycle (always >= 0) + */ +void tcComputeJerkLimitedStop(double v0, double a0, + double j_max, double a_max, + double dt, + double *v_out, double *a_out, + double *j_out, double *dist_out); + /** * Clean up Ruckig planner resources in a TC_STRUCT. * Called when the trajectory segment is removed or reset. */ void tcCleanupRuckig(TC_STRUCT * const tc); +#ifdef __cplusplus +} +#endif + #endif /* TC_H */ diff --git a/src/emc/tp/tc_9d.c b/src/emc/tp/tc_9d.c new file mode 100644 index 00000000000..f5d5be73055 --- /dev/null +++ b/src/emc/tp/tc_9d.c @@ -0,0 +1,122 @@ +/******************************************************************** +* Description: tc_9d.c +* 9D trajectory component helper functions implementation +* +* Provides 9-axis specific calculations for length, acceleration, +* and velocity limits. +* +* Author: LinuxCNC community (ported from Tormach) +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +********************************************************************/ + +#include "tc_9d.h" +#include "posemath.h" +#include "rtapi_math.h" + +/** + * @brief Compute length of a 9D line segment + * + * LinuxCNC's PmLine9 has separate xyz, abc, uvw components, + * each of which is a PmCartLine. + */ +double pmLine9Length(PmLine9 const * const line9) +{ + if (!line9) return 0.0; + + double len_xyz = 0.0, len_abc = 0.0, len_uvw = 0.0; + + // Get length of each component + // Each PmCartLine has .tmag field for magnitude + len_xyz = line9->xyz.tmag; + len_abc = line9->abc.tmag; + len_uvw = line9->uvw.tmag; + + // Euclidean combination of all axes + double total_length = sqrt(len_xyz * len_xyz + + len_abc * len_abc + + len_uvw * len_uvw); + + return total_length; +} + +/** + * @brief Compute length of a 9D circle/arc segment + * + * For circular motion: + * - XYZ follows an arc (use fit or circle arc length) + * - ABC and UVW are linear motions + */ +double pmCircle9Length(PmCircle9 const * const circle9) +{ + if (!circle9) return 0.0; + + // For circles, we need the arc length in XYZ plane + // plus linear length for ABC and UVW + + double arc_length = 0.0; + + // Get arc length from circle + // PmCircle has radius and angle members directly + arc_length = fabs(circle9->xyz.radius * circle9->xyz.angle); + + // Get linear lengths for rotary and auxiliary axes + double len_abc = circle9->abc.tmag; + double len_uvw = circle9->uvw.tmag; + + // Total length is Euclidean combination + // (arc length in XYZ + linear in ABC/UVW) + double total_length = sqrt(arc_length * arc_length + + len_abc * len_abc + + len_uvw * len_uvw); + + return total_length; +} + +/** + * @brief Get tangential maximum acceleration for 9D segment + * + * This is a simplified implementation. For full 9D support, + * should consider acceleration limits in all 9 axes and + * compute tangential component based on motion direction. + */ +double tcGetTangentialMaxAccel_9D(TC_STRUCT const * const tc) +{ + if (!tc) return 0.0; + + // For now, return the configured max acceleration + // TODO: Implement proper 9D acceleration limit calculation + // considering all axis limits and motion geometry + + return tc->maxaccel; +} + +/** + * @brief Get maximum velocity considering 9D axis limits + * + * Applies velocity limits for all 9 axes based on motion direction. + */ +double tcGetMaxVel_9D(TC_STRUCT const * const tc, double max_feed_scale) +{ + if (!tc) return 0.0; + + // Start with requested velocity + double vel = tc->reqvel; + + // Apply feed override + vel *= max_feed_scale; + + // Clamp to machine velocity limit + if (tc->maxvel > 0.0 && vel > tc->maxvel) { + vel = tc->maxvel; + } + + // TODO: For full 9D support, should check individual axis + // velocity limits based on motion direction unit vector + // and apply the most restrictive limit + + return vel; +} diff --git a/src/emc/tp/tc_9d.h b/src/emc/tp/tc_9d.h new file mode 100644 index 00000000000..16a2fa398b2 --- /dev/null +++ b/src/emc/tp/tc_9d.h @@ -0,0 +1,73 @@ +/******************************************************************** +* Description: tc_9d.h +* 9D trajectory component helper functions +* +* Provides 9-axis specific calculations for length, acceleration, +* and velocity limits. +* +* Author: LinuxCNC community (ported from Tormach) +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2022-2026 All rights reserved. +* +********************************************************************/ +#ifndef TC_9D_H +#define TC_9D_H + +#include "posemath.h" +#include "tc_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Compute length of a 9D line segment + * + * Returns the Euclidean distance considering all 9 axes: + * XYZ + ABC + UVW + * + * @param line9 9D line structure + * @return Total length + */ +double pmLine9Length(PmLine9 const * const line9); + +/** + * @brief Compute length of a 9D circle/arc segment + * + * For circular motion, this includes the arc length in the XYZ plane + * plus linear motion in ABC and UVW. + * + * @param circle9 9D circle structure + * @return Total length + */ +double pmCircle9Length(PmCircle9 const * const circle9); + +/** + * @brief Get tangential maximum acceleration for 9D segment + * + * Computes the maximum acceleration in the direction of motion, + * considering all 9 axis limits and the geometry. + * + * @param tc Trajectory component + * @return Maximum tangential acceleration + */ +double tcGetTangentialMaxAccel_9D(TC_STRUCT const * const tc); + +/** + * @brief Get maximum velocity considering 9D axis limits + * + * Applies velocity limits for all 9 axes based on motion direction. + * + * @param tc Trajectory component + * @param max_feed_scale Feed override scale factor + * @return Maximum velocity + */ +double tcGetMaxVel_9D(TC_STRUCT const * const tc, double max_feed_scale); + +#ifdef __cplusplus +} +#endif + +#endif // TC_9D_H diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index a9a3e16fa62..1ef37386a16 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -18,6 +18,7 @@ #include "emcpos.h" #include "emcmotcfg.h" #include "state_tag.h" +#include "bezier9.h" #define BLEND_DIST_FRACTION 0.5 /* values for endFlag */ @@ -32,9 +33,51 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_DWELL = 5, // Zero-length dwell segment (G4) + TC_BEZIER = 6 // Cubic Bezier blend segment (Phase 4) } tc_motion_type_t; +/** + * Segment action types for inline M-code execution + * + * Industrial controllers (Siemens, Fanuc) process M-codes inline with motion. + * This enum defines action types that can be attached to segments and fired + * by RT when the segment starts, maintaining proper synchronization without + * forcing queue drain (which breaks velocity continuity in planner_type 2). + * + * Actions fire via tpFireSegmentActions() when segment becomes active. + */ +typedef enum { + SEG_ACTION_NONE = 0, + SEG_ACTION_SPINDLE_CW, // M3: Spindle on clockwise + SEG_ACTION_SPINDLE_CCW, // M4: Spindle on counter-clockwise + SEG_ACTION_SPINDLE_OFF, // M5: Spindle off + SEG_ACTION_COOLANT_MIST, // M7: Mist coolant on + SEG_ACTION_COOLANT_FLOOD, // M8: Flood coolant on + SEG_ACTION_COOLANT_OFF, // M9: All coolant off + SEG_ACTION_TOOL_CHANGE, // M6: Tool change (may need special handling) + SEG_ACTION_PROGRAM_STOP, // M0/M1: Program stop + SEG_ACTION_PROBE_START, // G38.x: Start probe move + SEG_ACTION_CUSTOM, // For future extensibility + SEG_ACTION_MAX +} segment_action_type_t; + +/** + * Segment action structure + * + * Attached to TC_STRUCT to fire actions when segment activates. + * Multiple actions can be queued by setting multiple flags. + * RT fires these via HAL pins or direct calls in tpActivateSegment(). + */ +typedef struct { + unsigned int action_mask; // Bitmask of segment_action_type_t flags + int spindle_num; // Which spindle (0-based) + double spindle_speed; // RPM for spindle on + int custom_action_id; // For SEG_ACTION_CUSTOM + double custom_value; // Parameter for custom action +} segment_actions_t; + typedef enum { TC_SYNC_NONE = 0, TC_SYNC_VELOCITY, @@ -89,6 +132,23 @@ typedef struct { PmCartesian uvw; } Arc9; +/** + * Joint-space linear segment for userspace kinematics + * Stores start/end joint positions for linear interpolation. + * Polynomial coefficients may be added later for smoother trajectories. + */ +#define JOINT_SPACE_MAX_JOINTS 9 + +typedef struct { + double start[JOINT_SPACE_MAX_JOINTS]; // Joint positions at segment start + double end[JOINT_SPACE_MAX_JOINTS]; // Joint positions at segment end + double vel_limit_start; // Max velocity at start (from Jacobian) + double vel_limit_end; // Max velocity at end + double acc_limit; // Max acceleration through segment + int num_joints; // Number of active joints + int valid; // True if joint data is valid +} JointSpaceSegment; + typedef enum { RIGIDTAP_START, TAPPING, REVERSING, RETRACTION, FINAL_REVERSAL, FINAL_PLACEMENT @@ -116,6 +176,125 @@ typedef struct { RIGIDTAP_STATE state; } PmRigidTap; +/** + * Planning state for 9D optimizer + * Tracks optimization status of segments + */ +typedef enum { + TC_PLAN_UNTOUCHED = 0, // Segment not yet optimized + TC_PLAN_OPTIMIZED = 1, // Velocity limits computed + TC_PLAN_SMOOTHED = 2, // Peak smoothing applied + TC_PLAN_FINALIZED = 3 // Ready for execution +} TCPlanningState; + +/** + * Ruckig trajectory profile for a single segment + * + * Stores the 9-phase jerk-limited profile computed by Ruckig. + * RT layer samples this at each servo cycle using polynomial evaluation. + * + * Phases 0-1: Brake pre-trajectory (brings initial state within limits) + * Zero-duration when no brake needed. + * Phases 2-8: Main 7-phase S-curve: + * accel_jerk -> accel_const -> accel_jerk -> cruise -> + * decel_jerk -> decel_const -> decel_jerk + * + * Each phase has constant jerk from Ruckig's native arrays (not derived). + */ +#define RUCKIG_PROFILE_PHASES 9 + +typedef struct { + int valid; // Profile has been computed + int locked; // DEBUG: 1 = Ruckig Working result (non-monotonic risk) + volatile int generation; // Incremented each time profile is rewritten (stopwatch reset detection) + double duration; // Total profile duration (seconds) + double computed_feed_scale; // Feed scale when profile was computed + double computed_vel_limit; // Velocity limit when profile was computed (for vLimit change detection) + double computed_vLimit; // tp->vLimit when profile was computed (absolute cap, not scaled by feed) + double computed_desired_fvel; // Desired final velocity when profile was computed (for convergence detection) + double t[RUCKIG_PROFILE_PHASES]; // Phase durations + double t_sum[RUCKIG_PROFILE_PHASES]; // Cumulative times (t_sum[i] = sum of t[0..i]) + double j[RUCKIG_PROFILE_PHASES]; // Jerk for each phase + double p[RUCKIG_PROFILE_PHASES + 1]; // Position at phase boundaries + double v[RUCKIG_PROFILE_PHASES + 1]; // Velocity at phase boundaries + double a[RUCKIG_PROFILE_PHASES + 1]; // Acceleration at phase boundaries +} ruckig_profile_t; + +/** + * Pending branch for feed override replanning + * + * Branch/Merge Architecture: + * - Userspace computes speculative branches when feed override changes + * - RT takes the branch if it reaches handoff_time before window_end_time + * - RT sets 'taken' flag when it takes the branch + * - Userspace merges (branch becomes canonical) when it sees taken=1 + * - If RT passes window_end_time without taking, branch is discarded + * + * Invariants: + * - RT never waits - old plan always valid + * - Only userspace sets 'valid', only RT sets 'taken' + * - Single branch slot per segment (simplicity) + */ +typedef struct { + ruckig_profile_t profile; // The main trajectory (position control) + ruckig_profile_t brake_profile; // Optional brake trajectory (velocity control) + int has_brake; // 1 if brake_profile should execute first + double brake_end_position; // Position where brake ends, main begins + double handoff_time; // When RT should take the branch (elapsed_time) + double handoff_position; // Position at handoff (for merge reconciliation) + double feed_scale; // Feed scale this branch was computed for + double window_end_time; // Deadline - if RT past this, branch is stale + volatile int valid; // Userspace sets: branch is ready + volatile int taken; // RT sets: I took this branch + volatile int brake_done; // RT sets: brake phase complete, now on main +} pending_branch_t; + +/** + * Shared optimization data for 9D planner + * + * This structure holds data that is shared between the userspace + * planning layer and the RT execution layer using atomic operations. + * + * Used only when planner_type == 2 (9D planner) + */ +typedef struct { + TCPlanningState optimization_state; // Atomic state flag + double final_vel; // Target exit velocity + double final_vel_limit; // Max reachable exit velocity + double computed_acc; // Computed acceleration limit + double entry_vel; // Computed entry velocity + ruckig_profile_t profile; // Active profile being sampled by RT + + // Branch/Merge architecture for feed override handling + pending_branch_t branch; // Single pending branch slot + double canonical_feed_scale; // Feed scale of current canonical plan + + // Sequence counter for torn read detection during profile copy + // RT increments before copy (odd = in progress), after copy (even = complete) + // Userspace checks: if odd or changed, retry read + volatile unsigned int copy_sequence; + + // Achievable feed cascade + // When segment is too short to achieve requested feed, we apply what's + // achievable and pass the remainder to the next segment. + double requested_feed_scale; // What user requested (may not be achievable) + double achieved_exit_vel; // Actual exit velocity we can achieve + // Phase 4 TODO: With blending, achieved_exit_vel becomes the entry velocity + // constraint for the next segment, enabling smooth velocity handoff. + + // Alt-entry profile for smooth brake junction transitions. + // When a brake branch on seg N changes its exit velocity, + // userspace pre-computes an alternate profile for seg N+1 + // with v0 matching the brake's exit velocity. + // RT picks whichever (main or alt_entry) has v0 closer to + // actual junction velocity — smooth either way. + struct { + ruckig_profile_t profile; // Alternate profile with adjusted entry velocity + double v0; // Entry velocity this profile was computed for + volatile int valid; // Userspace sets: alt-entry ready for RT + } alt_entry; +} shared_optimization_data_9d_t; + typedef struct { double cycle_time; //Position stuff @@ -123,33 +302,35 @@ typedef struct { double progress; // where are we in the segment? 0..target double nominal_length; - //Velocity - double reqvel; // vel requested by F word, calc'd by task - double target_vel; // velocity to actually track, limited by other factors - double maxvel; // max possible vel (feed override stops here) - double currentvel; // keep track of current step (vel * cycle_time) + // === Velocity fields === + // All planner types (0=trapezoidal, 1=S-curve, 2=9D Ruckig) + double reqvel; // F-word request velocity, set by task (all planners) + double target_vel; // velocity to track; planner 0/1: from optimizer; planner 2: from shared_9d.final_vel + double maxvel; // absolute velocity cap (feed override cannot exceed this) + double currentvel; // RT execution: current velocity (updated each servo cycle) double last_move_length;// last move length - double finalvel; // velocity to aim for at end of segment - double term_vel; // actual velocity at termination of segment - double kink_vel; // Temporary way to store our calculation of maximum velocity we can handle if this segment is declared tangent with the next - double kink_accel_reduce_prev; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. - double kink_accel_reduce; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. + double finalvel; // planner 0/1: rising tide backward pass; planner 2: backward pass cap (kink-limited) + double term_vel; // planner 0 only: actual velocity at segment termination (trapezoidal split handoff) + double kink_vel; // junction velocity limit from Jacobian projection (all planners) + double kink_accel_reduce_prev; // accel reduction at approximate tangent intersection (predecessor) + double kink_accel_reduce; // accel reduction at approximate tangent intersection (this segment) - double factor; + // === Jerk fields === + double maxjerk; // max jerk limit (all planners: S-curve native, planner 2 passed to Ruckig) + double currentjerk; // planner 1/2: current jerk (S-curve native, Ruckig for status reporting) + double currentacc; // planner 1/2: current acceleration (S-curve native, Ruckig + split inheritance) - double targetvel; - double vt; + // === Acceleration fields === + double maxaccel; // max acceleration limit, set by task (all planners) + double acc_ratio_tan; // ratio between normal and tangential accel - //Jerk - double maxjerk; // max jerk for S-curve motion - double blend_maxjerk; // max jerk during blend (set by look-ahead) - double currentjerk; // current jerk for S-curve planning - double currentacc; // current acceleration for S-curve planning - double lastacc; + // === Execution state === + double elapsed_time; // time elapsed since segment activation (all planners) - //Acceleration - double maxaccel; // accel calc'd by task - double acc_ratio_tan;// ratio between normal and tangential accel + // Execution state for predictive handoff (owned by RT, atomically readable from userspace) + double position_base; // Accumulated offset from profile swaps + int last_profile_generation; // Last profile generation seen by RT (stopwatch reset detection) + volatile int active_segment_id; // Unique ID, atomically updated by RT int id; // segment's serial number struct state_tag_t tag; // state tag corresponding to running motion @@ -159,8 +340,12 @@ typedef struct { PmCircle9 circle; PmRigidTap rigidtap; Arc9 arc; + Bezier9 bezier; // Phase 4: quintic Bezier blend curve (G2 continuity) } coords; + // Joint-space segment data (when using userspace kinematics) + JointSpaceSegment joint_space; + int motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or // TC_RIGIDTAP (coords.rigidtap) @@ -170,19 +355,32 @@ typedef struct { // this segment (g64 mode) int blending_next; // segment is being blended into following segment - double blend_vel; // velocity below which we should start blending + double blend_vel; // planner 0/1: velocity to start blending; planner 2: backward pass exit cap double tolerance; // during the blend at the end of this move, // stay within this distance from the path. int synchronized; // spindle sync state double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi) - double vel_at_blend_start; + double vel_at_blend_start; // planner 0/1 only: velocity when blend phase starts int sync_accel; // we're accelerating up to sync with the spindle unsigned char enables; // Feed scale, etc, enable bits for this move int atspeed; // wait for the spindle to be at-speed before starting this move syncdio_t syncdio; // synched DIO's for this move. what to turn on/off + + // Inline segment actions (planner_type 2) + // Fire when segment activates, without forcing queue drain + segment_actions_t actions; + + // Dwell support (TC_DWELL segments) + double dwell_time; // Total dwell duration in seconds + double dwell_remaining; // Countdown timer (decremented by RT each cycle) int indexer_jnum; // which joint to unlock (for a locking indexer) to make this move, -1 for none int optimization_state; // At peak velocity during blends) int on_final_decel; + + // 9D planner shared optimization data (planner_type == 2 only) + // Shared between userspace planning layer and RT execution layer + // Access via atomic operations only + shared_optimization_data_9d_t shared_9d; int blend_prev; int accel_mode; int splitting; // the segment is less than 1 cycle time diff --git a/src/emc/tp/tcq.c b/src/emc/tp/tcq.c index ce2a711c9e3..6c7ab7b1827 100644 --- a/src/emc/tp/tcq.c +++ b/src/emc/tp/tcq.c @@ -19,14 +19,29 @@ #include "tcq.h" #include +#include "motion.h" +#include "atomic_9d.h" +#include "rtapi.h" + +/* Access to global emcmotStatus for planner_type checking */ +extern emcmot_status_t *emcmotStatus; + +#ifndef GET_TRAJ_PLANNER_TYPE +#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) +#endif + +/* Queue margin constants */ +#define TCQ_REVERSE_MARGIN 200 +#define TC_QUEUE_MARGIN (TCQ_REVERSE_MARGIN+20) /** Return 0 if queue is valid, -1 if not */ static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq) { - if ((0 == tcq) || (0 == tcq->queue)) + if (0 == tcq) { return -1; } + /* queue is embedded in struct, no need to check for NULL */ return 0; } @@ -34,21 +49,21 @@ static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq) * * \brief Creates a new queue for TC elements. * - * This function creates a new queue for TC elements. + * This function initializes a queue for TC elements. + * The queue array is embedded in the TC_QUEUE_STRUCT. * It gets called by tpCreate() * - * @param tcq pointer to the new TC_QUEUE_STRUCT - * @param _size size of the new queue - * @param tcSpace holds the space allocated for the new queue, allocated in motion.c + * @param tcq pointer to the TC_QUEUE_STRUCT + * @param _size size of the queue * * @return int returns success or failure */ -int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace) +int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size) { - if (!tcq || !tcSpace || _size < 1) { + if (!tcq || _size < 1) { return -1; } - tcq->queue = tcSpace; + /* queue array is now embedded in struct, no pointer assignment needed */ tcq->size = _size; tcqInit(tcq); @@ -70,11 +85,8 @@ int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace) */ int tcqDelete(TC_QUEUE_STRUCT * const tcq) { - if (!tcqCheck(tcq)) { - /* free(tcq->queue); */ - tcq->queue = 0; - } - + /* Queue is embedded in struct, nothing to free or clear */ + (void)tcq; /* suppress unused parameter warning */ return 0; } @@ -100,6 +112,10 @@ int tcqInit(TC_QUEUE_STRUCT * const tcq) tcq->_rlen = 0; tcq->allFull = 0; + /* Initialize atomic indices for planner_type 2 */ + __atomic_store_n(&tcq->start_atomic, 0, __ATOMIC_SEQ_CST); + __atomic_store_n(&tcq->end_atomic, 0, __ATOMIC_SEQ_CST); + return 0; } @@ -120,6 +136,32 @@ int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc) /* check for initialized */ if (tcqCheck(tcq)) return -1; + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load current indices atomically */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Calculate next end index */ + int next_end = (current_end + 1) % tcq->size; + + /* Check if queue is full (with margin for reverse history) */ + int available_space = (current_start - next_end + tcq->size) % tcq->size; + if (available_space < TCQ_REVERSE_MARGIN + 20) { + return -1; /* Queue full */ + } + + /* Copy TC element to queue */ + tcq->queue[current_end] = *tc; + + /* Atomically update end index (producer) - use exchange for stronger visibility */ + __atomic_exchange_n(&tcq->end_atomic, next_end, __ATOMIC_ACQ_REL); + + return 0; + } + + /* Original mutex-based implementation for planner_type 0/1 */ + /* check for allFull, so we don't overflow the queue */ if (tcq->allFull) { return -1; @@ -166,8 +208,6 @@ int tcqPopBack(TC_QUEUE_STRUCT * const tcq) return 0; } -#define TCQ_REVERSE_MARGIN 200 - int tcqPop(TC_QUEUE_STRUCT * const tcq) { @@ -175,7 +215,30 @@ int tcqPop(TC_QUEUE_STRUCT * const tcq) return -1; } - if (tcq->_len < 1 && !tcq->allFull) { + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load indices atomically */ + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + + /* Check if queue is empty */ + if (current_start == current_end) { + return -1; + } + + /* Atomically increment start index (RT consumer) - use exchange for stronger visibility */ + int new_start = (current_start + 1) % tcq->size; + __atomic_exchange_n(&tcq->start_atomic, new_start, __ATOMIC_ACQ_REL); + + /* Note: For planner_type 2, reverse history is managed differently + * by the userspace planning layer, so we don't update rend/rlen here */ + + return 0; + } + + /* Original implementation for planner_type 0/1 */ + + if (tcq->_len < 1 && !tcq->allFull) { return -1; } @@ -268,6 +331,18 @@ int tcqLen(TC_QUEUE_STRUCT const * const tcq) { if (tcqCheck(tcq)) return -1; + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Read producer index first (end), then consumer index (start) */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Calculate length with circular buffer wraparound */ + int len = (current_end - current_start + tcq->size) % tcq->size; + return len; + } + + /* Original implementation for planner_type 0/1 */ return tcq->_len; } @@ -281,19 +356,29 @@ int tcqLen(TC_QUEUE_STRUCT const * const tcq) * * @return TC_STRUCT returns the TC elements */ -TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n) +TC_STRUCT * tcqItem(TC_QUEUE_STRUCT * const tcq, int n) { - if (tcqCheck(tcq) || (n < 0) || (n >= tcq->_len)) return NULL; + if (tcqCheck(tcq) || (n < 0)) return NULL; + + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load indices atomically */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Calculate length and check bounds */ + int len = (current_end - current_start + tcq->size) % tcq->size; + if (n >= len) return NULL; + + /* Return item at offset n from start */ + return &(tcq->queue[(current_start + n) % tcq->size]); + } + /* Original implementation for planner_type 0/1 */ + if (n >= tcq->_len) return NULL; return &(tcq->queue[(tcq->start + n) % tcq->size]); } -/*! - * \def TC_QUEUE_MARGIN - * sets up a margin at the end of the queue, to reduce effects of race conditions - */ -#define TC_QUEUE_MARGIN (TCQ_REVERSE_MARGIN+20) - /*! tcqFull() function * * \brief get the full status of the queue @@ -311,6 +396,32 @@ int tcqFull(TC_QUEUE_STRUCT const * const tcq) return 1; /* null queue is full, for safety */ } + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load indices atomically */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Calculate length */ + int len = (current_end - current_start + tcq->size) % tcq->size; + + /* Check if queue is into the margin */ + if (tcq->size <= TC_QUEUE_MARGIN) { + /* No margin available, so full means really all full */ + return (len >= tcq->size - 1); + } + + if (len >= tcq->size - TC_QUEUE_MARGIN) { + /* We're into the margin, so call it full */ + return 1; + } + + /* We're not into the margin */ + return 0; + } + + /* Original implementation for planner_type 0/1 */ + /* call the queue full if the length is into the margin, so reduce the effect of a race condition where the appending process may not see the full status immediately and send another motion */ @@ -338,11 +449,29 @@ int tcqFull(TC_QUEUE_STRUCT const * const tcq) * * @return TC_STRUCT returns the TC element */ -TC_STRUCT *tcqLast(TC_QUEUE_STRUCT const * const tcq) +TC_STRUCT *tcqLast(TC_QUEUE_STRUCT * const tcq) { if (tcqCheck(tcq)) { return NULL; } + + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load indices atomically */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Check if queue is empty */ + if (current_end == current_start) { + return NULL; + } + + /* Get last element (end - 1), fix for negative modulus error */ + int n = current_end - 1 + tcq->size; + return &(tcq->queue[n % tcq->size]); + } + + /* Original implementation for planner_type 0/1 */ if (tcq->_len == 0) { return NULL; } @@ -352,3 +481,80 @@ TC_STRUCT *tcqLast(TC_QUEUE_STRUCT const * const tcq) } +/*! tcqBack() function + * + * \brief gets the n-th item from the back of the queue + * + * This function allows backward iteration from the end of the queue. + * n=0 is the most recently added item (same as tcqLast) + * n=-1 is the second-to-last item + * n=-2 is the third-to-last item, etc. + * + * Used by the backward velocity pass optimization in planner_type 2. + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * @param n negative offset from end (0, -1, -2, ...) + * + * @return TC_STRUCT returns the TC element, or NULL if invalid + */ +TC_STRUCT * tcqBack(TC_QUEUE_STRUCT * const tcq, int n) +{ + if (tcqCheck(tcq)) { + return NULL; + } + + /* For planner_type 2, use atomic lock-free queue */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + /* Load producer index first (end), then consumer index (start) */ + int current_end = __atomic_load_n(&tcq->end_atomic, __ATOMIC_ACQUIRE); + int current_start = __atomic_load_n(&tcq->start_atomic, __ATOMIC_ACQUIRE); + + /* Calculate length */ + int len = (current_end - current_start + tcq->size) % tcq->size; + + /* Check for empty queue */ + if (0 == len) { + return NULL; + } + /* Only allow negative indices (from back) */ + else if (n > 0) { + return NULL; + } + /* Check if index is within valid range */ + else if (n > -len) { + /* Calculate index from end: end + n - 1 + * Fix for negative modulus error by adding tcq->size */ + int k = current_end + n - 1 + tcq->size; + int idx = k % tcq->size; + return &(tcq->queue[idx]); + } + else { + return NULL; + } + } + + /* Original implementation for planner_type 0/1 */ + /* Uses same logic as atomic version but with _len and end */ + if (0 == tcq->_len) { + return NULL; + } else if (n > 0) { + return NULL; + } else if (n > -tcq->_len) { + int k = tcq->end + n - 1 + tcq->size; + int idx = k % tcq->size; + return &(tcq->queue[idx]); + } else { + return NULL; + } +} + +/*! tcqBackConst() function + * + * \brief const-correct version of tcqBack + */ +TC_STRUCT const * tcqBackConst(TC_QUEUE_STRUCT const * const tcq, int n) +{ + /* Cast away const for the pointer, not the data */ + return (TC_STRUCT const *)tcqBack((TC_QUEUE_STRUCT *)tcq, n); +} + diff --git a/src/emc/tp/tcq.h b/src/emc/tp/tcq.h index a7e137c791e..4a98030fd39 100644 --- a/src/emc/tp/tcq.h +++ b/src/emc/tp/tcq.h @@ -23,21 +23,29 @@ #include "tc_types.h" +#ifdef __cplusplus +extern "C" { +#endif + typedef struct { - TC_STRUCT *queue; /* ptr to the tcs */ + TC_STRUCT queue[DEFAULT_TC_QUEUE_SIZE + 10]; /* embedded TC array */ int size; /* size of queue */ int _len; /* number of tcs now in queue */ int _rlen; /* number of tcs now in reverse history */ int start, end; /* indices to next to get, next to put */ int rend; int allFull; /* flag meaning it's actually full */ + + /* For planner_type 2 (9D) - atomic lock-free queue */ + /* These are used instead of mutex when planner_type == 2 */ + int start_atomic; /* RT consumer index (atomically updated) */ + int end_atomic; /* Userspace producer index (atomically updated) */ } TC_QUEUE_STRUCT; /* TC_QUEUE_STRUCT functions */ /* create queue of _size */ -extern int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, - TC_STRUCT * const tcSpace); +extern int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size); /* free up queue */ extern int tcqDelete(TC_QUEUE_STRUCT * const tcq); @@ -62,14 +70,27 @@ extern int tcqBackStep(TC_QUEUE_STRUCT * const tcq); extern int tcqLen(TC_QUEUE_STRUCT const * const tcq); /* look at nth item, first is 0 */ -extern TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n); +extern TC_STRUCT * tcqItem(TC_QUEUE_STRUCT * const tcq, int n); /** * Get the "end" of the queue, the most recently added item. */ -extern TC_STRUCT * tcqLast(TC_QUEUE_STRUCT const * const tcq); +extern TC_STRUCT * tcqLast(TC_QUEUE_STRUCT * const tcq); + +/** + * Get item from the back of the queue. + * n=0 is the most recently added item (same as tcqLast) + * n=-1 is the second-to-last item, etc. + * Used by backward velocity pass optimization. + */ +extern TC_STRUCT * tcqBack(TC_QUEUE_STRUCT * const tcq, int n); +extern TC_STRUCT const * tcqBackConst(TC_QUEUE_STRUCT const * const tcq, int n); /* get full status */ extern int tcqFull(TC_QUEUE_STRUCT const * const tcq); +#ifdef __cplusplus +} +#endif + #endif diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index ece2681e34e..55585fe4e0b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -22,6 +22,12 @@ #include "spherical_arc.h" #include "blendmath.h" #include "axis.h" + +/* Stub out EXPORT_SYMBOL for userspace builds where rtapi.h doesn't define it */ +#ifndef EXPORT_SYMBOL +#define EXPORT_SYMBOL(x) +#endif + //KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND //definitions until we can break the emc constants out into a separate file. //#include "emc.hh" @@ -41,7 +47,14 @@ #include "tp_debug.h" #include "sp_scurve.h" #include "ruckig_wrapper.h" +#include "atomic_9d.h" // Atomic operations for 9D planner #include + +// Forward declaration for 9D planner userspace function +// (Actual implementation is in motion_planning_9d.cc, a C++ file) +// tp.c is compiled as C code, so we can't include C++ headers - just declare the C interface +extern int tpClearPlanning_9D(TP_STRUCT * const tp); + // FIXME: turn off this feature, which causes blends between rapids to // use the feed override instead of the rapid override #undef TP_SHOW_BLENDS @@ -58,10 +71,23 @@ #include "hal.h" #endif // } +/* When building for userspace library, these are extern (defined in milltask) + * When building for RT module, these are defined here and initialized via tpMotData() */ +#ifdef USERSPACE_LIB_BUILD +extern emcmot_status_t *emcmotStatus; +extern emcmot_config_t *emcmotConfig; +extern emcmot_command_t *emcmotCommand; +extern emcmot_hal_data_t *emcmot_hal_data; +extern struct emcmot_struct_t *emcmotStruct; +extern struct emcmot_internal_t *emcmotInternal; +#else emcmot_status_t *emcmotStatus; emcmot_config_t *emcmotConfig; emcmot_command_t *emcmotCommand; emcmot_hal_data_t *emcmot_hal_data; +struct emcmot_struct_t *emcmotStruct; +struct emcmot_internal_t *emcmotInternal; +#endif #ifndef GET_TRAJ_PLANNER_TYPE #define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) @@ -70,7 +96,20 @@ emcmot_hal_data_t *emcmot_hal_data; #endif -#define GET_TRAJ_HOME_USE_TP() (emcmotStatus->home_use_tp) +/** + * Keeps track of time required to drain motion smoothing filters after TP + * has reached zero velocity. Commanded motion is not actually stopped until + * the TP and any time-delayed smoothing is done. + * + * Stub: Always returns true (no joint filters yet). + * Future: return tp->filters_at_rest; + */ +static bool checkJointFiltersEmpty(TP_STRUCT * const tp) +{ + (void)tp; // Will be used for tp->filters_at_rest when joint filters implemented + // No joint filters yet, always return true + return true; +} //========================================================== // tp module interface @@ -100,10 +139,14 @@ void tpMotFunctions(void( *pDioWrite)(int,char) void tpMotData(emcmot_status_t *pstatus ,emcmot_config_t *pconfig + ,struct emcmot_struct_t *pstruct + ,struct emcmot_internal_t *pinternal ) { emcmotStatus = pstatus; emcmotConfig = pconfig; + emcmotStruct = pstruct; + emcmotInternal = pinternal; } //========================================================= @@ -142,6 +185,11 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); +STATIC void tpFireSegmentActions(TC_STRUCT * const tc); + +/* Forward declaration - defined later, needed by tpAddSegmentToQueue */ +int tpClearSegmentActions(TP_STRUCT * const tp); + /** * @section tpcheck Internal state check functions. * These functions compartmentalize some of the messy state checks. @@ -155,6 +203,7 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { switch (tc->motion_type) { //Note lack of break statements due to every path returning case TC_RIGIDTAP: + case TC_DWELL: // Dwell has no motion return false; case TC_LINEAR: if (tc->coords.line.abc.tmag_zero && tc->coords.line.uvw.tmag_zero) { @@ -169,6 +218,7 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { return true; } case TC_SPHERICAL: + case TC_BEZIER: return true; default: tp_debug_print("Unknown motion type!\n"); @@ -372,8 +422,9 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp __attribute__(( else { a_scale *= 8.0/15.0; } - if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { - //Limit acceleration for cirular arcs to allow for normal acceleration + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL || + tc->motion_type == TC_BEZIER) { + //Limit acceleration for curved segments to allow for normal acceleration a_scale *= tc->acc_ratio_tan; } return a_scale; @@ -397,9 +448,7 @@ STATIC inline double tpGetSignedSpindlePosition(spindle_status_t *status) { * @section tpaccess tp class-like API */ -/* space for trajectory planner queues, plus 10 more for safety */ -/*! \todo FIXME-- default is used; dynamic is not honored */ - TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10]; +/* Queue is now embedded in TC_QUEUE_STRUCT, no static allocation needed */ /** * Create the trajectory planner structure with an empty queue. @@ -452,10 +501,9 @@ int tpCreate(TP_STRUCT * const tp, int _queueSize,int id) } else { tp->queueSize = _queueSize; } - TC_STRUCT * const tcSpace = queueTcSpace; - /* create the queue */ - if (-1 == tcqCreate(&tp->queue, tp->queueSize, tcSpace)) { + /* create the queue (queue array is embedded in struct) */ + if (-1 == tcqCreate(&tp->queue, tp->queueSize)) { return TP_ERR_FAIL; } @@ -510,10 +558,12 @@ int tpClear(TP_STRUCT * const tp) struct state_tag_t tag = {}; tp->execTag = tag; tp->motionType = 0; - tp->done = 1; + tp->joint_filter_drain_counter = 0; + tp->filters_at_rest = true; tp->depth = tp->activeDepth = 0; tp->aborting = 0; tp->pausing = 0; + tp->abort_profiles_written = 0; tp->reverse_run = 0; tp->synchronized = 0; tp->uu_per_rev = 0.0; @@ -525,6 +575,14 @@ int tpClear(TP_STRUCT * const tp) // equivalent to: SET_MOTION_INPOS_FLAG(1): emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; + // Clear userspace planning state for 9D planner + // (Only available in userspace build - RT kernel can't call userspace functions) +#ifdef USERSPACE_LIB_BUILD + if (GET_TRAJ_PLANNER_TYPE() == 2) { + tpClearPlanning_9D(tp); + } +#endif + return tpClearDIOs(tp); } @@ -569,7 +627,14 @@ int tpInit(TP_STRUCT * const tp) tpGetMachineVelBounds(&vel_bound); tpGetMachineActiveLimit(&tp->vMax, &vel_bound); - return tpClear(tp); + // Initialize queue and set safety markers for 9D planner + int result = tpClear(tp); + if (result == 0) { + // Set initialization markers (safety validation for 9D planner) + tp->magic = TP_MAGIC; + tp->queue_ready = 1; + } + return result; } /** @@ -739,6 +804,68 @@ int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos) } +/** + * Sync goalPos and clear abort state for 9D planner at mode entry. + * Called at COORD mode entry (EMCMOT_COORD command) to ensure: + * 1. goalPos is valid before userspace adds new segments + * 2. aborting flag is cleared so new segments don't get wiped + * + * This is critical for program re-runs where we're already in COORD mode + * and the control.c tpSetPos() path won't be taken. + * + * Follows Tormach's tpResetAtModeChange() pattern where state is cleaned up + * when entering coordinated mode, not when queue empties. + */ +int tpSyncGoalPos_9D(TP_STRUCT * const tp, EmcPose const * const pos) +{ + if (!tp || GET_TRAJ_PLANNER_TYPE() != 2) { + return TP_ERR_OK; + } + + tp->goalPos = *pos; + + /* CRITICAL: Clear aborting flag before new segments arrive. + * Without this, tpHandleAbort() will wipe the queue immediately + * because tc->currentvel == 0 for segments that haven't started yet. + * This follows Tormach's tpCleanupAfterAbort() pattern. + */ + tp->aborting = 0; + tp->abort_profiles_written = 0; + + return TP_ERR_OK; +} + +/** + * Clean up abort state for 9D planner. + * This function unconditionally clears the aborting flag and related state. + * Called after mode entry and after abort completion to ensure clean state. + * Follows Tormach's tpCleanupAfterAbort() pattern. + */ +int tpCleanupAfterAbort_9D(TP_STRUCT * const tp) +{ + if (!tp) { + return -1; + } + + // Unconditionally clear the aborting flag + tp->aborting = 0; + tp->abort_profiles_written = 0; + + // CRITICAL FIX: Clear pausing flag set by tpAbort()->tpPause() + // If pausing=1, RT won't execute new segments after restart + tp->pausing = 0; + + // Clear reverse run state (all history discarded at abort/stop) + tp->reverse_run = 0; + + // Debug logging + rtapi_print_msg(RTAPI_MSG_DBG, + "9D: tpCleanupAfterAbort_9D - cleared aborting, pausing, and reverse_run\n"); + + return 0; +} + + /** * Set current position. * It sets the current position AND the goal position to be the same. Used @@ -1594,6 +1721,15 @@ STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pr STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id) { tc->id = tp->nextId; + + // Attach any pending segment actions (for planner_type 2 inline M-codes) + if (tp->pending_actions.action_mask != 0) { + tc->actions = tp->pending_actions; + tpClearSegmentActions(tp); + tp_debug_print("Attached pending actions (mask=0x%x) to segment %d\n", + tc->actions.action_mask, tc->id); + } + if (tcqPut(&tp->queue, tc) == -1) { rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n"); return TP_ERR_FAIL; @@ -1607,9 +1743,7 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc if (tc->motion_type != TC_RIGIDTAP) { tcGetEndpoint(tc, &tp->goalPos); } - tp->done = 0; tp->depth = tcqLen(&tp->queue); - //Fixing issue with duplicate id's? tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target); return TP_ERR_OK; @@ -1715,7 +1849,12 @@ int tpAddRigidTap(TP_STRUCT * const tp, tcFinalizeLength(prev_tc); tcFlagEarlyStop(prev_tc, &tc); int retval = tpAddSegmentToQueue(tp, &tc, true); - tpRunOptimization(tp); + // Planner 2 uses its own jerk-aware backward pass in userspace + // (computeLimitingVelocities_9D), so the RT trapezoidal backward pass + // is redundant — its values are overwritten before RT needs them. + if (GET_TRAJ_PLANNER_TYPE() != 2) { + tpRunOptimization(tp); + } return retval; } @@ -2049,20 +2188,159 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, // TODO: this should ideally depend on some function of segment length and acceleration for better optimization const double kink_ratio = tpGetTangentKinkRatio(); + double kink_vel; + double accel_reduction; + int result; + if (acc_scale_max < kink_ratio) { tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); - tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max); - return TP_ERR_OK; + kink_vel = v_max; + accel_reduction = acc_scale_max; + result = TP_ERR_OK; } else { - tcSetKinkProperties(prev_tc, tc, v_max * kink_ratio / acc_scale_max, kink_ratio); + kink_vel = v_max * kink_ratio / acc_scale_max; + accel_reduction = kink_ratio; tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n", acc_scale_max, kink_ratio, - prev_tc->kink_vel); - // NOTE: acceleration will be reduced later if tangent blend is used - return TP_ERR_NO_ACTION; + kink_vel); + result = TP_ERR_NO_ACTION; + } + + // Centripetal acceleration discontinuity limit. + // At arc-to-arc (or arc-to-line / line-to-arc) junctions, the centripetal + // acceleration direction changes instantaneously. The per-axis jerk from + // this is v^2 * |delta_curvature_j| / dt. Limit junction velocity so that + // this stays within maxjerk for each axis. + double v_cent_max = -1.0; // -1 means N/A + if (prev_tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_CIRCULAR) { + PmCartesian curv_prev = {0, 0, 0}; + PmCartesian curv_tc = {0, 0, 0}; + + if (prev_tc->motion_type == TC_CIRCULAR) { + PmCartesian endpoint; + pmCirclePoint(&prev_tc->coords.circle.xyz, + prev_tc->coords.circle.xyz.angle, &endpoint); + pmCartCartSub(&prev_tc->coords.circle.xyz.center, + &endpoint, &curv_prev); + double R = prev_tc->coords.circle.xyz.radius; + pmCartScalMultEq(&curv_prev, 1.0 / (R * R)); + } + + if (tc->motion_type == TC_CIRCULAR) { + PmCartesian startpoint; + pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint); + pmCartCartSub(&tc->coords.circle.xyz.center, + &startpoint, &curv_tc); + double R = tc->coords.circle.xyz.radius; + pmCartScalMultEq(&curv_tc, 1.0 / (R * R)); + } + + PmCartesian delta_curv; + pmCartCartSub(&curv_tc, &curv_prev, &delta_curv); + + double maxjerk = fmin(prev_tc->maxjerk, tc->maxjerk); + double dt = tp->cycleTime; + v_cent_max = 1e12; + + if (fabs(delta_curv.x) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(delta_curv.x)); + if (v_lim < v_cent_max) v_cent_max = v_lim; + } + if (fabs(delta_curv.y) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(delta_curv.y)); + if (v_lim < v_cent_max) v_cent_max = v_lim; + } + if (fabs(delta_curv.z) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(delta_curv.z)); + if (v_lim < v_cent_max) v_cent_max = v_lim; + } + + tp_debug_print("centripetal limit: delta_curv=(%g,%g,%g) v_cent_max=%f kink_vel_before=%f\n", + delta_curv.x, delta_curv.y, delta_curv.z, v_cent_max, kink_vel); + + if (v_cent_max < kink_vel) { + kink_vel = v_cent_max; + } + } + + // Virtual arc centripetal limit for line-line junctions. + // Consecutive small line segments approximate a smooth curve. Compute the + // discrete curvature from three points (start of prev, junction, end of tc) + // and apply the same per-axis jerk limit. Lines have zero curvature, so + // delta_curvature at the junction equals the virtual arc curvature itself. + // For G61 (Exact Path) mode, this also enables TANGENT promotion so the + // machine can traverse small-line approximations without stopping at every + // junction. + if (prev_tc->motion_type == TC_LINEAR && tc->motion_type == TC_LINEAR) { + PmCartesian P0 = prev_tc->coords.line.xyz.start; + PmCartesian P1 = prev_tc->coords.line.xyz.end; // junction point + PmCartesian P2 = tc->coords.line.xyz.end; + + PmCartesian A, B; + pmCartCartSub(&P1, &P0, &A); // prev_tc direction + pmCartCartSub(&P2, &P1, &B); // tc direction + + double A_mag, B_mag; + pmCartMag(&A, &A_mag); + pmCartMag(&B, &B_mag); + + if (A_mag > 1e-12 && B_mag > 1e-12) { + // Unit tangent vectors + PmCartesian t_in, t_out, delta_t; + pmCartScalMult(&A, 1.0 / A_mag, &t_in); + pmCartScalMult(&B, 1.0 / B_mag, &t_out); + pmCartCartSub(&t_out, &t_in, &delta_t); + + // Discrete curvature vector: kappa = 2*(t_out - t_in) / (|A| + |B|) + // This equals the curvature of the circumscribed circle through P0, P1, P2. + double L_sum = A_mag + B_mag; + PmCartesian curv_vec; + pmCartScalMult(&delta_t, 2.0 / L_sum, &curv_vec); + + double maxjerk = fmin(prev_tc->maxjerk, tc->maxjerk); + double dt = tp->cycleTime; + double v_line_cent = 1e12; + + if (fabs(curv_vec.x) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(curv_vec.x)); + if (v_lim < v_line_cent) v_line_cent = v_lim; + } + if (fabs(curv_vec.y) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(curv_vec.y)); + if (v_lim < v_line_cent) v_line_cent = v_lim; + } + if (fabs(curv_vec.z) > 1e-12) { + double v_lim = sqrt(maxjerk * dt / fabs(curv_vec.z)); + if (v_lim < v_line_cent) v_line_cent = v_lim; + } + + tp_debug_print("line-line virtual arc: curv=(%g,%g,%g) v_line_cent=%f kink_vel_before=%f\n", + curv_vec.x, curv_vec.y, curv_vec.z, v_line_cent, kink_vel); + + if (v_line_cent < kink_vel) { + kink_vel = v_line_cent; + } + if (v_cent_max < 0) { + v_cent_max = v_line_cent; + } + + // For G61 (Exact Path): promote to TANGENT with the bounded kink_vel. + // Blend arcs are blocked for EXACT segments, so without promotion the + // junction would be forced to v=0. The virtual arc kink_vel ensures + // the centripetal jerk stays within limits. + if (result == TP_ERR_NO_ACTION && + prev_tc->term_cond == TC_TERM_COND_EXACT) { + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + tp_debug_print("G61 line-line: promoting to TANGENT, kink_vel=%f\n", kink_vel); + result = TP_ERR_OK; + } + } } + + tcSetKinkProperties(prev_tc, tc, kink_vel, accel_reduction); + return result; } static bool tpCreateBlendIfPossible( @@ -2222,8 +2500,9 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, tcFlagEarlyStop(prev_tc, &tc); int retval = tpAddSegmentToQueue(tp, &tc, true); - //Run speed optimization (will abort safely if there are no tangent segments) - tpRunOptimization(tp); + if (GET_TRAJ_PLANNER_TYPE() != 2) { + tpRunOptimization(tp); + } return retval; } @@ -2318,8 +2597,9 @@ int tpAddCircle(TP_STRUCT * const tp, tcFlagEarlyStop(prev_tc, &tc); int retval = tpAddSegmentToQueue(tp, &tc, true); - - tpRunOptimization(tp); + if (GET_TRAJ_PLANNER_TYPE() != 2) { + tpRunOptimization(tp); + } return retval; } @@ -2565,57 +2845,75 @@ STATIC double estimateParabolicBlendPerformance( */ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run) { - // If the resulting velocity is less than zero, than we're done. This - // causes a small overshoot, but in practice it is very small. - //double v_next = tc->currentvel + acc * tc->cycle_time; - double v_next; int planner_type = GET_TRAJ_PLANNER_TYPE(); - if(planner_type == 1) planner_type = 0; // if is 1, and inside here. it's means the jerk less than 1 + if(planner_type == 1) planner_type = 0; // if is 1, and inside here, it means jerk less than 1 + + double dx = tcGetDistanceToGo(tc, reverse_run); + double v_next = tc->currentvel + acc * tc->cycle_time; + + // Planner type 2 (9D): Check for segment completion BEFORE update + // This prevents velocity reaching zero before position reaches target. + // Based on Tormach's END_CONDITION_COMPLETE approach. + if (planner_type == 2) { + // Calculate displacement this cycle would produce + double displacement = (tc->currentvel + v_next) * 0.5 * tc->cycle_time; + + // Check if we will complete within this cycle: + // 1. Displacement would overshoot remaining distance, OR + // 2. Very close to target (within one cycle's worth of motion) and decelerating + // NOTE: Removed condition "(v_next <= 0.0 && dx > TP_POS_EPSILON)" which caused + // teleportation during abort - it would snap to endpoint whenever velocity hit + // zero regardless of how far from the target we were. + int will_complete = (displacement >= dx - TP_POS_EPSILON) || + (dx < tc->currentvel * tc->cycle_time && acc < 0.0); + + if (will_complete && dx > TP_POS_EPSILON) { + // Snap to endpoint exactly - don't let incremental updates miss + tc->progress = tcGetTarget(tc, reverse_run); + tc->currentvel = 0.0; + tc->on_final_decel = 1; + return TP_ERR_OK; + } + + // If velocity goes to zero but we're NOT close to target, just stop here + // (this happens during abort/pause - don't teleport to endpoint) + if (v_next <= 0.0) { + v_next = 0.0; + // Don't update progress - stay where we are + tc->currentvel = v_next; + tc->on_final_decel = 1; + return TP_ERR_OK; + } + + // Normal update - not completing this cycle + double disp_sign = reverse_run ? -1 : 1; + tc->progress += (disp_sign * displacement); + tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + tc->currentvel = v_next; + tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc <= 0.0); + return TP_ERR_OK; + } - v_next = tc->currentvel + acc * tc->cycle_time; - // update position in this tc using trapezoidal integration - // Note that progress can be greater than the target after this step. - //if (v_next < 0.0) { - if (planner_type == 0 && v_next < 0.0) { + // Planner type 0 (legacy LinuxCNC): Original KLUDGE logic + if (v_next < 0.0) { v_next = 0.0; - //KLUDGE: the trapezoidal planner undershoots by half a cycle time, so - //forcing the endpoint here is necessary. However, velocity undershoot - //also occurs during pausing and stopping, which can happen far from - //the end. If we could "cruise" to the endpoint within a cycle at our - //current speed, then assume that we want to be at the end. - if (tcGetDistanceToGo(tc,reverse_run) < (tc->currentvel * tc->cycle_time)) { - tc->progress = tcGetTarget(tc,reverse_run); + // KLUDGE: the trapezoidal planner undershoots by half a cycle time, so + // forcing the endpoint here is necessary. However, velocity undershoot + // also occurs during pausing and stopping, which can happen far from + // the end. If we could "cruise" to the endpoint within a cycle at our + // current speed, then assume that we want to be at the end. + if (dx < (tc->currentvel * tc->cycle_time)) { + tc->progress = tcGetTarget(tc, reverse_run); } } else { double displacement = (v_next + tc->currentvel) * 0.5 * tc->cycle_time; - // Account for reverse run (flip sign if need be) double disp_sign = reverse_run ? -1 : 1; - if(planner_type == 0) - tc->progress += (disp_sign * displacement); - - //Progress has to be within the allowable range + tc->progress += (disp_sign * displacement); tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); } - // Calculate jerk as rate of change of acceleration (for trapezoidal, this is high) - double jerk = 0.0; - if (tc->cycle_time > TP_TIME_EPSILON) { - jerk = (acc - tc->currentacc) / tc->cycle_time; - } - if(planner_type == 0){ tc->currentvel = v_next; - tc->currentacc = acc; - tc->currentjerk = jerk; - - // Check if we can make the desired velocity tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc < 0.0); - }else{ - // Check if we can make the desired velocity - tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc <= 0.0); - tc->currentvel = v_next; - tc->currentacc = acc; - tc->currentjerk = jerk; - } return TP_ERR_OK; } @@ -3304,15 +3602,49 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, * Cleanup if tc is not valid (empty queue). * If the program ends, or we hit QUEUE STARVATION, do a soft reset on the trajectory planner. * TODO merge with tpClear? + * + * For planner_type 2 (9D dual-layer): DON'T reset the queue indices. + * Userspace may be writing to the queue asynchronously, and resetting + * would wipe out those writes. Only update status fields. + * (Following Tormach's tpCleanupAtEmptyQueue() approach) */ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) { + /* For planner_type 2 (9D architecture with batch queueing): + * + * An empty queue is a NORMAL TRANSIENT STATE, not program completion. + * Userspace adds segments in batch, RT consumes over many cycles. + * + * Just clean up execution state. tpIsDone() will compute completion + * dynamically based on queue state and filter draining. + */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + // For planner type 2, empty queue is a normal transient state + // DO NOT reset queue structure (tcqInit) - userspace may be writing to it + // DO NOT sync goalPos - userspace OWNS goalPos and updates it after each add + // (Following Tormach's tpCleanupAtEmptyQueue which explicitly doesn't touch goalPos) + tp->depth = tp->activeDepth = 0; + tp->execId = 0; + tp->motionType = 0; + + // Safety net: if abort/pause flags are still set when queue empties, + // clean them up so the system doesn't get stuck + if (tp->aborting || tp->pausing) { + tpCleanupAfterAbort_9D(tp); + } + + // Update movement status + tpUpdateMovementStatus(tp, NULL); + return; + } + + /* Original behavior for planner_type 0/1 (demand-driven planning) */ tcqInit(&tp->queue); tp->goalPos = tp->currentPos; - tp->done = 1; tp->depth = tp->activeDepth = 0; tp->aborting = 0; + tp->abort_profiles_written = 0; tp->execId = 0; tp->motionType = 0; @@ -3375,7 +3707,6 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, tc->currentvel = 0.0; tc->term_vel = 0.0; - // Clean up Ruckig planner resources tcCleanupRuckig(tc); //TODO make progress to match target? @@ -3404,19 +3735,40 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, //Don't need to do anything if not aborting return TP_ERR_NO_ACTION; } + + // REMOVED: Workaround for late abort no longer needed. + // The abort-before-mode-change fix in emctask.cc (switching order of + // emcTaskAbort() and emcTrajSetMode()) ensures aborts arrive before + // fresh segments are queued, eliminating the race condition this was + // trying to detect. + //If the motion has stopped, then it's safe to reset the TP struct. if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || - (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { + (fabs(tc->currentvel) < TP_VEL_EPSILON && (!nexttc || fabs(nexttc->currentvel) < TP_VEL_EPSILON))) { tcqInit(&tp->queue); tp->goalPos = tp->currentPos; - tp->done = 1; tp->depth = tp->activeDepth = 0; - tp->aborting = 0; + + // Use centralized cleanup for 9D planner + if (GET_TRAJ_PLANNER_TYPE() == 2) { + tpCleanupAfterAbort_9D(tp); + // WORKAROUND: DO NOT clear userspace planning state on abort. + // Clearing g_smoothing_data causes optimizer to compute vel=0.000 on next run. + // The optimizer needs "warm" state to compute proper velocities. + // TODO: Implement proper state sync between RT and userspace. + // #ifdef USERSPACE_LIB_BUILD + // tpClearPlanning_9D(tp); + // #endif + } else { + tp->aborting = 0; + tp->abort_profiles_written = 0; + tp->reverse_run = 0; + } + tp->execId = 0; tp->motionType = 0; tp->synchronized = 0; - tp->reverse_run = 0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; tpResume(tp); @@ -3487,6 +3839,8 @@ STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) */ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { + /* Removed activation debug - now proven to work */ + //Check if already active if (!tc || tc->active) { return TP_ERR_OK; @@ -3562,6 +3916,43 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tc->blending_next = 0; tc->on_final_decel = 0; + tc->elapsed_time = 0; + + // Initialize execution state for branch/merge architecture + tc->position_base = 0.0; + tc->last_profile_generation = __atomic_load_n( + &tc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + + // Clear any pending branch from previous segment usage + __atomic_store_n(&tc->shared_9d.branch.valid, 0, __ATOMIC_RELEASE); + __atomic_store_n(&tc->shared_9d.branch.taken, 0, __ATOMIC_RELEASE); + + // Initialize canonical feed scale based on segment's motion type. + // Use feed_scale for feed moves, rapid_scale for traverses — not + // net_feed_scale which reflects the *previous* segment's type due + // to servo loop ordering (net_feed computed before tpRunCycle). + if (GET_TRAJ_PLANNER_TYPE() == 2) { + double actual_feed = 1.0; + if (emcmotStatus) { + if (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) { + actual_feed = emcmotStatus->rapid_scale; + } else { + actual_feed = emcmotStatus->feed_scale; + } + if (actual_feed < 0.0) actual_feed = 0.0; + if (actual_feed > 10.0) actual_feed = 10.0; + } + + tc->shared_9d.canonical_feed_scale = actual_feed; + tc->shared_9d.requested_feed_scale = actual_feed; + tc->shared_9d.achieved_exit_vel = 0.0; + + } else { + tc->shared_9d.canonical_feed_scale = 1.0; + tc->shared_9d.requested_feed_scale = 1.0; + tc->shared_9d.achieved_exit_vel = 0.0; + } + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { tp_debug_print("Setting up position sync\n"); // if we aren't already synced, wait @@ -3576,6 +3967,12 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { // Update the modal state displayed by the TP tp->execTag = tc->tag; + // Fire any queued segment actions (M-codes, etc.) + // This replaces the flush_segments() mechanism for planner_type 2 + if (GET_TRAJ_PLANNER_TYPE() == 2) { + tpFireSegmentActions(tc); + } + return TP_ERR_OK; } @@ -3711,6 +4108,33 @@ STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, STATIC int tpUpdateCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode) { + //=================================================================== + // TC_DWELL: Handle dwell segments (G4) inline with motion + // Dwell holds position for specified duration, then completes + //=================================================================== + if (tc->motion_type == TC_DWELL) { + // Count down the dwell timer + tc->dwell_remaining -= tc->cycle_time; + + // Keep velocity at zero during dwell + tc->currentvel = 0.0; + tc->currentacc = 0.0; + tc->currentjerk = 0.0; + + // Check if dwell is complete + if (tc->dwell_remaining <= 0.0) { + tc->dwell_remaining = 0.0; + tc->remove = 1; // Mark for removal from queue + tp_debug_print("Dwell complete, id=%d\n", tc->id); + } + + // Update status (position unchanged during dwell) + emcmotStatus->distance_to_go = 0.0; + emcmotStatus->current_vel = 0.0; + + return TP_ERR_OK; + } + //placeholders for position for this update EmcPose before; @@ -3727,9 +4151,242 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, double acc=0, vel_desired=0; int planner_type = GET_TRAJ_PLANNER_TYPE(); - if(mode == NULL) planner_type = 0; + // If mode is NULL and we're using scurve (planner_type 1), force to trapezoidal + // to avoid NULL dereference. planner_type 0 and 2 don't use mode. + if(mode == NULL && planner_type == 1) planner_type = 0; + + // Planner type 2 with valid Ruckig profile: use pre-computed trajectory + // When aborting/pausing: compute jerk-limited deceleration along the path + int use_ruckig = 0; + int use_ruckig_stopping = 0; + + if (planner_type == 2 && __atomic_load_n(&tc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + // Always use Ruckig branch/merge path, including for pause/abort + // Userspace computes a stop branch (same as 0% feed hold) via manageBranches() + // This provides time-optimal jerk-limited deceleration + use_ruckig = 1; + } + + if (use_ruckig_stopping) { + // LEGACY: Cycle-by-cycle jerk-limited stopping (dead code, kept for rollback) + // Now pause/abort uses the same Ruckig branch path as feed hold for + // time-optimal deceleration. This block is never reached but preserved + // for easy rollback if issues arise. + // Jerk-limited stopping: decelerate to v=0 while staying on path + // Use current state (already set from previous cycles) and compute + // one cycle of jerk-limited deceleration + double v0 = tc->currentvel; + double a0 = tc->currentacc; + double j_max = tc->maxjerk > 0 ? tc->maxjerk : 50000.0; + double a_max = tc->maxaccel; + double dt = tc->cycle_time; + + // Compute jerk-limited deceleration for this cycle + double v_new, a_new, j_cmd, dist; + tcComputeJerkLimitedStop(v0, a0, j_max, a_max, dt, + &v_new, &a_new, &j_cmd, &dist); + + // Update segment state + tc->progress += dist; + tc->currentvel = v_new; + tc->currentacc = a_new; + tc->currentjerk = j_cmd; + + // Clamp position to segment bounds + if (tc->progress > tc->target) tc->progress = tc->target; + if (tc->progress < 0.0) tc->progress = 0.0; + + // Check for stop completion + if (fabs(v_new) < 1e-6 && fabs(a_new) < 1e-6) { + tc->on_final_decel = 1; + // Mark as PAUSE-STOPPED (distinct from feed hold which uses 0.0) + // -1.0 signals: "stopped via cycle-by-cycle, profile is invalid" + // 0.0 signals: "feed hold profile, sample it normally" + tc->shared_9d.profile.computed_feed_scale = -1.0; + // Update position_base to current stopped position for resume + tc->position_base = tc->progress; + } + + tpDebugCycleInfo(tp, tc, nexttc, a_new); + } else if (use_ruckig) { + // === PHASE 3: BRANCH/MERGE - CHECK FOR PENDING BRANCH === + // Check if userspace has committed a branch for feed override + int branch_valid = __atomic_load_n(&tc->shared_9d.branch.valid, __ATOMIC_ACQUIRE); + int branch_taken = __atomic_load_n(&tc->shared_9d.branch.taken, __ATOMIC_ACQUIRE); + + if (branch_valid && !branch_taken) { + // A branch is waiting - check if we're in the handoff window + double handoff_time = tc->shared_9d.branch.handoff_time; + double window_end = tc->shared_9d.branch.window_end_time; + + if (tc->elapsed_time >= handoff_time && tc->elapsed_time < window_end) { + // Compute time offset for new profile (maintains continuity) + double new_elapsed = tc->elapsed_time - handoff_time; + + // Take the branch - swap profile + // Use sequence counter to signal copy in progress (odd = copying) + unsigned int seq = __atomic_load_n(&tc->shared_9d.copy_sequence, __ATOMIC_ACQUIRE); + __atomic_store_n(&tc->shared_9d.copy_sequence, seq + 1, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + + // Check if this is a two-stage branch (brake + main) + int has_brake = tc->shared_9d.branch.has_brake; + + if (has_brake) { + // TWO-STAGE: Start with brake profile + tc->shared_9d.profile = tc->shared_9d.branch.brake_profile; + } else { + // SINGLE-STAGE: Just main profile + tc->shared_9d.profile = tc->shared_9d.branch.profile; + } + + tc->position_base = tc->shared_9d.branch.handoff_position; + // Both two-stage and single-stage use new_elapsed to maintain continuity + // This accounts for time past the intended handoff moment + tc->elapsed_time = new_elapsed; + + // Signal copy complete (even = done) + __atomic_thread_fence(__ATOMIC_SEQ_CST); + __atomic_store_n(&tc->shared_9d.copy_sequence, seq + 2, __ATOMIC_RELEASE); + + // Sync generation counter so stopwatch-reset check below + // doesn't double-fire on the profile we just installed. + tc->last_profile_generation = __atomic_load_n( + &tc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + + // Signal that RT took the branch (userspace will merge) + __atomic_store_n(&tc->shared_9d.branch.taken, 1, __ATOMIC_RELEASE); + // Clear brake_done flag for fresh two-stage execution + __atomic_store_n(&tc->shared_9d.branch.brake_done, 0, __ATOMIC_RELEASE); + + } + } + + // === CHECK FOR BRAKE->MAIN TRANSITION (two-stage profiles) === + // If we're in a two-stage branch and brake is complete, switch to main profile + // Reload branch_taken since it might have just been set above + branch_taken = __atomic_load_n(&tc->shared_9d.branch.taken, __ATOMIC_ACQUIRE); + int has_brake = tc->shared_9d.branch.has_brake; + int brake_done = __atomic_load_n(&tc->shared_9d.branch.brake_done, __ATOMIC_ACQUIRE); + + if (branch_taken && has_brake && !brake_done) { + // Check if brake profile is complete + double brake_duration = tc->shared_9d.branch.brake_profile.duration; + if (tc->elapsed_time >= brake_duration) { + // Brake complete - switch to main profile + unsigned int seq = __atomic_load_n(&tc->shared_9d.copy_sequence, __ATOMIC_ACQUIRE); + __atomic_store_n(&tc->shared_9d.copy_sequence, seq + 1, __ATOMIC_RELEASE); + __atomic_thread_fence(__ATOMIC_SEQ_CST); + + // Update position base to brake end position + tc->position_base = tc->shared_9d.branch.brake_end_position; + + // Main profile starts at (elapsed - brake_duration) to maintain continuity + // Clamp to 0 to avoid negative elapsed time from floating point rounding + double time_past_brake = tc->elapsed_time - brake_duration; + tc->elapsed_time = (time_past_brake > 0.0) ? time_past_brake : 0.0; + tc->shared_9d.profile = tc->shared_9d.branch.profile; + + __atomic_thread_fence(__ATOMIC_SEQ_CST); + __atomic_store_n(&tc->shared_9d.copy_sequence, seq + 2, __ATOMIC_RELEASE); + + // Sync generation counter so stopwatch-reset doesn't double-fire + tc->last_profile_generation = __atomic_load_n( + &tc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + + // Mark brake as done + __atomic_store_n(&tc->shared_9d.branch.brake_done, 1, __ATOMIC_RELEASE); + } + } + + + // Publish execution state for userspace to read (each cycle) + __atomic_store_n(&tc->active_segment_id, tc->id, __ATOMIC_RELEASE); + + // === STOPWATCH RESET: detect profile swap by generation counter === + // When userspace recomputes a profile (backward pass convergence), + // the generation counter increments. If we detect a new generation, + // reset the stopwatch so we sample the NEW profile from t=0, with + // position_base absorbing all progress made so far. + // This mirrors the branch/merge mechanism for feed override. + // NOTE: Branch take and brake->main transitions update + // last_profile_generation above, so this only fires for + // convergence-driven profile rewrites (the intended case). + { + int prof_gen = __atomic_load_n(&tc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + if (prof_gen != tc->last_profile_generation) { + // Profile was swapped — reset stopwatch + tc->position_base = tc->progress; // absorb current progress + tc->elapsed_time = 0.0; + tc->last_profile_generation = prof_gen; + } + } + + // === SAMPLE ACTIVE PROFILE === + // Clamp sample time to profile duration. When elapsed_time overshoots + // duration, this ensures we land exactly at the profile's end position + // rather than extrapolating, and the displacement naturally covers + // the correct fractional time. + double sample_time = tc->elapsed_time; + double duration = tc->shared_9d.profile.duration; + if (sample_time > duration) { + sample_time = duration; + } + + double pos, vel, acc_ruckig, jerk; + int sample_ok = ruckigProfileSample(&tc->shared_9d.profile, + sample_time, + &pos, &vel, &acc_ruckig, &jerk); + + if (sample_ok == 0) { + // Apply position base offset from profile swaps + double total_pos = tc->position_base + pos; - if(planner_type != 1){ + // Clamp position to segment bounds + if (total_pos > tc->target) { + total_pos = tc->target; + } + if (total_pos < 0.0) { total_pos = 0.0; } + + + tc->progress = total_pos; + tc->currentvel = vel; + tc->currentacc = acc_ruckig; + tc->currentjerk = jerk; + + tc->elapsed_time += tc->cycle_time; + + // Check for segment completion + // For two-stage profiles (brake + main), don't complete during brake phase + int in_brake_phase = (tc->shared_9d.branch.has_brake && + __atomic_load_n(&tc->shared_9d.branch.taken, __ATOMIC_ACQUIRE) && + !__atomic_load_n(&tc->shared_9d.branch.brake_done, __ATOMIC_ACQUIRE)); + + // Feed hold detection: if computed_feed_scale is ~0, this is a "stop in place" + // profile using velocity control. Do NOT complete the segment until: + // - Feed is restored (new branch will be computed), OR + // - Position actually reaches target (shouldn't happen in feed hold) + // This prevents "teleporting" to segment end when feed hold profile completes. + int in_feed_hold = (tc->shared_9d.profile.computed_feed_scale < 0.001); + + // Note: For Ruckig, we do NOT snap progress to target here. + // The split mechanism (Option E) detects completion one cycle early + // via look-ahead in tpCheckEndCondition, and the split handler samples + // the profile at duration for the exact final position. + // Only set on_final_decel flag for non-tangent segments that need to stop. + if (!in_brake_phase && !in_feed_hold && + tc->progress >= tc->target - TP_POS_EPSILON) { + tc->progress = tc->target; + tc->on_final_decel = 1; + } + + tpDebugCycleInfo(tp, tc, nexttc, acc_ruckig); + } else { + // Ruckig sample failed - fall back to trapezoidal + goto fallback_trapezoidal; + } + } else if(planner_type != 1){ +fallback_trapezoidal: // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity // Also, don't ramp up for parabolic blends if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -3745,7 +4402,8 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); tpDebugCycleInfo(tp, tc, nexttc, acc); - }else{ + } + else{ if(*mode == 1){ double jerk; double perror; @@ -3868,29 +4526,163 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, double dx = tcGetDistanceToGo(tc, tp->reverse_run); tc_debug_print("tpCheckEndCondition: dx = %e\n",dx); + // FIX: For planner type 2, force completion if stuck + // This handles case where motion stops before reaching target + // TODO: Proper fix with backward velocity pass in optimization + if (GET_TRAJ_PLANNER_TYPE() == 2 && dx > TP_POS_EPSILON && dx < 1.5) { + // IMPORTANT: Don't force completion during feed hold! + // Feed hold intentionally stops the machine - this is not "stuck" + int in_feed_hold = (tc->shared_9d.profile.computed_feed_scale < 0.001); + + // Force complete if velocity very low OR if we've been stuck for a while + // But NOT during feed hold + static int stuck_cycles = 0; + static double last_progress = 0.0; + + if (in_feed_hold || tp->aborting) { + // Reset stuck counter during feed hold or abort - we're stopped on purpose + stuck_cycles = 0; + } else if (fabs(tc->progress - last_progress) < 1e-6 && + (fabs(tc->currentvel) < 1e-3 || + tc->elapsed_time >= tc->shared_9d.profile.duration)) { + // Stuck: progress not changing AND either velocity is near zero + // or the profile has finished (elapsed >= duration). The second + // condition catches TANGENT segments where the profile ends with + // non-zero final velocity but landed short of target. + stuck_cycles++; + } else { + stuck_cycles = 0; + } + last_progress = tc->progress; + + if (stuck_cycles > 10) { // Stuck for 10ms + // Force to target + printf("STUCK_FORCE seg=%d progress=%.6f target=%.6f dx=%.6f " + "vel=%.6f elapsed=%.6f dur=%.6f pos_base=%.6f\n", + tc->id, tc->progress, tc->target, dx, + tc->currentvel, tc->elapsed_time, + tc->shared_9d.profile.duration, tc->position_base); + dx = 0.0; + tc->progress = tc->target; + stuck_cycles = 0; + } + } + if (dx <= TP_POS_EPSILON) { //If the segment is close to the target position, then we assume that it's done. tp_debug_print("close to target, dx = %.12f\n",dx); + //Force progress to land exactly on the target to prevent numerical errors. tc->progress = tcGetTarget(tc, tp->reverse_run); if (!tp->reverse_run) { tcSetSplitCycle(tc, 0.0, tc->currentvel); } - if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT || tp->reverse_run) { + if (tc->term_cond == TC_TERM_COND_STOP || tp->reverse_run || + (tc->term_cond != TC_TERM_COND_TANGENT && tc->finalvel <= TP_VEL_EPSILON)) { + // STOP, reverse, or unpromoted EXACT (finalvel=0): remove immediately. + // Promoted segments (TANGENT with finalvel > 0) go through split cycle. tc->remove = 1; } return TP_ERR_OK; } else if (tp->reverse_run) { return TP_ERR_NO_ACTION; - } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { + } else if (tc->term_cond == TC_TERM_COND_STOP || + (tc->term_cond != TC_TERM_COND_TANGENT && tc->finalvel <= TP_VEL_EPSILON)) { + // STOP or unpromoted (finalvel=0): no split cycle needed, just run normally. return TP_ERR_NO_ACTION; } + // Ruckig look-ahead: use profile duration to compute exact split time. + // At this point elapsed_time has been advanced past the current sample: + // last_sample_time = elapsed_time - cycleTime + // elapsed_time = next sample time + // The actual remaining motion time is from last_sample_time to duration: + // actual_remaining = duration - last_sample_time = duration - elapsed + cycleTime + // If this fits within one cycleTime, we split now. + // + // IMPORTANT: Only use duration-based split when the active profile actually + // reaches the segment target. Partial profiles — brake phases (speed adjustment), + // feed-hold stops (decel to zero mid-segment), or any branch that doesn't cover + // the remaining distance — have short durations unrelated to segment completion. + // Using their duration would falsely trigger a split, truncating the segment. + // These partial profiles complete naturally, a new profile (main phase or resume) + // swaps in, and then this check works correctly with the full-distance profile. + if (GET_TRAJ_PLANNER_TYPE() == 2 && __atomic_load_n(&tc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + double duration = tc->shared_9d.profile.duration; + double last_sample_time = tc->elapsed_time - tp->cycleTime; + double actual_remaining = duration - last_sample_time; + + // Sample profile at its end to see what position/velocity it actually reaches + double end_pos, end_vel, end_acc, end_jrk; + int end_ok = ruckigProfileSample(&tc->shared_9d.profile, duration, + &end_pos, &end_vel, &end_acc, &end_jrk); + double profile_covers = (end_ok == 0) ? (tc->target - tc->position_base - end_pos) : -1.0; + + // If the profile doesn't reach the segment target, it's a partial profile + // (brake, feed-hold stop, or mid-segment branch). Don't use its duration + // for split timing — wait for a profile that covers the full remaining distance. + double gap_threshold = tc->target * 1e-6; // relative tolerance + if (gap_threshold < TP_POS_EPSILON) gap_threshold = TP_POS_EPSILON; + if (end_ok != 0 || profile_covers > gap_threshold) { + return TP_ERR_NO_ACTION; + } + + // Planner 2 split handler uses tc->currentvel directly (not tc->term_vel), + // so v_f=0.0 is fine here — tcSetSplitCycle stores it in term_vel which + // only planner 0 reads in tpHandleSplitCycle. + if (actual_remaining <= 0.0) { + // Profile already completed at last sample — split with zero time + tcSetSplitCycle(tc, 0.0, 0.0); + return TP_ERR_OK; + } else if (actual_remaining < tp->cycleTime) { + // Remaining motion fits within one cycle — split with exact time + tcSetSplitCycle(tc, actual_remaining, 0.0); + return TP_ERR_OK; + } else { + // More than one cycle remaining based on duration — no split needed. + // But check if POSITION will overshoot the segment at next sample. + // For stop profiles that overshoot, duration is long but position + // reaches the segment boundary before profile ends. + double next_sample_time = tc->elapsed_time; // already advanced + double next_pos, next_vel, next_acc, next_jrk; + int next_ok = ruckigProfileSample(&tc->shared_9d.profile, + next_sample_time, + &next_pos, &next_vel, &next_acc, &next_jrk); + double next_total = tc->position_base + next_pos; + if (next_ok == 0 && next_total > tc->target) { + // Position will overshoot segment boundary next cycle. + // Binary search for exact crossing time between last sample + // and next sample, then split at that time. + double remaining_dist = tc->target - tc->position_base; + double t_lo = next_sample_time - tp->cycleTime; // last sample time + double t_hi = next_sample_time; + for (int i = 0; i < 50; i++) { + double t_mid = (t_lo + t_hi) * 0.5; + double p, v, a, j; + ruckigProfileSample(&tc->shared_9d.profile, t_mid, &p, &v, &a, &j); + if (p < remaining_dist) { + t_lo = t_mid; + } else { + t_hi = t_mid; + } + if (t_hi - t_lo < 1e-9) break; + } + // split_time = time from last sample to crossing + double split_time = t_hi - (next_sample_time - tp->cycleTime); + if (split_time < 0.0) split_time = 0.0; + if (split_time > tp->cycleTime) split_time = tp->cycleTime; + + tcSetSplitCycle(tc, split_time, 0.0); + return TP_ERR_OK; + } + return TP_ERR_NO_ACTION; + } + } + double v_f = tpGetRealFinalVel(tp, tc, nexttc); double v_avg = (tc->currentvel + v_f) / 2.0; - //Check that we have a non-zero "average" velocity between now and the //finish. If not, it means that we have to accelerate from a stop, which //will take longer than the minimum 2 timesteps that each segment takes, so @@ -3974,13 +4766,72 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, return TP_ERR_NO_ACTION; } + tp_debug_print("tc id %d splitting\n",tc->id); + //Pose data to calculate movement due to finishing current TC EmcPose before; tcGetPos(tc, &before); - tp_debug_print("tc id %d splitting\n",tc->id); - //Shortcut tc update by assuming we arrive at end - tc->progress = tcGetTarget(tc,tp->reverse_run); + if (GET_TRAJ_PLANNER_TYPE() == 2 && __atomic_load_n(&tc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + // For Ruckig: sample at the exact crossing time (when position reaches + // segment target) to get physically correct vel/acc for handoff. + // tc->cycle_time was set by tpCheckEndCondition to the time from + // last sample to segment boundary crossing. + double dur = tc->shared_9d.profile.duration; + double last_sample_time = tc->elapsed_time - tp->cycleTime; + double crossing_time = last_sample_time + tc->cycle_time; + if (crossing_time > dur) crossing_time = dur; + if (crossing_time < 0.0) crossing_time = 0.0; + + double pos, vel, acc, jrk; + int ok = ruckigProfileSample(&tc->shared_9d.profile, crossing_time, + &pos, &vel, &acc, &jrk); + if (ok == 0) { + double total_pos = tc->position_base + pos; + int split_clamped = 0; + if (total_pos > tc->target) { total_pos = tc->target; split_clamped = 1; } + if (total_pos < 0.0) { total_pos = 0.0; split_clamped = -1; } + + tc->progress = total_pos; + if (!split_clamped) { + // Crossing is within segment — use vel/acc at crossing time + tc->currentvel = vel; + tc->currentacc = acc; + tc->currentjerk = jrk; + } else { + // Position overshoot: binary search for exact crossing time + // between last_sample_time and crossing_time + double remaining_dist = tc->target - tc->position_base; + double t_lo = last_sample_time; + double t_hi = crossing_time; + for (int i = 0; i < 50; i++) { + double t_mid = (t_lo + t_hi) * 0.5; + double p, v, a, j; + ruckigProfileSample(&tc->shared_9d.profile, t_mid, &p, &v, &a, &j); + if (p < remaining_dist) { + t_lo = t_mid; + } else { + t_hi = t_mid; + } + if (t_hi - t_lo < 1e-9) break; + } + // Sample at the converged crossing point for vel/acc + double cp, cv, ca, cj; + ruckigProfileSample(&tc->shared_9d.profile, t_hi, &cp, &cv, &ca, &cj); + tc->currentvel = cv; + tc->currentacc = ca; + tc->currentjerk = cj; + } + + + } else { + tc->progress = tcGetTarget(tc, tp->reverse_run); + } + } else { + //Shortcut tc update by assuming we arrive at end (non-Ruckig path) + tc->progress = tcGetTarget(tc, tp->reverse_run); + } + //Get displacement from prev. position EmcPose displacement; tcGetPos(tc, &displacement); @@ -3992,7 +4843,7 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, #ifdef TC_DEBUG double mag; emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n",mag); + tc_debug_print("cycle movement = %f\n", mag); #endif // Trigger removal of current segment at the end of the cycle @@ -4008,15 +4859,16 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, nexttc->cycle_time = tp->cycleTime - tc->cycle_time; // In S-curve mode, use actual current velocity instead of expected term_vel // S-curve can't change velocity instantly, term_vel is just desired value - if (GET_TRAJ_PLANNER_TYPE() == 1) { + if (GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2) { + // Jerk-limited planners (S-curve / Ruckig): use actual current velocity + // These planners can't change velocity instantly; term_vel is just the desired value nexttc->currentvel = tc->currentvel; // Inherit acceleration, but limit to nexttc's allowed range // Important for line-to-arc transitions where arc has lower tangential accel double maxacc_next = tcGetTangentialMaxAccel(nexttc); nexttc->currentacc = saturate(tc->currentacc, maxacc_next); - nexttc->currentjerk = tc->currentjerk; - tp_debug_print("Doing tangent split (S-curve): vel=%f, acc=%f (limited by %f), jerk=%f\n", - nexttc->currentvel, nexttc->currentacc, maxacc_next, nexttc->currentjerk); + + } else { // Trapezoidal: can use term_vel (assumes instant velocity change) nexttc->currentvel = tc->term_vel; @@ -4035,14 +4887,137 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, tc->id); } + // Full split-cycle activation for Ruckig (bypasses tpActivateSegment). + // Must replicate all critical initialization that tpActivateSegment performs. + if (GET_TRAJ_PLANNER_TYPE() == 2) { + nexttc->active = 1; + nexttc->on_final_decel = 0; + nexttc->blending_next = 0; + nexttc->position_base = 0.0; + + // Snapshot current profile generation so stopwatch-reset check + // doesn't false-trigger on the first sampling cycle. + nexttc->last_profile_generation = __atomic_load_n( + &nexttc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + + // Clear any pending branch from previous queue slot usage + __atomic_store_n(&nexttc->shared_9d.branch.valid, 0, __ATOMIC_RELEASE); + __atomic_store_n(&nexttc->shared_9d.branch.taken, 0, __ATOMIC_RELEASE); + + // Initialize feed override state based on segment's motion type. + // Use feed_scale/rapid_scale directly instead of net_feed_scale, + // which reflects the previous segment's type due to servo loop ordering. + double actual_feed = 1.0; + if (emcmotStatus) { + if (nexttc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) { + actual_feed = emcmotStatus->rapid_scale; + } else { + actual_feed = emcmotStatus->feed_scale; + } + if (actual_feed < 0.0) actual_feed = 0.0; + if (actual_feed > 10.0) actual_feed = 10.0; + } + nexttc->shared_9d.canonical_feed_scale = actual_feed; + nexttc->shared_9d.requested_feed_scale = actual_feed; + nexttc->shared_9d.achieved_exit_vel = 0.0; + + // Pre-advance so Ruckig samples at remain_time instead of t=0. + // Without this, the first sample returns pos=0 (zero displacement). + nexttc->elapsed_time = nexttc->cycle_time; + + } + + // Save remain_time before tpUpdateCycle — tpCheckEndCondition inside + // tpUpdateCycle will overwrite nexttc->cycle_time to tp->cycleTime. + double nexttc_remain_time = nexttc->cycle_time; + // Run split cycle update with remaining time in nexttc // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL) int queue_dir_step = tp->reverse_run ? -1 : 1; TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2); - + + + // Save actual junction velocity before tpUpdateCycle overwrites it + // with the profile's (potentially stale) entry velocity. + double junction_vel = nexttc->currentvel; + + // Alt-entry profile selection: when a brake on the previous segment + // changed the exit velocity, pick whichever profile (main or alt_entry) + // has v0 closer to the actual junction velocity. + int blend_alt_taken = 0; + if (GET_TRAJ_PLANNER_TYPE() == 2 && + __atomic_load_n(&nexttc->shared_9d.alt_entry.valid, __ATOMIC_ACQUIRE)) { + double main_v0 = nexttc->shared_9d.profile.v[0]; + double alt_v0 = nexttc->shared_9d.alt_entry.v0; + if (fabs(junction_vel - alt_v0) < fabs(junction_vel - main_v0)) { + nexttc->shared_9d.profile = nexttc->shared_9d.alt_entry.profile; + // Sync generation counter so stopwatch reset doesn't fire — + // the alt_entry profile has a different generation from the + // one snapshotted during split setup, which would cause a + // spurious reset (elapsed_time=0 → zero displacement). + nexttc->last_profile_generation = __atomic_load_n( + &nexttc->shared_9d.profile.generation, __ATOMIC_ACQUIRE); + blend_alt_taken = 1; + } + __atomic_store_n(&nexttc->shared_9d.alt_entry.valid, 0, __ATOMIC_RELEASE); + } + + // Debug: log junction handoff for blend segments + if (GET_TRAJ_PLANNER_TYPE() == 2 && + (tc->motion_type == TC_BEZIER || nexttc->motion_type == TC_BEZIER)) { + static int junc_dbg = 0; + if (junc_dbg < 20) { + junc_dbg++; + double pv0 = nexttc->shared_9d.profile.v[0]; + double pvf = nexttc->shared_9d.profile.v[RUCKIG_PROFILE_PHASES]; + rtapi_print_msg(RTAPI_MSG_ERR, + "BLEND_RT[tc=%d->%d]: junc_vel=%.3f profile_v0=%.3f pvf=%.3f " + "tc_type=%d->%d alt=%d remain=%.6f target=%.4f\n", + tc->id, nexttc->id, junction_vel, pv0, pvf, + tc->motion_type, nexttc->motion_type, blend_alt_taken, + nexttc_remain_time, nexttc->target); + } + } + int mode = 0; tpUpdateCycle(tp, nexttc, next2tc, &mode); + // Correct profile v0 mismatch at split junction. + // When feed override changes between profile computation and junction + // arrival, the profile's v[0] doesn't match the actual junction velocity. + // tpUpdateCycle samples the profile and overwrites currentvel with the + // profile velocity, creating a discontinuity. Correct by shifting + // velocity and position by the mismatch delta. Both progress and + // position_base are shifted equally so subsequent cycle displacements + // (which depend on delta-progress) are unaffected. + // IMPORTANT: Only apply when junction_vel > profile_v0 (positive correction). + // Negative correction makes position_base negative, and since the profile + // covers exactly target distance (from 0), the segment can never reach + // target, causing a multi-cycle stall and large velocity dip. + if (GET_TRAJ_PLANNER_TYPE() == 2 && + __atomic_load_n(&nexttc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + double profile_v0 = nexttc->shared_9d.profile.v[0]; + double vel_mismatch = junction_vel - profile_v0; + if (vel_mismatch > 0.01) { + nexttc->currentvel += vel_mismatch; + // Correct split-cycle displacement to reflect actual velocity + double pos_correction = vel_mismatch * nexttc_remain_time; + nexttc->progress += pos_correction; + nexttc->position_base += pos_correction; + } + } + + // Post-correct elapsed_time for Ruckig split cycle. + // tpUpdateCycle sampled at remain_time then advanced by remain_time internally + // (because cycle_time was remain_time at sample time), giving elapsed = 2*remain_time. + // But tpCheckEndCondition inside tpUpdateCycle then overwrote cycle_time to cycleTime. + // The next regular cycle should sample at remain_time + cycleTime. + if (GET_TRAJ_PLANNER_TYPE() == 2 && + __atomic_load_n(&nexttc->shared_9d.profile.valid, __ATOMIC_ACQUIRE)) { + nexttc->elapsed_time = nexttc_remain_time + tp->cycleTime; + } + + // Update status for the split portion // FIXME redundant tangent check, refactor to switch if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -4072,27 +5047,32 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, int mode = 0; tpUpdateCycle(tp, tc, nexttc, &mode); - /* Parabolic blending */ - - double v_this = 0.0, v_next = 0.0; - - // cap the blend velocity at the current requested speed (factoring in feed override) - double target_vel_this = tpGetRealTargetVel(tp, tc); - double target_vel_next = tpGetRealTargetVel(tp, nexttc); - - if(mode != TP_SCURVE_ACCEL_ERROR && GET_TRAJ_PLANNER_TYPE() == 1) - tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); - else - tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); - tc->blend_vel = v_this; - if (nexttc) { - nexttc->blend_vel = v_next; - } + /* Parabolic blending — only for planner 0/1. + * Planner 2 uses Ruckig profiles with tangent-mode blending, + * never parabolic blending (term_cond is never TC_TERM_COND_PARABOLIC). */ + if (GET_TRAJ_PLANNER_TYPE() != 2) { + double v_this = 0.0, v_next = 0.0; + + // cap the blend velocity at the current requested speed (factoring in feed override) + double target_vel_this = tpGetRealTargetVel(tp, tc); + double target_vel_next = tpGetRealTargetVel(tp, nexttc); + + if(mode != TP_SCURVE_ACCEL_ERROR && GET_TRAJ_PLANNER_TYPE() == 1) + tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + else + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + tc->blend_vel = v_this; + if (nexttc) { + nexttc->blend_vel = v_next; + } - if (nexttc && tcIsBlending(tc)) { - tpDoParabolicBlending(tp, tc, nexttc); + if (nexttc && tcIsBlending(tc)) { + tpDoParabolicBlending(tp, tc, nexttc); + } else { + tpToggleDIOs(tc); + tpUpdateMovementStatus(tp, tc); + } } else { - //Update status for a normal step tpToggleDIOs(tc); tpUpdateMovementStatus(tp, tc); } @@ -4139,8 +5119,80 @@ int tpRunCycle(TP_STRUCT * const tp, long period) return TP_ERR_WAITING; } + /* For planner type 2: Startup queue depth check and profile validity + * + * Problem 1: When the first segment is added, RT immediately starts executing. + * If userspace is slow adding segments, RT can consume the first segment + * before the second is added, losing velocity continuity. + * + * Problem 2: If RT starts executing before the Ruckig profile is computed, + * it uses trapezoidal fallback. When the profile becomes valid mid-segment, + * switching from trapezoidal to Ruckig causes a position discontinuity + * because the two methods compute different positions for the same elapsed time. + * + * Solution: Wait for Ruckig profile validity AND queue depth before starting. + */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + int queue_len = tcqLen(&tp->queue); + + /* Wait for Ruckig profile to be computed before starting ANY segment. + * This prevents trapezoidal→Ruckig switch mid-execution which causes + * position discontinuity in cycles 2-8 after motion start. */ + if (!__atomic_load_n(&tc->shared_9d.profile.valid, __ATOMIC_ACQUIRE) && tc->progress < TP_POS_EPSILON) { + static int profile_wait_count = 0; + profile_wait_count++; + + /* Wait up to 200 cycles (200ms at 1kHz) for profile */ + if (profile_wait_count < 200) { + return TP_ERR_WAITING; + } + /* Timeout - userspace too slow, will use trapezoidal for entire segment */ + rtapi_print_msg(RTAPI_MSG_WARN, + "Ruckig profile timeout seg=%d, using trapezoidal fallback\n", tc->id); + profile_wait_count = 0; + } + + /* Only delay on the FIRST segment of a program (id == 0). + * Without the id check, this also triggers on the LAST segment + * (which also has progress=0, queue_len=1, nexttc=NULL) causing + * a 100-cycle delay before the final move executes. */ + if (tc->id == 0 && tc->progress < TP_POS_EPSILON && queue_len < 2 && nexttc == NULL) { + static int startup_wait_count = 0; + startup_wait_count++; + + /* Wait up to 100 cycles (100ms) for more segments */ + if (startup_wait_count < 100) { + return TP_ERR_WAITING; + } + startup_wait_count = 0; // Reset for next program + } + } + + /* Removed verbose segment debug - now proven to work */ + tc_debug_print("-------------------\n"); + /* Apply pre-computed 9D optimizer velocities if planner_type == 2 + * Userspace optimizer writes to shared_9d structure atomically, + * RT layer reads here before executing segment. + * + * SAFETY: Check optimization_state first before reading final_vel + */ + if (GET_TRAJ_PLANNER_TYPE() == 2) { + // Read optimization state atomically (int-sized enum) + int opt_state = __atomic_load_n(&tc->shared_9d.optimization_state, __ATOMIC_SEQ_CST); + + // Only apply optimized velocity if segment has been processed by optimizer + if (opt_state >= TC_PLAN_OPTIMIZED) { + double opt_vel = atomicLoadDouble(&tc->shared_9d.final_vel); + + // Sanity check: velocity should be positive and reasonable + if (opt_vel > 0.0 && opt_vel < 1e6) { + tc->target_vel = opt_vel; + tc_debug_print("9D: Applied optimized velocity %g (state=%d)\n", opt_vel, opt_state); + } + } + } /* If the queue empties enough, assume that the program is near the end. * This forces the last segment to be "finalized" to let the optimizer run.*/ @@ -4190,9 +5242,9 @@ int tpRunCycle(TP_STRUCT * const tp, long period) EmcPose pos_before = tp->currentPos; #endif - tcClearFlags(tc); tcClearFlags(nexttc); + // Update the current tc if (tc->splitting) { tpHandleSplitCycle(tp, tc, nexttc); @@ -4201,21 +5253,24 @@ int tpRunCycle(TP_STRUCT * const tp, long period) } #ifdef TC_DEBUG - double mag; - EmcPose disp; - emcPoseSub(&tp->currentPos, &pos_before, &disp); - emcPoseMagnitude(&disp, &mag); - tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", - time_elapsed, - mag, emcmotStatus->current_vel); - - tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", - disp.tran.x, - disp.tran.y, - disp.tran.z, - time_elapsed); + { + double mag; + EmcPose disp; + emcPoseSub(&tp->currentPos, &pos_before, &disp); + emcPoseMagnitude(&disp, &mag); + tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", + time_elapsed, + mag, emcmotStatus->current_vel); + + tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", + disp.tran.x, + disp.tran.y, + disp.tran.z, + time_elapsed); + } #endif + // If TC is complete, remove it from the queue. if (tc->remove) { tpCompleteSegment(tp, tc); @@ -4267,6 +5322,31 @@ int tpAbort(TP_STRUCT * const tp) /* const to abort, signal a pause and set our abort flag */ tpPause(tp); tp->aborting = 1; + + /* For planner_type 2: do NOT clear the branch slot. + * Userspace (tpRequestAbortBranch_9D) has already computed a stop + * branch before this command arrives. Clearing it would destroy the + * stop trajectory and cause a freeze. + * + * For other planner types: clear pending branch on abort/E-stop + * to ensure E-stop takes priority over feed override replanning. */ + if (GET_TRAJ_PLANNER_TYPE() != 2) { + TC_STRUCT *tc = tcqItem(&tp->queue, 0); + if (tc) { + __atomic_store_n(&tc->shared_9d.branch.valid, 0, __ATOMIC_RELEASE); + __atomic_store_n(&tc->shared_9d.branch.taken, 0, __ATOMIC_RELEASE); + } + } + + /* FIX: Clear nexttc->currentvel to prevent blocking abort cleanup. + * During tangent blending, nexttc->currentvel is set to the blend velocity. + * If abort happens mid-blend, this stale velocity blocks tpHandleAbort's + * cleanup condition (tc->currentvel==0 && nexttc->currentvel==0). + * Since nexttc won't execute during abort, clear its velocity. */ + TC_STRUCT *nexttc = tcqItem(&tp->queue, 1); + if (nexttc) { + nexttc->currentvel = 0.0; + } } return tpClearDIOs(tp); //clears out any already cached DIOs } @@ -4289,13 +5369,78 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) return TP_ERR_OK; } +/** + * @brief Get joint positions from userspace kinematics joint-space segment + * + * Evaluates pre-computed joint-space segment using linear interpolation. + * If joint_space data is not valid, returns failure (caller should use kinematicsInverse). + * + * @param tp Trajectory planner + * @param joints Output array of joint positions [JOINT_SPACE_MAX_JOINTS] + * @return TP_ERR_OK on success, TP_ERR_FAIL if joint_space not valid + */ +int tpGetJointPositions(TP_STRUCT const * const tp, double * const joints) +{ + int j; + + if (0 == tp || 0 == joints) { + return TP_ERR_FAIL; + } + + // Get current segment + TC_STRUCT *tc = tcqItem(&((TP_STRUCT*)tp)->queue, 0); + if (!tc) { + // No current segment - return current position as joints + // (for identity kinematics, world = joints) + joints[0] = tp->currentPos.tran.x; + joints[1] = tp->currentPos.tran.y; + joints[2] = tp->currentPos.tran.z; + joints[3] = tp->currentPos.a; + joints[4] = tp->currentPos.b; + joints[5] = tp->currentPos.c; + joints[6] = tp->currentPos.u; + joints[7] = tp->currentPos.v; + joints[8] = tp->currentPos.w; + return TP_ERR_OK; + } + + // Check if userspace kinematics joint_space data is valid + if (!tc->joint_space.valid) { + return TP_ERR_FAIL; // Caller should use kinematicsInverse + } + + // Compute normalized progress [0, 1] + double progress = 0.0; + if (tc->target > 1e-9) { + progress = tc->progress / tc->target; + if (progress < 0.0) progress = 0.0; + if (progress > 1.0) progress = 1.0; + } + + // Linear interpolation between start and end joint positions + const JointSpaceSegment *js = &tc->joint_space; + for (j = 0; j < js->num_joints && j < JOINT_SPACE_MAX_JOINTS; j++) { + joints[j] = js->start[j] + progress * (js->end[j] - js->start[j]); + } + + // Zero unused joints + for (; j < JOINT_SPACE_MAX_JOINTS; j++) { + joints[j] = 0.0; + } + + return TP_ERR_OK; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { return TP_ERR_OK; } - return tp->done; + // Motion is done when queue is empty and filters drained + // checkJointFiltersEmpty() is stubbed to return true (no filters yet) + // Future: properly track joint filter draining + return checkJointFiltersEmpty(tp) && !tcqLen(&tp->queue); } int tpQueueDepth(TP_STRUCT * const tp) @@ -4373,6 +5518,271 @@ int tpIsMoving(TP_STRUCT const * const tp) return false; } +//============================================================================ +// SEGMENT ACTIONS (Inline M-code execution for planner_type 2) +//============================================================================ + +/** + * @brief Queue an action to fire when next segment activates + * + * Actions are attached to the next motion segment added to the queue. + * When that segment activates (starts executing), the actions fire. + * This maintains synchronization without forcing queue drain. + */ +int tpSetSegmentAction(TP_STRUCT * const tp, segment_action_type_t action_type, + int spindle_num, double value) +{ + if (!tp) return TP_ERR_FAIL; + if (action_type <= SEG_ACTION_NONE || action_type >= SEG_ACTION_MAX) { + return TP_ERR_FAIL; + } + + // Set the action bit in the mask + tp->pending_actions.action_mask |= (1u << action_type); + + // Store parameters based on action type + switch (action_type) { + case SEG_ACTION_SPINDLE_CW: + case SEG_ACTION_SPINDLE_CCW: + tp->pending_actions.spindle_num = spindle_num; + tp->pending_actions.spindle_speed = value; + break; + case SEG_ACTION_SPINDLE_OFF: + tp->pending_actions.spindle_num = spindle_num; + break; + case SEG_ACTION_CUSTOM: + tp->pending_actions.custom_value = value; + break; + default: + // Other actions don't need extra parameters + break; + } + + rtapi_print_msg(RTAPI_MSG_DBG, + "tpSetSegmentAction: queued action %d (mask=0x%x)\n", + action_type, tp->pending_actions.action_mask); + + return TP_ERR_OK; +} + +/** + * @brief Clear all pending segment actions + */ +int tpClearSegmentActions(TP_STRUCT * const tp) +{ + if (!tp) return TP_ERR_FAIL; + + tp->pending_actions.action_mask = 0; + tp->pending_actions.spindle_num = 0; + tp->pending_actions.spindle_speed = 0.0; + tp->pending_actions.custom_action_id = 0; + tp->pending_actions.custom_value = 0.0; + + return TP_ERR_OK; +} + +/** + * @brief Fire segment actions when segment activates + * + * Called from tpActivateSegment() to execute queued actions. + * Actions are fired via HAL pins that the motion controller monitors. + * + * For now, we set status flags that task layer can read. + * Future: Direct HAL pin writes for immediate action. + */ +STATIC void tpFireSegmentActions(TC_STRUCT * const tc) +{ + if (!tc) return; + if (tc->actions.action_mask == 0) return; + + unsigned int mask = tc->actions.action_mask; + int spindle_num = tc->actions.spindle_num; + + // Validate spindle number + if (spindle_num < 0 || spindle_num >= emcmotConfig->numSpindles) { + rtapi_print_msg(RTAPI_MSG_ERR, + "tpFireSegmentActions: Invalid spindle %d (max %d)\n", + spindle_num, emcmotConfig->numSpindles); + tc->actions.action_mask = 0; + return; + } + + // Fire spindle ON clockwise (M3) + if (mask & (1u << SEG_ACTION_SPINDLE_CW)) { + double speed = tc->actions.spindle_speed; + rtapi_print_msg(RTAPI_MSG_DBG, + "tpFireSegmentActions: Spindle %d ON CW @ %.1f RPM\n", + spindle_num, speed); + + // Set spindle status - control.c will update HAL pins + emcmotStatus->spindle_status[spindle_num].state = 1; + emcmotStatus->spindle_status[spindle_num].speed = speed; + emcmotStatus->spindle_status[spindle_num].direction = 1; + emcmotStatus->spindle_status[spindle_num].brake = 0; + +#ifndef USERSPACE_LIB_BUILD + // Clear orient flags (RT only - has access to HAL pins) + if (emcmot_hal_data) { + *(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0; + *(emcmot_hal_data->spindle[spindle_num].spindle_locked) = 0; + } +#endif + emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_NONE; + } + + // Fire spindle ON counter-clockwise (M4) + if (mask & (1u << SEG_ACTION_SPINDLE_CCW)) { + double speed = tc->actions.spindle_speed; + rtapi_print_msg(RTAPI_MSG_DBG, + "tpFireSegmentActions: Spindle %d ON CCW @ %.1f RPM\n", + spindle_num, speed); + + // Set spindle status - speed is negative for CCW + emcmotStatus->spindle_status[spindle_num].state = 1; + emcmotStatus->spindle_status[spindle_num].speed = -speed; + emcmotStatus->spindle_status[spindle_num].direction = -1; + emcmotStatus->spindle_status[spindle_num].brake = 0; + +#ifndef USERSPACE_LIB_BUILD + // Clear orient flags (RT only) + if (emcmot_hal_data) { + *(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0; + *(emcmot_hal_data->spindle[spindle_num].spindle_locked) = 0; + } +#endif + emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_NONE; + } + + // Fire spindle OFF (M5) + if (mask & (1u << SEG_ACTION_SPINDLE_OFF)) { + rtapi_print_msg(RTAPI_MSG_DBG, + "tpFireSegmentActions: Spindle %d OFF\n", spindle_num); + + emcmotStatus->spindle_status[spindle_num].state = 0; + emcmotStatus->spindle_status[spindle_num].speed = 0; + emcmotStatus->spindle_status[spindle_num].direction = 0; + emcmotStatus->spindle_status[spindle_num].brake = 1; + +#ifndef USERSPACE_LIB_BUILD + // Clear orient flags (RT only) + if (emcmot_hal_data) { + *(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0; + *(emcmot_hal_data->spindle[spindle_num].spindle_locked) = 0; + } +#endif + emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_NONE; + } + + // Fire mist coolant ON (M7) + // Note: Coolant is typically controlled via IO, not motion. + // For inline sync, we use synched DIO pins. DIO index 0 = mist by convention. + if (mask & (1u << SEG_ACTION_COOLANT_MIST)) { + rtapi_print_msg(RTAPI_MSG_DBG, "tpFireSegmentActions: Mist coolant ON\n"); + // Use synched DIO for coolant - can be mapped in HAL + if (_DioWrite) { + _DioWrite(0, 1); // DIO 0 = mist on (convention) + } + } + + // Fire flood coolant ON (M8) + if (mask & (1u << SEG_ACTION_COOLANT_FLOOD)) { + rtapi_print_msg(RTAPI_MSG_DBG, "tpFireSegmentActions: Flood coolant ON\n"); + if (_DioWrite) { + _DioWrite(1, 1); // DIO 1 = flood on (convention) + } + } + + // Fire coolant OFF (M9) + if (mask & (1u << SEG_ACTION_COOLANT_OFF)) { + rtapi_print_msg(RTAPI_MSG_DBG, "tpFireSegmentActions: Coolant OFF\n"); + if (_DioWrite) { + _DioWrite(0, 0); // mist off + _DioWrite(1, 0); // flood off + } + } + + // Clear the mask after firing + tc->actions.action_mask = 0; +} + +//============================================================================ +// DWELL SEGMENTS (G4 inline with motion) +//============================================================================ + +/** + * @brief Add a dwell segment to the queue (G4) + * + * Creates a zero-length TC_DWELL segment that holds position for the + * specified duration. Processed inline with motion, maintaining sync. + * + * For planner_type 2, this replaces the task-layer EMC_TRAJ_DELAY mechanism + * which forces queue drain and breaks velocity continuity. + */ +int tpAddDwell(TP_STRUCT * const tp, double seconds, struct state_tag_t tag) +{ + if (!tp) return TP_ERR_FAIL; + if (seconds < 0.0) seconds = 0.0; + + TC_STRUCT tc = {0}; + + // Set up dwell segment + tc.motion_type = TC_DWELL; + tc.canon_motion_type = EMC_MOTION_TYPE_FEED; // Treat as feed for status + tc.target = 0.0; // Zero length + tc.progress = 0.0; + tc.dwell_time = seconds; + tc.dwell_remaining = seconds; + + // Copy current position as start/end (no movement) + tc.coords.line.xyz.start = tp->goalPos.tran; + tc.coords.line.xyz.end = tp->goalPos.tran; + tc.coords.line.abc.start = (PmCartesian){tp->goalPos.a, tp->goalPos.b, tp->goalPos.c}; + tc.coords.line.abc.end = tc.coords.line.abc.start; + tc.coords.line.uvw.start = (PmCartesian){tp->goalPos.u, tp->goalPos.v, tp->goalPos.w}; + tc.coords.line.uvw.end = tc.coords.line.uvw.start; + + // Velocity/accel don't matter for dwell, but set reasonable values + tc.reqvel = 0.0; + tc.maxvel = tp->vMax; + tc.maxaccel = tp->aMax; + tc.currentvel = 0.0; + tc.finalvel = 0.0; + + // Terminal condition: must stop (no blending through a dwell) + tc.term_cond = TC_TERM_COND_STOP; + + // Copy state tag + tc.tag = tag; + + // Assign unique ID + tc.id = tp->nextId++; + + // Attach any pending segment actions + tc.actions = tp->pending_actions; + tpClearSegmentActions(tp); + + // Also attach any pending syncdio + if (tp->syncdio.anychanged) { + tc.syncdio = tp->syncdio; + tpClearDIOs(tp); + } + + tc.cycle_time = tp->cycleTime; + + // Add to queue + int retval = tcqPut(&tp->queue, &tc); + if (retval != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpAddDwell: queue full\n"); + return TP_ERR_FAIL; + } + + rtapi_print_msg(RTAPI_MSG_DBG, + "tpAddDwell: added dwell segment id=%d duration=%.3fs\n", + tc.id, seconds); + + return TP_ERR_OK; +} + // api: functions called by motion: EXPORT_SYMBOL(tpMotFunctions); EXPORT_SYMBOL(tpMotData); @@ -4388,6 +5798,7 @@ EXPORT_SYMBOL(tpGetExecId); EXPORT_SYMBOL(tpGetExecTag); EXPORT_SYMBOL(tpGetMotionType); EXPORT_SYMBOL(tpGetPos); +EXPORT_SYMBOL(tpGetJointPositions); EXPORT_SYMBOL(tpIsDone); EXPORT_SYMBOL(tpPause); EXPORT_SYMBOL(tpQueueDepth); @@ -4399,11 +5810,16 @@ EXPORT_SYMBOL(tpSetCycleTime); EXPORT_SYMBOL(tpSetDout); EXPORT_SYMBOL(tpSetId); EXPORT_SYMBOL(tpSetPos); +EXPORT_SYMBOL(tpSyncGoalPos_9D); +EXPORT_SYMBOL(tpCleanupAfterAbort_9D); EXPORT_SYMBOL(tpSetRunDir); EXPORT_SYMBOL(tpSetSpindleSync); EXPORT_SYMBOL(tpSetTermCond); EXPORT_SYMBOL(tpSetVlimit); EXPORT_SYMBOL(tpSetVmax); +EXPORT_SYMBOL(tpAddDwell); +EXPORT_SYMBOL(tpSetSegmentAction); +EXPORT_SYMBOL(tpClearSegmentActions); EXPORT_SYMBOL(tcqFull); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index c8867bf45eb..19b0e852536 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -19,6 +19,14 @@ #include "tp_types.h" #include "tcq.h" +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct emcmot_struct_t; +struct emcmot_internal_t; + // functions not used by motmod: int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); @@ -41,6 +49,8 @@ int tpGetExecId(TP_STRUCT * tp); struct state_tag_t tpGetExecTag(TP_STRUCT * const tp); int tpSetTermCond(TP_STRUCT * tp, int cond, double tolerance); int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos); +int tpSyncGoalPos_9D(TP_STRUCT * tp, EmcPose const * const pos); +int tpCleanupAfterAbort_9D(TP_STRUCT * const tp); int tpRunCycle(TP_STRUCT * tp, long period); int tpPause(TP_STRUCT * tp); int tpResume(TP_STRUCT * tp); @@ -62,6 +72,7 @@ int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, char atspeed, struct state_tag_t tag); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpGetJointPositions(TP_STRUCT const * const tp, double * const joints); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); @@ -71,6 +82,43 @@ int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int wait); int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end); int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end); //gets called to place DIO toggles on the TC queue +/** + * @brief Add a dwell segment to the queue (G4) + * + * Creates a zero-length TC_DWELL segment that holds position for the + * specified duration. The dwell is processed inline with motion, + * maintaining proper synchronization without forcing queue drain. + * + * @param tp Trajectory planner + * @param seconds Dwell duration in seconds + * @param tag State tag for tracking + * @return 0 on success + */ +int tpAddDwell(TP_STRUCT * const tp, double seconds, struct state_tag_t tag); + +/** + * @brief Queue an action to fire when next segment activates + * + * Actions are processed inline with motion (like industrial controllers). + * Multiple actions can be queued before adding a motion segment. + * For planner_type 2, this avoids flush_segments() which breaks velocity continuity. + * + * @param tp Trajectory planner + * @param action_type Type of action (SEG_ACTION_SPINDLE_CW, etc.) + * @param spindle_num Spindle number for spindle actions (0-based) + * @param value Action parameter (speed for spindle, etc.) + * @return 0 on success + */ +int tpSetSegmentAction(TP_STRUCT * const tp, segment_action_type_t action_type, + int spindle_num, double value); + +/** + * @brief Clear pending segment actions + * @param tp Trajectory planner + * @return 0 on success + */ +int tpClearSegmentActions(TP_STRUCT * const tp); + int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir); //--------------------------------------------------------------------- @@ -85,7 +133,13 @@ void tpMotFunctions(void(*pDioWrite)(int,char) void tpMotData(emcmot_status_t * ,emcmot_config_t * + ,struct emcmot_struct_t * + ,struct emcmot_internal_t * ); //--------------------------------------------------------------------- +#ifdef __cplusplus +} +#endif + #endif /* TP_H */ diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 89dd0849055..cec6d8b670b 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -20,6 +20,9 @@ #include +/* Magic number for 9D planner initialization check */ +#define TP_MAGIC 0x54503944 /* "TP9D" in hex */ + #define TP_DEFAULT_QUEUE_SIZE 32 /* Minimum length of a segment in cycles (must be greater than 1 to ensure each * segment is hit at least once.) */ @@ -90,10 +93,14 @@ typedef struct { * Stores persistent data for the trajectory planner that should be accessible * by outside functions. */ -typedef struct { +typedef struct TP_STRUCT { TC_QUEUE_STRUCT queue; tp_spindle_t spindle; //Spindle data + /* 9D planner initialization markers (safety validation) */ + unsigned int magic; /* Set to TP_MAGIC after tpInit() */ + int queue_ready; /* Set to 1 after queue initialization */ + EmcPose currentPos; EmcPose goalPos; @@ -118,11 +125,15 @@ typedef struct { int execId; struct state_tag_t execTag; /* state tag corresponding to running motion */ int termCond; - int done; + int joint_filter_drain_counter; /* Cycles remaining for joint filter draining */ + bool filters_at_rest; /* True when joint smoothing filters have settled */ int depth; /* number of total queued motions */ int activeDepth; /* number of motions blending */ int aborting; int pausing; + int abort_profiles_written; /* Set by userspace after writing abort stop + profiles; prevents feed-override merge from + overwriting them before RT sees tp->aborting */ int reverse_run; /* Indicates that TP is running in reverse */ int motionType; double tolerance; /* for subsequent motions, stay within this @@ -136,6 +147,10 @@ typedef struct { syncdio_t syncdio; //record tpSetDout's here + // Pending segment actions (planner_type 2) + // Actions queued via tpSetSegmentAction() before the next motion segment + segment_actions_t pending_actions; + } TP_STRUCT;