wangdequan/linuxcncWebWASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Run XYZAC through upstream LinuxCNC TP

Browse Source
This commit is contained in:
wangdequan
2026-09-01 11:44:05 +08:00
parent bbdfad8364
commit d18141ec81
23 changed files with 1550 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
wasm-core/CMakeLists.txt
View File

@@ -6,7 +6,7 @@ set(CMAKE_C_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_linuxcnc_tp.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
target_include_directories(sim_core PUBLIC include)
target_compile_definitions(sim_core PRIVATE SIM_API_BUILD=1)
@@ -26,6 +26,43 @@ target_compile_options(sim_posemath PRIVATE -ffp-contract=off -fno-fast-math)
target_link_libraries(sim_core PUBLIC sim_posemath)
target_compile_options(sim_core PRIVATE -ffp-contract=off -fno-fast-math)
# Build the upstream LinuxCNC TP/TC implementation as a source-owned slice.
# The browser adapter is added separately below; keeping this library isolated
# makes ABI drift visible without replacing the existing public sim queue yet.
add_library(linuxcnc_tp_source STATIC
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tp.c
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tc.c
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/tcq.c
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/blendmath.c
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp/spherical_arc.c
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf/emcpose.c
src/linuxcnc_tp_compat.c
)
target_include_directories(linuxcnc_tp_source PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal
)
target_compile_definitions(linuxcnc_tp_source PRIVATE ULAPI)
target_compile_options(linuxcnc_tp_source PRIVATE -ffp-contract=off -fno-fast-math "-DEXPORT_SYMBOL(x)=")
target_link_libraries(linuxcnc_tp_source PUBLIC sim_posemath)
target_include_directories(sim_core PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal
)
target_link_libraries(sim_core PUBLIC linuxcnc_tp_source)
if(NOT EMSCRIPTEN)
add_executable(sim_api_test tests/sim_api_test.c)
target_link_libraries(sim_api_test PRIVATE sim_core)
@@ -150,6 +187,12 @@ if(NOT EMSCRIPTEN)
target_include_directories(sim_xyzac_trace_test PRIVATE include)
target_compile_options(sim_xyzac_trace_test PRIVATE -UNDEBUG)
add_test(NAME sim_xyzac_trace_test COMMAND sim_xyzac_trace_test)
add_executable(sim_linuxcnc_tp_test tests/sim_linuxcnc_tp_test.c src/sim_linuxcnc_tp.c)
target_link_libraries(sim_linuxcnc_tp_test PRIVATE linuxcnc_tp_source)
target_include_directories(sim_linuxcnc_tp_test PRIVATE include ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal ${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src)
target_compile_options(sim_linuxcnc_tp_test PRIVATE -UNDEBUG)
add_test(NAME sim_linuxcnc_tp_test COMMAND sim_linuxcnc_tp_test)
endif()
if(EMSCRIPTEN)
@@ -231,15 +274,20 @@ if(EMSCRIPTEN)
target_compile_options(sim_xyzac_trace_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_xyzac_trace_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_task.c src/sim_scheduler.c src/sim_hal.c src/sim_device_io.c src/sim_posemath.c src/sim_interpreter.c src/sim_program.c src/sim_motion_queue.c src/sim_linuxcnc_tp.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
target_include_directories(sim_core_module PRIVATE
include
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/tp
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/motion
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/nml_intf
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/emc/kinematics
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/hal
)
target_compile_options(sim_core_module PRIVATE -ffp-contract=off -fno-fast-math)
target_link_libraries(sim_core_module PRIVATE sim_posemath)
target_link_libraries(sim_core_module PRIVATE sim_posemath linuxcnc_tp_source)
set_target_properties(sim_core_module PROPERTIES
OUTPUT_NAME "linuxcnc-sim-core"
SUFFIX ".js"
@@ -248,6 +296,9 @@ if(EMSCRIPTEN)
get_target_property(_sim_module_link_flags sim_core_module LINK_FLAGS)
string(REPLACE "_sim_task_recover','_sim_task_restore" "_sim_task_recover','_sim_task_command_gate','_sim_task_restore" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_interpreter_size" "_sim_program_size','_sim_program_requires_subroutines','_sim_program_init','_sim_program_reset','_sim_program_destroy','_sim_program_load','_sim_program_add_subroutine','_sim_program_set_rapid_velocity','_sim_program_start','_sim_program_pause','_sim_program_resume','_sim_program_abort','_sim_program_step','_sim_program_execute_block','_sim_program_seek','_sim_program_read_motion','_sim_program_clear_motion','_sim_program_canonical','_sim_program_state_hash','_sim_interpreter_size" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_kinematics_switch_select','_sim_kinematics_switch_forward" "_sim_kinematics_switch_select','_sim_kinematics_switch_select_physical','_sim_kinematics_switch_forward" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_kinematics_switch_inverse','_sim_kinematics_switch_active" "_sim_kinematics_switch_inverse','_sim_kinematics_switch_inverse_mode','_sim_kinematics_switch_active" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_motion_init" "_sim_motion_set_profile','_sim_motion_set_lookahead','_sim_motion_set_dynamics','_sim_motion_set_feed_override','_sim_motion_set_paused','_sim_motion_peek','_sim_motion_init" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_hal_set_pin" "_sim_hal_set_pin_scalar','_sim_hal_set_pin" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_hal_set_param" "_sim_hal_set_param_scalar','_sim_hal_set_param" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_io_init" "_sim_io_size','_sim_io_init" _sim_module_link_flags "${_sim_module_link_flags}")
@@ -255,5 +306,6 @@ if(EMSCRIPTEN)
string(REPLACE "_sim_device_init" "_sim_device_io_size','_sim_device_init" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_motion_step','_sim_motion_read_state" "_sim_motion_step','_sim_motion_set_profile','_sim_motion_set_lookahead','_sim_motion_set_feed_override','_sim_motion_peek','_sim_motion_read_state" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_motion_state_hash','_sim_motion_canonical" "_sim_motion_state_hash','_sim_motion_plan_hash','_sim_motion_canonical" _sim_module_link_flags "${_sim_module_link_flags}")
string(REPLACE "_sim_motion_init" "_sim_motion_init','_sim_linuxcnc_tp_size','_sim_linuxcnc_tp_init','_sim_linuxcnc_tp_reset','_sim_linuxcnc_tp_set_profile','_sim_linuxcnc_tp_set_feed_override','_sim_linuxcnc_tp_set_dynamics','_sim_linuxcnc_tp_set_paused','_sim_linuxcnc_tp_abort','_sim_linuxcnc_tp_set_position','_sim_linuxcnc_tp_set_goal_position','_sim_linuxcnc_tp_add_line','_sim_linuxcnc_tp_add_circle','_sim_linuxcnc_tp_step','_sim_linuxcnc_tp_idle','_sim_linuxcnc_tp_read_state','_sim_linuxcnc_tp_queue_depth','_sim_linuxcnc_tp_queue_segment','_sim_linuxcnc_tp_restore_progress','_sim_linuxcnc_tp_state_hash','_sim_linuxcnc_tp_canonical" _sim_module_link_flags "${_sim_module_link_flags}")
set_target_properties(sim_core_module PROPERTIES LINK_FLAGS "${_sim_module_link_flags}")
endif()

8
wasm-core/include/sim_kinematics.h
View File

@@ -260,6 +260,10 @@ int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info);
int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode,
const double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info);
int sim_kinematics_switch_forward(sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
@@ -268,6 +272,10 @@ int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info);
int sim_kinematics_switch_inverse_mode(const sim_kinematics_switch *switcher, int mode,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info);
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher);
uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,

52
wasm-core/include/sim_linuxcnc_tp.h Normal file
View File

@@ -0,0 +1,52 @@
#ifndef LINUXCNC_WEB_WASM_SIM_LINUXCNC_TP_H
#define LINUXCNC_WEB_WASM_SIM_LINUXCNC_TP_H
#include <stdint.h>
#include "sim_posemath.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct sim_linuxcnc_tp sim_linuxcnc_tp;
typedef struct sim_linuxcnc_tp_segment {
sim_posemath_segment segment;
uint32_t id;
int term_cond;
double progress;
double target;
} sim_linuxcnc_tp_segment;
uint32_t sim_linuxcnc_tp_size(void);
int32_t sim_linuxcnc_tp_init(sim_linuxcnc_tp *planner, double cycle_time,
double velocity_limit, double acceleration_limit);
int32_t sim_linuxcnc_tp_reset(sim_linuxcnc_tp *planner);
int32_t sim_linuxcnc_tp_set_profile(sim_linuxcnc_tp *planner, int profile, double tolerance);
int32_t sim_linuxcnc_tp_set_feed_override(sim_linuxcnc_tp *planner, double scale);
int32_t sim_linuxcnc_tp_set_dynamics(sim_linuxcnc_tp *planner, double acceleration_limit);
int32_t sim_linuxcnc_tp_set_paused(sim_linuxcnc_tp *planner, int paused);
int32_t sim_linuxcnc_tp_abort(sim_linuxcnc_tp *planner);
int32_t sim_linuxcnc_tp_set_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position);
int32_t sim_linuxcnc_tp_set_goal_position(sim_linuxcnc_tp *planner, const sim_posemath_pose *position);
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);
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);
int32_t sim_linuxcnc_tp_step(sim_linuxcnc_tp *planner);
int32_t sim_linuxcnc_tp_idle(sim_linuxcnc_tp *planner);
int32_t sim_linuxcnc_tp_read_state(const sim_linuxcnc_tp *planner, sim_tp_state *state);
uint32_t sim_linuxcnc_tp_queue_depth(const sim_linuxcnc_tp *planner);
int32_t sim_linuxcnc_tp_queue_segment(const sim_linuxcnc_tp *planner, uint32_t index, sim_linuxcnc_tp_segment *segment);
int32_t sim_linuxcnc_tp_restore_progress(sim_linuxcnc_tp *planner, const sim_posemath_pose *position, double velocity, uint32_t tick);
uint64_t sim_linuxcnc_tp_state_hash(const sim_linuxcnc_tp *planner);
uint32_t sim_linuxcnc_tp_canonical(const sim_linuxcnc_tp *planner, char *output, uint32_t capacity);
#ifdef __cplusplus
}
#endif
#endif

