Run XYZAC through upstream LinuxCNC TP

wasm-core/src/sim_linuxcnc_tp.c

@@ -0,0 +1,336 @@
+#include "sim_linuxcnc_tp.h"
+
+#include "motion.h"
+#include "tp.h"
+#include "emcpose.h"
+#include "motion_types.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <string.h>
+
+extern int tcGetPosReal(TC_STRUCT const *tc, int of_point, EmcPose *pos);
+
+struct sim_linuxcnc_tp {
+    TP_STRUCT tp;
+    emcmot_status_t status;
+    emcmot_config_t config;
+    double axis_velocity[9];
+    double axis_acceleration[9];
+    uint32_t tick;
+    uint32_t last_active_id;
+    int just_completed;
+    int initialized;
+};
+
+static sim_linuxcnc_tp *active_planner;
+
+static void noop_dio(int index, char value) { (void)index; (void)value; }
+static void noop_aio(int index, double value) { (void)index; (void)value; }
+static void noop_rotary_unlock(int index, int value) { (void)index; (void)value; }
+static int noop_rotary_locked(int index) { (void)index; return 0; }
+static double axis_velocity(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_velocity[axis] : 0.0; }
+static double axis_acceleration(int axis) { return active_planner && axis >= 0 && axis < 9 ? active_planner->axis_acceleration[axis] : 0.0; }
+
+static double pose_delta(const EmcPose *start, const EmcPose *end, int axis) {
+    const double values_start[9] = {start->tran.x, start->tran.y, start->tran.z, start->a, start->b, start->c, start->u, start->v, start->w};
+    const double values_end[9] = {end->tran.x, end->tran.y, end->tran.z, end->a, end->b, end->c, end->u, end->v, end->w};
+    return values_end[axis] - values_start[axis];
+}
+
+static void xyzac_line_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, double *velocity, double *acceleration) {
+    double linear_length = 0.0, angular_length = 0.0, max_velocity_time = 0.0, max_acceleration_time = 0.0;
+    int has_linear = 0, has_angular = 0;
+    for (int axis = 0; axis < 9; ++axis) {
+        const double delta = fabs(pose_delta(start, end, axis));
+        const double velocity_limit = planner->axis_velocity[axis];
+        const double acceleration_limit = planner->axis_acceleration[axis];
+        if (delta <= 1e-12) continue;
+        if (axis < 3 || axis >= 6) { has_linear = 1; linear_length += delta * delta; }
+        else { has_angular = 1; angular_length += delta * delta; }
+        if (velocity_limit > 0.0) max_velocity_time = fmax(max_velocity_time, delta / velocity_limit);
+        if (acceleration_limit > 0.0) max_acceleration_time = fmax(max_acceleration_time, delta / acceleration_limit);
+    }
+    const double distance = has_linear ? sqrt(linear_length) : has_angular ? sqrt(angular_length) : 0.0;
+    if (!(distance > 0.0)) return;
+    if (max_velocity_time > 0.0) *velocity = fmin(*velocity, distance / max_velocity_time);
+    if (max_acceleration_time > 0.0) *acceleration = fmax(*acceleration, distance / max_acceleration_time);
+}
+
+static void xyzac_circle_limits(const sim_linuxcnc_tp *planner, const EmcPose *start, const EmcPose *end, const PmCartesian *center, const PmCartesian *normal, int turn, double *velocity, double *initial_velocity, double *acceleration) {
+    const double sx = start->tran.x - center->x, sy = start->tran.y - center->y, sz = start->tran.z - center->z;
+    const double ex = end->tran.x - center->x, ey = end->tran.y - center->y, ez = end->tran.z - center->z;
+    // ARC_FEED measures radius in the active plane.  For the XYZAC case the
+    // normal is +Z, so the axial helix delta must not become a radial spiral
+    // term (the previous 3-D radius caused a small but accumulating TP error).
+    const double normal_abs_x = fabs(normal->x), normal_abs_y = fabs(normal->y), normal_abs_z = fabs(normal->z);
+    const double start_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(sx, sy) : normal_abs_y >= normal_abs_x ? hypot(sx, sz) : hypot(sy, sz);
+    const double end_radius = normal_abs_z >= fmax(normal_abs_x, normal_abs_y) ? hypot(ex, ey) : normal_abs_y >= normal_abs_x ? hypot(ex, ez) : hypot(ey, ez);
+    const double dot = sx * ex + sy * ey + sz * ez;
+    const double cross_x = sy * ez - sz * ey, cross_y = sz * ex - sx * ez, cross_z = sx * ey - sy * ex;
+    double angle = fabs(atan2(normal->x * cross_x + normal->y * cross_y + normal->z * cross_z, dot));
+    if (angle <= 1e-12) angle = 2.0 * M_PI;
+    angle += 2.0 * M_PI * (fabs((double)turn) - 1.0);
+    const double spiral = end_radius - start_radius;
+    const double circular_length = fmin(start_radius, end_radius) * fabs(angle);
+    const double spiral_length = hypot(circular_length, spiral);
+    const double effective_radius = sqrt((spiral / fmax(fabs(angle), 1e-12)) * (spiral / fmax(fabs(angle), 1e-12)) + fmin(start_radius, end_radius) * fmin(start_radius, end_radius));
+    const double planar_velocity = fmin(sqrt(planner->axis_acceleration[0] * sqrt(3.0) * 0.5 * effective_radius), fmin(planner->axis_velocity[0], fmin(planner->axis_velocity[1], planner->axis_velocity[2])));
+    double total_xyz_length = hypot(spiral_length, normal->x * (end->tran.x - start->tran.x) + normal->y * (end->tran.y - start->tran.y) + normal->z * (end->tran.z - start->tran.z));
+    double time_motion_velocity = 0.0, time_motion_acceleration = 0.0;
+    for (int axis = 0; axis < 9; ++axis) {
+        const double delta = fabs(pose_delta(start, end, axis));
+        if (planner->axis_velocity[axis] > 0.0) time_motion_velocity = fmax(time_motion_velocity, delta / planner->axis_velocity[axis]);
+        if (planner->axis_acceleration[axis] > 0.0) time_motion_acceleration = fmax(time_motion_acceleration, delta / planner->axis_acceleration[axis]);
+    }
+    const double time_spiral_velocity = planar_velocity > 0.0 ? spiral_length / planar_velocity : INFINITY;
+    const double time_spiral_acceleration = planner->axis_acceleration[0] > 0.0 ? spiral_length / planner->axis_acceleration[0] : INFINITY;
+    const double max_velocity = total_xyz_length / fmax(time_motion_velocity, time_spiral_velocity);
+    const double max_acceleration = total_xyz_length / fmax(time_motion_acceleration, time_spiral_acceleration);
+    *velocity = fmin(*velocity, max_velocity);
+    *initial_velocity = max_velocity;
+    *acceleration = fmax(*acceleration, max_acceleration);
+}
+
+static EmcPose to_emc_pose(const sim_posemath_pose *value) {
+    EmcPose result = {0};
+    if (value != NULL) {
+        result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
+        result.a = value->a; result.b = value->b; result.c = value->c;
+        result.u = value->u; result.v = value->v; result.w = value->w;
+    }
+    return result;
+}
+
+static PmCartesian to_pm_cart(const sim_posemath_cartesian *value) {
+    PmCartesian result = {0, 0, 0};
+    if (value != NULL) { result.x = value->x; result.y = value->y; result.z = value->z; }
+    return result;
+}
+
+static sim_posemath_pose from_emc_pose(const EmcPose *value) {
+    sim_posemath_pose result = {{0, 0, 0}, 0, 0, 0, 0, 0, 0};
+    if (value != NULL) {
+        result.tran.x = value->tran.x; result.tran.y = value->tran.y; result.tran.z = value->tran.z;
+        result.a = value->a; result.b = value->b; result.c = value->c;
+        result.u = value->u; result.v = value->v; result.w = value->w;
+    }
+    return result;
+}
+
+static uint64_t hash_bytes(const unsigned char *bytes, size_t length) {
+    uint64_t hash = UINT64_C(1469598103934665603);
+    for (size_t index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
+    return hash;
+}
+
+uint32_t sim_linuxcnc_tp_size(void) { return (uint32_t)sizeof(sim_linuxcnc_tp); }
+
+int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time, double velocity_limit, double acceleration_limit) {
+    if (planner == NULL || !isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 || !isfinite(velocity_limit) || velocity_limit <= 0.0 || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1200;
+    memset(planner, 0, sizeof(*planner));
+    planner->config.numJoints = 5; planner->config.numExtraJoints = 0; planner->config.numSpindles = 1;
+    planner->config.numDIO = 0; planner->config.numAIO = 0; planner->config.numMiscError = 0;
+    planner->config.trajCycleTime = cycle_time; planner->config.servoCycleTime = cycle_time;
+    planner->config.limitVel = velocity_limit; planner->config.maxFeedScale = 2.0;
+    planner->config.arcBlendEnable = 1; planner->config.arcBlendFallbackEnable = 0;
+    planner->config.arcBlendOptDepth = 50; planner->config.arcBlendGapCycles = 4;
+    planner->config.arcBlendRampFreq = 100.0; planner->config.arcBlendTangentKinkRatio = 0.1;
+    // Match xyzac-trt.ini joint limits: the trajectory cap is 35 mm/s, but
+    // XYZ joints are limited to 20 and rotary A/C joints to 30 deg/s.  The
+    // upstream TP uses these per-axis callbacks when calculating tangent
+    // velocity and acceleration at a corner.
+    for (int axis = 0; axis < 9; ++axis) {
+        planner->axis_velocity[axis] = axis < 3 ? fmin(20.0, velocity_limit) : (axis == 3 || axis == 5) ? fmin(30.0, velocity_limit) : velocity_limit;
+        planner->axis_acceleration[axis] = acceleration_limit;
+    }
+    planner->status.feed_scale = 1.0; planner->status.rapid_scale = 1.0; planner->status.net_feed_scale = 1.0;
+    planner->status.enables_new = FS_ENABLED | FH_ENABLED; planner->status.enables_queued = planner->status.enables_new;
+    planner->status.motionFlag = EMCMOT_MOTION_ENABLE_BIT | EMCMOT_MOTION_COORD_BIT | EMCMOT_MOTION_INPOS_BIT;
+    active_planner = planner;
+    tpMotData(&planner->status, &planner->config);
+    tpMotFunctions(noop_dio, noop_aio, noop_rotary_unlock, noop_rotary_locked, axis_velocity, axis_acceleration);
+    if (tpCreate(&planner->tp, TP_DEFAULT_QUEUE_SIZE, 0) != TP_ERR_OK) return -1201;
+    if (tpSetCycleTime(&planner->tp, cycle_time) != TP_ERR_OK || tpSetVlimit(&planner->tp, velocity_limit) != TP_ERR_OK || tpSetAmax(&planner->tp, acceleration_limit) != TP_ERR_OK) return -1202;
+    planner->initialized = 1;
+    return 0;
+}
+
+int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner) {
+    if (planner == NULL || !planner->initialized) return -1203;
+    active_planner = planner;
+    planner->last_active_id = 0;
+    planner->just_completed = 0;
+    return tpClear(&planner->tp) == TP_ERR_OK ? 0 : -1204;
+}
+
+int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance) {
+    if (planner == NULL || !planner->initialized || profile < 0 || profile > 2 || !isfinite(tolerance) || tolerance < 0.0) return -1205;
+    active_planner = planner;
+    return tpSetTermCond(&planner->tp, profile, tolerance) == TP_ERR_OK ? 0 : -1206;
+}
+
+int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale) {
+    if (planner == NULL || !planner->initialized || !isfinite(scale) || scale < 0.0 || scale > 2.0) return -1207;
+    planner->status.net_feed_scale = scale; planner->status.feed_scale = scale; return 0;
+}
+
+int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit) {
+    if (planner == NULL || !planner->initialized || !isfinite(acceleration_limit) || acceleration_limit <= 0.0) return -1217;
+    active_planner = planner;
+    for (int axis = 0; axis < 9; ++axis) planner->axis_acceleration[axis] = acceleration_limit;
+    return tpSetAmax(&planner->tp, acceleration_limit) == TP_ERR_OK ? 0 : -1218;
+}
+
+int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused) {
+    if (planner == NULL || !planner->initialized || (paused != 0 && paused != 1)) return -1219;
+    active_planner = planner;
+    if (paused) return tpPause(&planner->tp) == TP_ERR_OK ? 0 : -1220;
+    return tpResume(&planner->tp) == TP_ERR_OK ? 0 : -1221;
+}
+
+int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner) {
+    if (planner == NULL || !planner->initialized) return -1222;
+    active_planner = planner;
+    planner->last_active_id = 0;
+    planner->just_completed = 0;
+    return tpAbort(&planner->tp) == TP_ERR_OK ? 0 : -1223;
+}
+
+int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
+    if (planner == NULL || !planner->initialized || position == NULL) return -1208;
+    active_planner = planner;
+    EmcPose pose = to_emc_pose(position);
+    return tpSetPos(&planner->tp, &pose) == TP_ERR_OK ? 0 : -1209;
+}
+
+int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position) {
+    if (planner == NULL || !planner->initialized || position == NULL) return -1228;
+    planner->tp.goalPos = to_emc_pose(position);
+    return 0;
+}
+
+int32_t sim_linuxcnc_tp_add_line(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
+    if (planner == NULL || !planner->initialized || end == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1210;
+    active_planner = planner;
+    struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
+    EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos;
+    xyzac_line_limits(planner, &start, &pose, &velocity, &acceleration);
+    if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1211;
+    return tpAddLine(&planner->tp, pose, canon_motion_type, velocity, velocity, acceleration, planner->status.enables_new, 0, -1, state_tag) == TP_ERR_OK ? 0 : -1211;
+}
+
+int32_t sim_linuxcnc_tp_add_circle(sim_linuxcnc_tp *planner, const sim_posemath_pose *end, const sim_posemath_cartesian *center, const sim_posemath_cartesian *normal, int turn, double velocity, double acceleration, uint32_t tag, int canon_motion_type) {
+    if (planner == NULL || !planner->initialized || end == NULL || center == NULL || normal == NULL || !isfinite(velocity) || velocity < 0.0 || !isfinite(acceleration) || acceleration < 0.0) return -1212;
+    active_planner = planner;
+    struct state_tag_t state_tag = {0}; state_tag.fields_float[GM_FIELD_FLOAT_LINE_NUMBER] = (float)tag;
+    EmcPose pose = to_emc_pose(end), start = planner->tp.goalPos; PmCartesian pm_center = to_pm_cart(center); PmCartesian pm_normal = to_pm_cart(normal);
+    double initial_velocity = velocity;
+    xyzac_circle_limits(planner, &start, &pose, &pm_center, &pm_normal, turn, &velocity, &initial_velocity, &acceleration);
+    if (tpSetId(&planner->tp, (int)tag) != TP_ERR_OK) return -1212;
+    // emccanon computes ini_maxvel independently of the programmed feed for
+    // ARC_FEED.  For this XYZAC case the planar XYZ limit (20 mm/s) is the
+    // governing bound; passing the feed here loses the native corner ramp.
+    return tpAddCircle(&planner->tp, pose, pm_center, pm_normal, turn, canon_motion_type, velocity, initial_velocity, acceleration, planner->status.enables_new, 0, state_tag) == TP_ERR_OK ? 0 : -1213;
+}
+
+int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner) {
+    if (planner == NULL || !planner->initialized) return -1214;
+    active_planner = planner;
+    planner->just_completed = 0;
+    const int prior_id = tpGetExecId(&planner->tp);
+    const int prior_depth = tcqLen(&planner->tp.queue);
+    const int result = tpRunCycle(&planner->tp, (long)llround(planner->tp.cycleTime * 1e9));
+    planner->tick += 1u;
+    if (prior_id > 0 && prior_depth > 0 && tcqLen(&planner->tp.queue) == 0) {
+        planner->last_active_id = (uint32_t)prior_id;
+        planner->just_completed = 1;
+    }
+    return result >= TP_ERR_OK && result <= TP_ERR_WAITING ? 0 : result;
+}
+
+int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner) {
+    if (planner == NULL || !planner->initialized) return -1227;
+    planner->just_completed = 0;
+    planner->tick += 1u;
+    return 0;
+}
+
+int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state) {
+    if (planner == NULL || !planner->initialized || state == NULL) return -1215;
+    EmcPose position = {0};
+    if (tpGetPos(&planner->tp, &position) != TP_ERR_OK) return -1216;
+    memset(state, 0, sizeof(*state)); state->position = from_emc_pose(&position); state->velocity = planner->status.current_vel;
+    TC_STRUCT *active = tcqItem(&planner->tp.queue, 0);
+    if (active != NULL && active->target > 0.0) state->progress = active->progress / active->target;
+    // tpQueueDepth() exposes TP's legacy cached depth, which is only updated
+    // when appending a TC.  Native emcmot reports the live tcq length after a
+    // segment pop, so use the upstream queue directly for oracle parity.
+    state->tick = planner->tick;
+    const uint32_t live_depth = (uint32_t)tcqLen(&planner->tp.queue);
+    state->queue_depth = live_depth > 0u ? live_depth : planner->just_completed ? 1u : 0u;
+    {
+        const int exec_id = tpGetExecId((TP_STRUCT *)&planner->tp);
+        state->active_id = state->queue_depth > 0 && exec_id > 0 ? (uint32_t)exec_id : planner->just_completed ? planner->last_active_id : 0u;
+    }
+    state->motion_type = tpGetMotionType((TP_STRUCT *)&planner->tp);
+    state->status = tpIsDone((TP_STRUCT *)&planner->tp) ? 0 : 1; return 0;
+}
+
+uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner) {
+    return planner == NULL || !planner->initialized ? 0u : (uint32_t)tcqLen(&planner->tp.queue);
+}
+
+int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment) {
+    TC_STRUCT *tc;
+    EmcPose start = {0}, end = {0};
+    if (planner == NULL || !planner->initialized || segment == NULL) return -1224;
+    tc = tcqItem(&planner->tp.queue, (int)index);
+    if (tc == NULL || tcGetPosReal(tc, TC_GET_STARTPOINT, &start) != TP_ERR_OK || tcGetPosReal(tc, TC_GET_ENDPOINT, &end) != TP_ERR_OK) return -1225;
+    memset(segment, 0, sizeof(*segment));
+    segment->segment.start = from_emc_pose(&start);
+    segment->segment.end = from_emc_pose(&end);
+    segment->segment.velocity = tc->reqvel;
+    segment->segment.acceleration = tc->maxaccel;
+    segment->segment.motion_type = tc->motion_type;
+    if (tc->motion_type == TC_CIRCULAR) {
+        const double turns = fabs(tc->coords.circle.xyz.angle) / (2.0 * M_PI);
+        segment->segment.turn = (tc->coords.circle.xyz.angle < 0.0 ? -1 : 1) * (turns >= 1.0 ? (int)llround(turns) : 1);
+    }
+    if (tc->motion_type == TC_CIRCULAR) {
+        segment->segment.center.x = tc->coords.circle.xyz.center.x;
+        segment->segment.center.y = tc->coords.circle.xyz.center.y;
+        segment->segment.center.z = tc->coords.circle.xyz.center.z;
+        segment->segment.normal.x = tc->coords.circle.xyz.normal.x;
+        segment->segment.normal.y = tc->coords.circle.xyz.normal.y;
+        segment->segment.normal.z = tc->coords.circle.xyz.normal.z;
+    } else {
+        segment->segment.normal.z = 1.0;
+    }
+    segment->id = tc->id;
+    segment->term_cond = tc->term_cond;
+    segment->progress = tc->progress;
+    segment->target = tc->target;
+    return 0;
+}
+
+int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick) {
+    if (planner == NULL || !planner->initialized || position == NULL || !isfinite(velocity) || velocity < 0.0) return -1226;
+    active_planner = planner;
+    planner->tp.currentPos = to_emc_pose(position);
+    planner->status.current_vel = velocity;
+    planner->tick = tick;
+    return 0;
+}
+
+uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner) {
+    sim_tp_state state; if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0; return hash_bytes((const unsigned char *)&state, sizeof(state));
+}
+
+uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity) {
+    sim_tp_state state; int written;
+    if (sim_linuxcnc_tp_read_state(planner, &state) != 0) return 0;
+    written = snprintf(output, capacity, "{\"active_id\":%u,\"motion_type\":%d,\"position\":{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"queue_depth\":%u,\"schema\":\"linuxcnc-source-tp.v1\",\"tick\":%u,\"velocity\":%.17g}", state.active_id, state.motion_type, state.position.a, state.position.c, state.position.tran.x, state.position.tran.y, state.position.tran.z, state.queue_depth, state.tick, state.velocity);
+    return written < 0 ? 0 : (uint32_t)written + 1u;
+}