From 55f66599935815f3ea85c2821775af1ed88f3c00 Mon Sep 17 00:00:00 2001 From: wangdequan Date: Sun, 7 Jun 2026 18:56:38 +0800 Subject: [PATCH] =?UTF-8?q?=E6=98=AF=E7=9A=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 结论：已将 LinuxCNC identity/trivial kinematics 源码纳入 WASM port 的 vendored manifest、native probe 和文档验证链，完整 native 验证通过。 --- wasm-port/docs/compatibility-validation.md | 4 + wasm-port/docs/drift-report.md | 6 +- wasm-port/docs/scope-and-baseline.md | 6 +- wasm-port/docs/source-reuse-map.md | 6 +- .../linuxcnc_wrap/linuxcnc_hal_adapter.cpp | 5 + .../linuxcnc_kinematics_probe.cpp | 81 ++++ wasm-port/runtime/core/shims/hal.h | 1 + wasm-port/runtime/core/shims/rtapi.h | 52 +++ .../tests/native/verify_native_probes.sh | 16 + wasm-port/tools/build_native_probes.sh | 41 ++ wasm-port/tools/source-manifest.txt | 3 + .../linuxcnc/src/emc/kinematics/kins_util.c | 366 ++++++++++++++++++ .../linuxcnc/src/emc/kinematics/trivkins.c | 87 +++++ .../vendor/linuxcnc/src/rtapi/rtapi_app.h | 36 ++ 14 files changed, 702 insertions(+), 8 deletions(-) create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_probe.cpp create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/kins_util.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/trivkins.c create mode 100644 wasm-port/vendor/linuxcnc/src/rtapi/rtapi_app.h diff --git a/wasm-port/docs/compatibility-validation.md b/wasm-port/docs/compatibility-validation.md index c5dce04..407355d 100644 --- a/wasm-port/docs/compatibility-validation.md +++ b/wasm-port/docs/compatibility-validation.md @@ -52,6 +52,7 @@ The validation fails if: | `linuxcnc_interp_minimal_harness` | Runs G-code fixtures through vendored LinuxCNC parser/execution/conversion code and captures canonical events. | | `linuxcnc_parameter_file_harness` | Validates LinuxCNC parameter file restore/save behavior and required/read-only parameter handling. | | `linuxcnc_tp_api_probe` | Validates vendored LinuxCNC trajectory planner calls for linear, arc, and queued motion paths. | +| `linuxcnc_kinematics_probe` | Validates vendored LinuxCNC `trivkins.c` plus `kins_util.c` initialize and perform identity forward/inverse mapping through the standalone HAL/RTAPI boundary. | ## Fixture Coverage @@ -93,3 +94,6 @@ machine-session validation remain future work. The current fixture expectations validate standalone behavior against the vendored LinuxCNC source path. They do not yet run a side-by-side native LinuxCNC executable comparison for each fixture. + +The kinematics probe currently covers LinuxCNC identity/trivial kinematics +only. Non-trivial and 5-axis kinematics still need dedicated machine baselines. diff --git a/wasm-port/docs/drift-report.md b/wasm-port/docs/drift-report.md index b74a234..6d4a743 100644 --- a/wasm-port/docs/drift-report.md +++ b/wasm-port/docs/drift-report.md @@ -29,6 +29,7 @@ semantic rewrites: | Python/remap | Python/remap hooks are stubbed at the runtime edge. | | Dynamic interpreter path | `interp_base.cc` probe uses standalone `EMC2_HOME` compile-time path boundary. | | Realtime scheduler | TP probes seed deterministic status/config data instead of running LinuxCNC realtime process topology. | +| Kinematics component lifecycle | `trivkins.c` is initialized through its LinuxCNC module entry point while HAL component init/ready/exit and RTAPI module metadata are handled by standalone shims. | | Browser storage | OPFS remains outside the native core and is not yet connected. | ## Enforced Non-Drift Rules @@ -46,8 +47,9 @@ semantic rewrites: - No browser/WASM parity tests yet. - No JS SDK validation yet. - No OPFS persistence validation yet. -- Full kinematics implementation files are not yet extracted. -- Full 3-axis, non-trivial kinematics, and 5-axis machine baselines are not +- Identity/trivial kinematics now have native source-probe coverage; non-trivial + and 5-axis kinematics implementation files are not yet extracted. +- Full machine baselines beyond the identity/trivial kinematics probe are not established. - Fixture expectations are currently checked against the standalone vendored source path, not by running a native LinuxCNC binary for every fixture. diff --git a/wasm-port/docs/scope-and-baseline.md b/wasm-port/docs/scope-and-baseline.md index 5af7057..cc5afb7 100644 --- a/wasm-port/docs/scope-and-baseline.md +++ b/wasm-port/docs/scope-and-baseline.md @@ -59,6 +59,6 @@ Negative fixture coverage includes: - missing tool - missing tool length offset -Machine baseline is still limited to standalone interpreter and trajectory -planner probes. Full 3-axis, non-trivial kinematics, and 5-axis machine -baselines remain future work. +Machine baseline is still limited to standalone interpreter, trajectory +planner, and LinuxCNC identity/trivial kinematics probes. Non-trivial +kinematics and 5-axis machine baselines remain future work. diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index d7da62c..1a9c5f3 100644 --- a/wasm-port/docs/source-reuse-map.md +++ b/wasm-port/docs/source-reuse-map.md @@ -35,7 +35,7 @@ Current validation is intentionally mechanical: | RTAPI compatibility headers | `src/rtapi/rtapi_*.h` in the manifest | Copy unchanged plus standalone shim include path | `runtime/core/shims/rtapi.h` supplies the minimal standalone RTAPI surface needed by vendored code | Vendor byte sync, compile coverage through dependent source probes | | Canon/NML-facing interpreter types | `src/emc/nml_intf/canon*.hh`, `emctool.h`, `interp_return.hh`, `motion_types.h`, `emcpose.*`, `emcpos.h`, `debugflags.h`, `src/emc/linuxcnc.h` | Copy unchanged | NML transport is not ported; wrappers provide only the status/type edges needed by standalone probes | Vendor byte sync, dependent source probes, `linuxcnc_tp_api_probe`, interpreter harnesses | | Motion state headers | `src/emc/motion/state_tag.h`, `emcmotcfg.h`, `simple_tp.h`, `motion.h`, `mot_priv.h`, `axis.h` | Copy unchanged | Realtime motion process is not ported; standalone probes seed the small motion status/config state required by TP calls | Vendor byte sync, `linuxcnc_tp_api_probe` | -| Kinematics headers | `src/emc/kinematics/kinematics.h`, `cubic.h` | Copy unchanged | Kinematics implementation modules are not yet extracted; current use is type/interface support | Vendor byte sync, dependent compile coverage | +| Identity/trivial kinematics | `src/emc/kinematics/kinematics.h`, `cubic.h`, `kins_util.c`, `trivkins.c` | Copy unchanged | HAL component lifecycle and RTAPI module metadata are replaced by standalone shims; forward/inverse mapping behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_kinematics_probe` | | Trajectory planner | `src/emc/tp/tp.c`, `tc.c`, `tcq.c`, `spherical_arc.c`, `blendmath.c`, `sp_scurve.c`, `ruckig_wrapper.c`, plus matching `*.h` files | Copy unchanged | Native realtime scheduling and motion process state are replaced by standalone probe setup | Vendor byte sync, per-file source probes, `linuxcnc_tp_api_probe` | | Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes | | Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe | @@ -51,7 +51,7 @@ Current validation is intentionally mechanical: | Native file IO | `inifile.cc`, `rs274ngc_pre.cc`, parameter file paths | Allowed in native probes; browser OPFS remains a host-side future adapter | | RTAPI | `rtapi_*.h`, TP, posemath, motion headers | Minimal standalone shim in `runtime/core/shims/rtapi.h` | | NML transport | `emc.hh`, motion/NML type headers | Transport is not ported; only structs/types needed by vendored compute code are used | -| HAL runtime | named parameter lookup and runtime status edges | Standalone HAL adapter under `runtime/core/linuxcnc_wrap/` | +| HAL runtime | named parameter lookup, kinematics component lifecycle, and runtime status edges | Standalone HAL adapter under `runtime/core/linuxcnc_wrap/` | | Python/remap | `rs274ngc_pre.cc`, `interp_o_word.cc`, remap hooks | Stubbed at runtime boundary; no Python CNC semantics are reimplemented | | Canonical machine actions | `interp_convert.cc`, `interp_execute.cc`, `interp_queue.cc` | Captured by standalone canonical event sink functions for regression fixtures | | Realtime scheduling | TP and motion headers | Not ported; native TP probes seed deterministic status/config state | @@ -59,7 +59,7 @@ Current validation is intentionally mechanical: ## Known Gaps -- Full kinematics implementation files are not yet extracted. +- Full non-trivial and 5-axis kinematics implementation files are not yet extracted. - Browser/WASM C ABI and JS SDK layers are not yet built for the full interpreter/planner core. - OPFS persistence is not yet connected to INI, tool table, parameter file, or diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp index e90e7ba..70af334 100644 --- a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp @@ -91,6 +91,11 @@ int hal_ready(int) return 0; } +int hal_exit(int) +{ + return 0; +} + int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr, bool *connected) { diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_probe.cpp new file mode 100644 index 0000000..2396a09 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_probe.cpp @@ -0,0 +1,81 @@ +#include + +#include "emc/kinematics/kinematics.h" +#include "emc/motion/emcmotcfg.h" + +int rtapi_app_main(void); +void rtapi_app_exit(void); + +namespace { + +void print_pose(const char *prefix, const EmcPose &pose) +{ + std::cout << prefix << "_xyz=" + << pose.tran.x << "," + << pose.tran.y << "," + << pose.tran.z << "\n"; + std::cout << prefix << "_abcuvw=" + << pose.a << "," + << pose.b << "," + << pose.c << "," + << pose.u << "," + << pose.v << "," + << pose.w << "\n"; +} + +void print_joints(const char *prefix, const double *joints) +{ + std::cout << prefix << "_xyz=" + << joints[0] << "," + << joints[1] << "," + << joints[2] << "\n"; + std::cout << prefix << "_abcuvw=" + << joints[3] << "," + << joints[4] << "," + << joints[5] << "," + << joints[6] << "," + << joints[7] << "," + << joints[8] << "\n"; +} + +} // namespace + +int main() +{ + const int init_rc = rtapi_app_main(); + std::cout << "kinematics_init=" << init_rc << "\n"; + std::cout << "kinematics_type=" << kinematicsType() << "\n"; + std::cout << "kinematics_switchable=" << kinematicsSwitchable() << "\n"; + std::cout << "kinematics_switch=" << kinematicsSwitch(0) << "\n"; + + double joints[EMCMOT_MAX_JOINTS]{}; + for (int index = 0; index < 9; ++index) { + joints[index] = index + 1; + } + + EmcPose forward_pose{}; + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags); + std::cout << "kinematics_forward=" << forward_rc << "\n"; + print_pose("kinematics_forward", forward_pose); + + EmcPose inverse_pose{}; + inverse_pose.tran.x = 10.0; + inverse_pose.tran.y = 20.0; + inverse_pose.tran.z = 30.0; + inverse_pose.a = 40.0; + inverse_pose.b = 50.0; + inverse_pose.c = 60.0; + inverse_pose.u = 70.0; + inverse_pose.v = 80.0; + inverse_pose.w = 90.0; + + double inverse_joints[EMCMOT_MAX_JOINTS]{}; + const int inverse_rc = kinematicsInverse(&inverse_pose, inverse_joints, &iflags, &fflags); + std::cout << "kinematics_inverse=" << inverse_rc << "\n"; + print_joints("kinematics_inverse", inverse_joints); + + rtapi_app_exit(); + return 0; +} diff --git a/wasm-port/runtime/core/shims/hal.h b/wasm-port/runtime/core/shims/hal.h index a4ac8b2..110e4e2 100644 --- a/wasm-port/runtime/core/shims/hal.h +++ b/wasm-port/runtime/core/shims/hal.h @@ -32,6 +32,7 @@ typedef unsigned long long hal_u64_t; int hal_init(const char *); int hal_ready(int); +int hal_exit(int); int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *); int hal_get_signal_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *); int hal_get_param_value_by_name(const char *, hal_type_t *, hal_data_u **); diff --git a/wasm-port/runtime/core/shims/rtapi.h b/wasm-port/runtime/core/shims/rtapi.h index 000c17f..9b053be 100644 --- a/wasm-port/runtime/core/shims/rtapi.h +++ b/wasm-port/runtime/core/shims/rtapi.h @@ -30,6 +30,58 @@ #define EXPORT_SYMBOL(symbol) #endif +#ifndef EXPORT_SYMBOL_GPL +#define EXPORT_SYMBOL_GPL(symbol) +#endif + +#ifndef MODULE_LICENSE +#define MODULE_LICENSE(value) +#endif + +#ifndef MODULE_AUTHOR +#define MODULE_AUTHOR(value) +#endif + +#ifndef MODULE_DESCRIPTION +#define MODULE_DESCRIPTION(value) +#endif + +#ifndef MODULE_SUPPORTED_DEVICE +#define MODULE_SUPPORTED_DEVICE(value) +#endif + +#ifndef MODULE_DEVICE_TABLE +#define MODULE_DEVICE_TABLE(type, name) +#endif + +#ifndef MODULE_INFO +#define MODULE_INFO(tag, value) +#endif + +#ifndef RTAPI_MP_INT +#define RTAPI_MP_INT(var, descr) +#endif + +#ifndef RTAPI_MP_LONG +#define RTAPI_MP_LONG(var, descr) +#endif + +#ifndef RTAPI_MP_STRING +#define RTAPI_MP_STRING(var, descr) +#endif + +#ifndef RTAPI_MP_ARRAY_INT +#define RTAPI_MP_ARRAY_INT(var, num, descr) +#endif + +#ifndef RTAPI_MP_ARRAY_LONG +#define RTAPI_MP_ARRAY_LONG(var, num, descr) +#endif + +#ifndef RTAPI_MP_ARRAY_STRING +#define RTAPI_MP_ARRAY_STRING(var, num, descr) +#endif + typedef enum { RTAPI_MSG_NONE = 0, RTAPI_MSG_ERR, diff --git a/wasm-port/tests/native/verify_native_probes.sh b/wasm-port/tests/native/verify_native_probes.sh index 2f716dc..9028b85 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -79,6 +79,8 @@ check_exitcode linuxcnc_interp_state_probe check_exitcode linuxcnc_inifile_source_probe check_exitcode linuxcnc_tp_api_probe check_exitcode linuxcnc_tp_api_probe.run +check_exitcode linuxcnc_kinematics_probe +check_exitcode linuxcnc_kinematics_probe.run for name in \ linuxcnc_tp_tp_source_probe \ linuxcnc_tp_tc_source_probe \ @@ -119,6 +121,8 @@ check_exitcode linuxcnc_interp_minimal_harness.run check_exitcode linuxcnc_parameter_file_harness check_exitcode linuxcnc_parameter_file_harness.run check_exitcode linuxcnc_rs274_compile_probe +check_exitcode linuxcnc_kins_util_source_probe +check_exitcode linuxcnc_trivkins_source_probe check_exitcode linuxcnc_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe @@ -202,6 +206,18 @@ grep -Fq "tp_done_after_queued_lines=1" "$TP_API_STDOUT" grep -Fq "tp_queue_final_depth=0" "$TP_API_STDOUT" grep -Fq "tp_queue_final_pos_near_end=1" "$TP_API_STDOUT" +KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_kinematics_probe.run.stdout.log" +grep -Fq "kinematics_init=0" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_type=1" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_switchable=0" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_switch=0" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_forward=0" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_forward_xyz=1,2,3" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_forward_abcuvw=4,5,6,7,8,9" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_inverse=0" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_inverse_xyz=10,20,30" "$KINEMATICS_STDOUT" +grep -Fq "kinematics_inverse_abcuvw=40,50,60,70,80,90" "$KINEMATICS_STDOUT" + check_fixture_output() { local fixture="$1" local expected="$2" diff --git a/wasm-port/tools/build_native_probes.sh b/wasm-port/tools/build_native_probes.sh index f4b2cf5..34dc52b 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -346,6 +346,13 @@ TP_API_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_tp_api_probe.cpp" ) +KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/kins_util.c" + "$VENDOR_DIR/src/emc/kinematics/trivkins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_kinematics_probe.cpp" +) + TP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/tp/tp.c" "$VENDOR_DIR/src/emc/tp/tc.c" @@ -519,6 +526,28 @@ build_binary_target \ TP_RUNTIME_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_kinematics_probe \ + "$BUILD_DIR/linuxcnc_kinematics_probe" \ + TP_FLAGS \ + KINEMATICS_PROBE_SOURCES \ + NO_LINK_FLAGS + +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_kinematics_probe.run.stderr.log" + KINEMATICS_RUN_RC=$? + set -e + echo "$KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_kinematics_probe.run.stderr.log" +fi + if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then set +e "$BUILD_DIR/linuxcnc_tp_api_probe" \ @@ -617,6 +646,18 @@ build_object_target \ "$WRAP_DIR/linuxcnc_rs274_compile_probe.cpp" \ COMMON_FLAGS +build_object_target \ + linuxcnc_kins_util_source_probe \ + "$BUILD_DIR/linuxcnc_kins_util_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/kins_util.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_trivkins_source_probe \ + "$BUILD_DIR/linuxcnc_trivkins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/trivkins.c" \ + TP_FLAGS + build_object_target \ linuxcnc_interp_convert_source_probe \ "$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \ diff --git a/wasm-port/tools/source-manifest.txt b/wasm-port/tools/source-manifest.txt index c1e496c..440c666 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -10,6 +10,7 @@ src/rtapi/rtapi_string.h src/rtapi/rtapi_gfp.h src/rtapi/rtapi_math.h src/rtapi/rtapi_byteorder.h +src/rtapi/rtapi_app.h src/emc/nml_intf/emcpos.h src/emc/nml_intf/emcpose.h src/emc/nml_intf/emcpose.c @@ -29,6 +30,8 @@ src/emc/motion/mot_priv.h src/emc/motion/axis.h src/emc/kinematics/kinematics.h src/emc/kinematics/cubic.h +src/emc/kinematics/kins_util.c +src/emc/kinematics/trivkins.c src/emc/tp/tp.h src/emc/tp/tp_types.h src/emc/tp/tc.h diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/kins_util.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/kins_util.c new file mode 100644 index 0000000..c82a4a2 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/kins_util.c @@ -0,0 +1,366 @@ +/* Utility routines for kinematics modules +** License GPL Version 2 +** +** utilities for use with switchkins.c +**--------------------------------------------------------------------- +** identityKinematicsSetup() +** identityKinematicsForward() +** identityKinematicsInverse() +** +** Routines for identity kinematics using mapping created by +** map_coordinates_to_jnumbers() +** +**--------------------------------------------------------------------- +** map_coordinates_to_jnumbers() +** +** Map a string of coordinate letters to joint numbers sequentially. +** If allow_duplicates==1, a coordinate letter may be specified more +** than once to assign it to multiple joint numbers (the kinematics +** module must support such usage). +** +** Default mapping if coordinates==NULL is: +** X:0 Y:1 Z:2 A:3 B:4 C:5 U:6 V:7 W:8 +** +** Example coordinates-to-joints mappings: +** coordinates=XYZ X:0 Y:1 Z:2 +** coordinates=ZYX Z:0 Y:1 X:2 +** coordinates=XYZZZZ x:0 Y:1 Z:2,3,4,5 +** coordinates=XXYZ X:0,1 Y:2 Z:3 +**--------------------------------------------------------------------- +** +** mapped_joints_to_position() +** +** Update position based mapping created by map_coordinates_to_jnumbers() +** (used for identity-based forward kinematics) +**--------------------------------------------------------------------- +** +** position_to_mapped_joints() +** +** Update joints (including joints for duplicate letters) +** based on mapping created by map_coordinates_to_jnumbers() +** (used for identity-based inverse kinematics) +** +**--------------------------------------------------------------------- +*/ + +#include +#include +#include +#include +#include + +// principal joint numbers based on module 'coordinates' parameter +static int JX = -1; +static int JY = -1; +static int JZ = -1; +static int JA = -1; +static int JB = -1; +static int JC = -1; +static int JU = -1; +static int JV = -1; +static int JW = -1; + +// bitmaps indicate joints used for each axis letter +static int X_joints_bitmap; +static int Y_joints_bitmap; +static int Z_joints_bitmap; +static int A_joints_bitmap; +static int B_joints_bitmap; +static int C_joints_bitmap; +static int U_joints_bitmap; +static int V_joints_bitmap; +static int W_joints_bitmap; + +static int map_initialized = 0; +#define MAX_COORDINATES_CHARS 32 +static char used_coordinates[MAX_COORDINATES_CHARS+1]; + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + bool found=0; + int dups[EMCMOT_MAX_AXIS]; + const char *coords = coordinates; + char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; + int i; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + for (i=0; i EMCMOT_MAX_JOINTS) ) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s bogus max_joints=%d\n", + errtag,max_joints); + return -1; + } + + // init all axis_idx_for_jno[] (-1 means unspecified) + for(jno=0; jno max_joints) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s too many coordinates <%s> for max_joints=%d\n", + errtag,coordinates,max_joints); + return -1; + } + } // while + + if (!found) { + rtapi_print_msg(RTAPI_MSG_ERR,"%s missing coordinates '%s'\n", + errtag,coordinates); + return -1; + } + if (!allow_duplicates) { + int ano; + for(ano=0; ano 1) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s duplicates not allowed in coordinates=%s, letter=%c\n", + errtag,coordinates,coord_letter[ano]); + return -1; + } + } + } + + for (jno=0; jno < max_joints; jno++) { + int bitnumber = 1<tran.x = joints[JX]; + if ( bit & Y_joints_bitmap ) pos->tran.y = joints[JY]; + if ( bit & Z_joints_bitmap ) pos->tran.z = joints[JZ]; + if ( bit & A_joints_bitmap ) pos->a = joints[JA]; + if ( bit & B_joints_bitmap ) pos->b = joints[JB]; + if ( bit & C_joints_bitmap ) pos->c = joints[JC]; + if ( bit & U_joints_bitmap ) pos->u = joints[JU]; + if ( bit & V_joints_bitmap ) pos->v = joints[JV]; + if ( bit & W_joints_bitmap ) pos->w = joints[JW]; + } + return 0; +} // mapped_joints_to_position() + +int position_to_mapped_joints(const int max_joints, + const EmcPose * pos, + double* joints) +{ + int jno; + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "position_to_mapped_joints before map_initialized\n"); + return -1; + } + for (jno=0; jno < max_joints; jno++) { + int bit = 1<tran.x; + if ( bit & Y_joints_bitmap ) joints[jno] = pos->tran.y; + if ( bit & Z_joints_bitmap ) joints[jno] = pos->tran.z; + if ( bit & A_joints_bitmap ) joints[jno] = pos->a; + if ( bit & B_joints_bitmap ) joints[jno] = pos->b; + if ( bit & C_joints_bitmap ) joints[jno] = pos->c; + if ( bit & U_joints_bitmap ) joints[jno] = pos->u; + if ( bit & V_joints_bitmap ) joints[jno] = pos->v; + if ( bit & W_joints_bitmap ) joints[jno] = pos->w; + } + return 0; +} // position_to_mapped_joints() + +static int identity_kinematics_initialized = 0; +static int identity_max_joints; + +int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)comp_id; + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno; + int show=0; + bool islathe; + + identity_max_joints = strlen(coordinates); + + if (map_coordinates_to_jnumbers(coordinates, + kp->max_joints, + kp->allow_duplicates, + axis_idx_for_jno)) { + return -1; //mapping failed + } + + /* print message for unconventional ordering; + ** a) duplicate coordinate letters + ** b) letters not ordered by "XYZABCUVW" sequence + ** (use kinstype=both works best for these) + */ + for (jno=0; jno Axis %c\n", + jno,*(p+axis_idx_for_jno[jno])); + } + if (kinematicsType() != KINEMATICS_BOTH) { + rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); + } + + identity_kinematics_initialized = 1; + return 0; +} // identityKinematicsSetup() + +int identityKinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsForward: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + mapped_joints_to_position(identity_max_joints,joints,pos); + return 0; +} // identityKinematicsForward() + +int identityKinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + if (!identity_kinematics_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "identityKinematicsInverse: not initialized\n"); + return -1; + } + + // support multiple-joint-per-coordinate-letter assignments: + position_to_mapped_joints(identity_max_joints,pos,joints); + + return 0; +} // identityKinematicsInverse() diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/trivkins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/trivkins.c new file mode 100644 index 0000000..3ea56b4 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/trivkins.c @@ -0,0 +1,87 @@ +/******************************************************************** +* Description: trivkins.c +* general trivkins for 3 axis Cartesian machine +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* License: GPL Version 2 +* +* Copyright (c) 2009 All rights reserved. +* +********************************************************************/ + +#include +#include /* RTAPI realtime OS API */ +#include /* RTAPI realtime module decls */ +#include +#include +#include +#include +#include + + +#define SET(f) pos->f = joints[i] + +int kinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + return identityKinematicsForward(joints, pos, fflags, iflags); +} + +int kinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + return identityKinematicsInverse(pos, joints, iflags, fflags); +} + +static KINEMATICS_TYPE ktype = -1; + +KINEMATICS_TYPE kinematicsType() +{ + return ktype; +} + +#define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" +static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; +RTAPI_MP_STRING(coordinates, "Existing Axes"); + +static char *kinstype = "1"; // use KINEMATICS_IDENTITY +RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +MODULE_LICENSE("GPL"); + +static int comp_id; + +int rtapi_app_main(void) { + kparms ksetup; + + switch (*kinstype) { + case 'b': case 'B': ktype = KINEMATICS_BOTH; break; + case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break; + case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break; + case '1': default: ktype = KINEMATICS_IDENTITY; + } + + comp_id = hal_init("trivkins"); + if(comp_id < 0) return comp_id; + + // see typedef for KS KinematicsSETUP: + ksetup.max_joints = EMCMOT_MAX_JOINTS; + ksetup.allow_duplicates = 1; + if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { + return -1; //setup failed + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_app.h b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_app.h new file mode 100644 index 0000000..a14578b --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_app.h @@ -0,0 +1,36 @@ +// Copyright 2003-2007, various authors +// +// 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. +#ifndef __LINUXCNC_RTAPI_APP_H +#define __LINUXCNC_RTAPI_APP_H + +/* + for Linux kernel modules, exactly one file needs to + include . We put this in this header. + If we ever support non-Linux platforms, this file will + get full of ifdefs. +*/ + +#if !defined(__KERNEL__) +EXPORT_SYMBOL(rtapi_app_main); +EXPORT_SYMBOL(rtapi_app_exit); +#else +#include + +#define rtapi_app_main(a) init_module(a) +#define rtapi_app_exit(a) cleanup_module(a) +#endif + +#endif /* RTAPI_APP_H */