14
wasm-core/src/linuxcnc_tp_compat.c Normal file
View File

@@ -0,0 +1,14 @@
#include <stdarg.h>
#include "rtapi.h"
/* LinuxCNC's TP diagnostics are intentionally silent in the browser. The
* source TP itself remains unchanged; only its realtime logging sink is
* replaced for deterministic WASM builds. */
void rtapi_print(const char *format, ...) {
(void)format;
}
void rtapi_print_msg(msg_level_t level, const char *format, ...) {
(void)level;
(void)format;
}

336
wasm-core/src/sim_linuxcnc_tp.c Normal file
View File

@@ -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;
}

31
wasm-core/src/sim_switchkins.c
View File

@@ -140,6 +140,16 @@ int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
return result;
}
int sim_kinematics_switch_inverse_mode(const sim_kinematics_switch *switcher, int mode,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info) {
if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
return sim_kinematics_inverse_ex(switcher->modules[mode], pose, joint_count, 0, joints, info);
}
int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
@@ -161,6 +171,27 @@ int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
return result;
}
int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode,
const double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
int result;
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
switcher->active_mode = mode;
switcher->pending_mode = mode;
switcher->switching = 0;
switcher->generation += 1;
switcher->switch_count += 1;
result = sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
if (result != SIM_KIN_OK) return result;
switcher->last_pose[mode] = *pose;
switcher->have_last_pose[mode] = 1;
return SIM_KIN_OK;
}
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher) {
return switcher == 0 ? SIM_KIN_ERR_ARGUMENT : switcher->active_mode;
}

39
wasm-core/tests/sim_linuxcnc_tp_test.c Normal file
View File

@@ -0,0 +1,39 @@
#include "sim_linuxcnc_tp.h"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
static sim_posemath_pose pose(double x, double y, double z) {
sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
return value;
}
int main(void) {
sim_linuxcnc_tp *planner = calloc(1, sim_linuxcnc_tp_size());
sim_tp_state state = {0};
char canonical[512] = {0};
sim_posemath_pose origin = pose(0, 0, 0);
sim_posemath_pose first = pose(0, 0, 10);
sim_posemath_pose second = pose(20, 20, 10);
assert(planner != NULL);
assert(sim_linuxcnc_tp_init(planner, 0.001, 35.0, 300.0) == 0);
assert(sim_linuxcnc_tp_set_position(planner, &origin) == 0);
assert(sim_linuxcnc_tp_set_profile(planner, 0, 0.0) == 0);
assert(sim_linuxcnc_tp_add_line(planner, &first, 20.0, 300.0, 16, 1) == 0);
assert(sim_linuxcnc_tp_add_line(planner, &second, 35.0, 300.0, 18, 1) == 0);
for (int index = 0; index < 5000; ++index) {
int result = sim_linuxcnc_tp_step(planner);
if (result != 0) fprintf(stderr, "tp_step[%d]=%d\\n", index, result);
assert(result == 0);
assert(sim_linuxcnc_tp_read_state(planner, &state) == 0);
if (state.queue_depth == 0) break;
}
assert(state.queue_depth == 0);
assert(state.position.tran.x > 19.9 && state.position.tran.y > 19.9 && state.position.tran.z > 9.9);
assert(sim_linuxcnc_tp_state_hash(planner) != 0);
assert(sim_linuxcnc_tp_canonical(planner, canonical, sizeof(canonical)) > 1);
puts(canonical);
free(planner);
return 0;
}