继续完成 web-rtcp-5axis-sim-plan
结论:完成 LinuxCNC kinematics WASM ABI 覆盖,并将 web-rtcp-5axis-sim-plan 的 RTCP frame/boundary adapter 接到 xyzac-trt kinematics SDK;Node、build、browser smoke 验证通过。
This commit is contained in:
358
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_wasm.c
Normal file
358
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_kinematics_wasm.c
Normal file
@@ -0,0 +1,358 @@
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <emscripten/emscripten.h>
|
||||
|
||||
#include "emc/kinematics/kinematics.h"
|
||||
#include "emc/motion/emcmotcfg.h"
|
||||
|
||||
int rtapi_app_main(void);
|
||||
void rtapi_app_exit(void);
|
||||
|
||||
static int initialized = 0;
|
||||
|
||||
static char *copy_string(const char *value)
|
||||
{
|
||||
size_t length = strlen(value);
|
||||
char *out = malloc(length + 1);
|
||||
if (!out) {
|
||||
return NULL;
|
||||
}
|
||||
memcpy(out, value, length + 1);
|
||||
return out;
|
||||
}
|
||||
|
||||
static int near_value(double actual, double expected)
|
||||
{
|
||||
return fabs(actual - expected) < 1e-9;
|
||||
}
|
||||
|
||||
static int near_joints(const double *actual, const double *expected, int count)
|
||||
{
|
||||
int index;
|
||||
for (index = 0; index < count; ++index) {
|
||||
if (!near_value(actual[index], expected[index])) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int ensure_initialized(void)
|
||||
{
|
||||
if (initialized) {
|
||||
return 0;
|
||||
}
|
||||
int rc = rtapi_app_main();
|
||||
initialized = rc == 0;
|
||||
return rc;
|
||||
}
|
||||
|
||||
static void pose_from_values(
|
||||
EmcPose *pose,
|
||||
double x,
|
||||
double y,
|
||||
double z,
|
||||
double a,
|
||||
double b,
|
||||
double c,
|
||||
double u,
|
||||
double v,
|
||||
double w)
|
||||
{
|
||||
memset(pose, 0, sizeof(*pose));
|
||||
pose->tran.x = x;
|
||||
pose->tran.y = y;
|
||||
pose->tran.z = z;
|
||||
pose->a = a;
|
||||
pose->b = b;
|
||||
pose->c = c;
|
||||
pose->u = u;
|
||||
pose->v = v;
|
||||
pose->w = w;
|
||||
}
|
||||
|
||||
static void append_text(char *out, size_t size, size_t *offset, const char *text)
|
||||
{
|
||||
int written;
|
||||
if (*offset >= size) {
|
||||
return;
|
||||
}
|
||||
written = snprintf(out + *offset, size - *offset, "%s", text);
|
||||
if (written > 0) {
|
||||
*offset += (size_t)written;
|
||||
}
|
||||
}
|
||||
|
||||
static void append_pose(char *out, size_t size, size_t *offset, const char *prefix, const EmcPose *pose)
|
||||
{
|
||||
char line[512];
|
||||
snprintf(line, sizeof(line),
|
||||
"%s_xyz=%.12g,%.12g,%.12g\n"
|
||||
"%s_abcuvw=%.12g,%.12g,%.12g,%.12g,%.12g,%.12g\n",
|
||||
prefix,
|
||||
pose->tran.x,
|
||||
pose->tran.y,
|
||||
pose->tran.z,
|
||||
prefix,
|
||||
pose->a,
|
||||
pose->b,
|
||||
pose->c,
|
||||
pose->u,
|
||||
pose->v,
|
||||
pose->w);
|
||||
append_text(out, size, offset, line);
|
||||
}
|
||||
|
||||
static void append_joints(char *out, size_t size, size_t *offset, const char *prefix, const double *joints, int count)
|
||||
{
|
||||
int index;
|
||||
char line[512];
|
||||
int written = snprintf(line, sizeof(line), "%s_joints=", prefix);
|
||||
size_t line_offset = written > 0 ? (size_t)written : 0;
|
||||
for (index = 0; index < count && line_offset < sizeof(line); ++index) {
|
||||
written = snprintf(
|
||||
line + line_offset,
|
||||
sizeof(line) - line_offset,
|
||||
"%s%.12g",
|
||||
index ? "," : "",
|
||||
joints[index]);
|
||||
if (written > 0) {
|
||||
line_offset += (size_t)written;
|
||||
}
|
||||
}
|
||||
append_text(line, sizeof(line), &line_offset, "\n");
|
||||
append_text(out, size, offset, line);
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_init(void)
|
||||
{
|
||||
return ensure_initialized();
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
void lckins_exit(void)
|
||||
{
|
||||
if (initialized) {
|
||||
rtapi_app_exit();
|
||||
initialized = 0;
|
||||
}
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_type(void)
|
||||
{
|
||||
if (ensure_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
return kinematicsType();
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_switchable(void)
|
||||
{
|
||||
if (ensure_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
return kinematicsSwitchable();
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_switch(int switchkins_type)
|
||||
{
|
||||
if (ensure_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
return kinematicsSwitch(switchkins_type);
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_forward(
|
||||
const double *joints,
|
||||
int joint_count,
|
||||
double *pose_out,
|
||||
unsigned long *fflags_out,
|
||||
unsigned long *iflags_out)
|
||||
{
|
||||
double local_joints[EMCMOT_MAX_JOINTS];
|
||||
KINEMATICS_FORWARD_FLAGS fflags = fflags_out ? *fflags_out : 0;
|
||||
KINEMATICS_INVERSE_FLAGS iflags = iflags_out ? *iflags_out : 0;
|
||||
EmcPose pose;
|
||||
int index;
|
||||
int rc;
|
||||
|
||||
if (!joints || !pose_out || joint_count < 0 || joint_count > EMCMOT_MAX_JOINTS) {
|
||||
return -1;
|
||||
}
|
||||
if (ensure_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
memset(local_joints, 0, sizeof(local_joints));
|
||||
for (index = 0; index < joint_count; ++index) {
|
||||
local_joints[index] = joints[index];
|
||||
}
|
||||
|
||||
pose_from_values(
|
||||
&pose,
|
||||
pose_out[0],
|
||||
pose_out[1],
|
||||
pose_out[2],
|
||||
pose_out[3],
|
||||
pose_out[4],
|
||||
pose_out[5],
|
||||
pose_out[6],
|
||||
pose_out[7],
|
||||
pose_out[8]);
|
||||
rc = kinematicsForward(local_joints, &pose, &fflags, &iflags);
|
||||
if (fflags_out) {
|
||||
*fflags_out = fflags;
|
||||
}
|
||||
if (iflags_out) {
|
||||
*iflags_out = iflags;
|
||||
}
|
||||
if (rc != 0) {
|
||||
return rc;
|
||||
}
|
||||
|
||||
pose_out[0] = pose.tran.x;
|
||||
pose_out[1] = pose.tran.y;
|
||||
pose_out[2] = pose.tran.z;
|
||||
pose_out[3] = pose.a;
|
||||
pose_out[4] = pose.b;
|
||||
pose_out[5] = pose.c;
|
||||
pose_out[6] = pose.u;
|
||||
pose_out[7] = pose.v;
|
||||
pose_out[8] = pose.w;
|
||||
return rc;
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
int lckins_inverse(
|
||||
const double *pose_values,
|
||||
double *joints_out,
|
||||
int joint_count,
|
||||
unsigned long *iflags_out,
|
||||
unsigned long *fflags_out)
|
||||
{
|
||||
EmcPose pose;
|
||||
double local_joints[EMCMOT_MAX_JOINTS];
|
||||
KINEMATICS_INVERSE_FLAGS iflags = iflags_out ? *iflags_out : 0;
|
||||
KINEMATICS_FORWARD_FLAGS fflags = fflags_out ? *fflags_out : 0;
|
||||
int index;
|
||||
int rc;
|
||||
|
||||
if (!pose_values || !joints_out || joint_count < 0 || joint_count > EMCMOT_MAX_JOINTS) {
|
||||
return -1;
|
||||
}
|
||||
if (ensure_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
pose_from_values(
|
||||
&pose,
|
||||
pose_values[0],
|
||||
pose_values[1],
|
||||
pose_values[2],
|
||||
pose_values[3],
|
||||
pose_values[4],
|
||||
pose_values[5],
|
||||
pose_values[6],
|
||||
pose_values[7],
|
||||
pose_values[8]);
|
||||
memset(local_joints, 0, sizeof(local_joints));
|
||||
for (index = 0; index < joint_count; ++index) {
|
||||
local_joints[index] = joints_out[index];
|
||||
}
|
||||
rc = kinematicsInverse(&pose, local_joints, &iflags, &fflags);
|
||||
if (iflags_out) {
|
||||
*iflags_out = iflags;
|
||||
}
|
||||
if (fflags_out) {
|
||||
*fflags_out = fflags;
|
||||
}
|
||||
if (rc != 0) {
|
||||
return rc;
|
||||
}
|
||||
|
||||
for (index = 0; index < joint_count; ++index) {
|
||||
joints_out[index] = local_joints[index];
|
||||
}
|
||||
return rc;
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
char *lckins_run_probe(void)
|
||||
{
|
||||
char out[4096];
|
||||
char line[512];
|
||||
size_t offset = 0;
|
||||
int init_rc = ensure_initialized();
|
||||
double joints[EMCMOT_MAX_JOINTS];
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS];
|
||||
KINEMATICS_FORWARD_FLAGS fflags = 0;
|
||||
KINEMATICS_INVERSE_FLAGS iflags = 0;
|
||||
EmcPose forward_pose;
|
||||
EmcPose identity_pose;
|
||||
int forward_rc;
|
||||
int inverse_rc;
|
||||
|
||||
memset(out, 0, sizeof(out));
|
||||
snprintf(line, sizeof(line), "kinematics_init=%d\n", init_rc);
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
if (init_rc != 0) {
|
||||
return copy_string(out);
|
||||
}
|
||||
|
||||
snprintf(line, sizeof(line),
|
||||
"kinematics_type=%d\n"
|
||||
"kinematics_switchable=%d\n",
|
||||
kinematicsType(),
|
||||
kinematicsSwitchable());
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
|
||||
memset(joints, 0, sizeof(joints));
|
||||
joints[0] = 10.0;
|
||||
joints[1] = 20.0;
|
||||
joints[2] = 30.0;
|
||||
joints[3] = 25.0;
|
||||
joints[4] = 40.0;
|
||||
|
||||
memset(&forward_pose, 0, sizeof(forward_pose));
|
||||
forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags);
|
||||
snprintf(line, sizeof(line), "kinematics_forward=%d\n", forward_rc);
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
append_pose(out, sizeof(out), &offset, "kinematics_forward", &forward_pose);
|
||||
|
||||
for (int index = 0; index < EMCMOT_MAX_JOINTS; ++index) {
|
||||
inverse_joints[index] = joints[index];
|
||||
}
|
||||
inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
snprintf(line, sizeof(line), "kinematics_inverse=%d\n", inverse_rc);
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
append_joints(out, sizeof(out), &offset, "kinematics_inverse", inverse_joints, 5);
|
||||
snprintf(line, sizeof(line), "kinematics_roundtrip_joints=%d\n", near_joints(inverse_joints, joints, 5));
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
|
||||
if (kinematicsSwitchable()) {
|
||||
int switch_rc = kinematicsSwitch(1);
|
||||
snprintf(line, sizeof(line), "kinematics_switch_identity=%d\n", switch_rc);
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
memset(&identity_pose, 0, sizeof(identity_pose));
|
||||
forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
|
||||
snprintf(line, sizeof(line), "kinematics_identity_forward=%d\n", forward_rc);
|
||||
append_text(out, sizeof(out), &offset, line);
|
||||
append_pose(out, sizeof(out), &offset, "kinematics_identity", &identity_pose);
|
||||
}
|
||||
|
||||
return copy_string(out);
|
||||
}
|
||||
|
||||
EMSCRIPTEN_KEEPALIVE
|
||||
void lckins_free_string(char *value)
|
||||
{
|
||||
free(value);
|
||||
}
|
||||
@@ -41,6 +41,8 @@ typedef unsigned int hal_u32_t;
|
||||
typedef long long hal_s64_t;
|
||||
typedef unsigned long long hal_u64_t;
|
||||
|
||||
RTAPI_BEGIN_DECLS
|
||||
|
||||
int hal_init(const char *);
|
||||
int hal_ready(int);
|
||||
int hal_exit(int);
|
||||
@@ -61,3 +63,5 @@ int hal_param_float_newf(hal_pin_dir_t, hal_float_t *, int, const char *, ...);
|
||||
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 **);
|
||||
|
||||
RTAPI_END_DECLS
|
||||
|
||||
@@ -118,6 +118,9 @@ import {
|
||||
createVirtualHalWasmBridgeSnapshot,
|
||||
executeVirtualHalCommand,
|
||||
executeVirtualHalcmd,
|
||||
createLinuxCncKinematicsSdk,
|
||||
linuxCncKinematicsWasmFile,
|
||||
supportedLinuxCncKinematicsModules,
|
||||
readVirtualHalPin,
|
||||
stepVirtualHalMotion,
|
||||
stepVirtualHalMotionController,
|
||||
@@ -221,6 +224,27 @@ import {
|
||||
LinuxCNC `inifile.cc` and exposes `getString()` plus `getBool()`. Boolean
|
||||
conversion is performed by vendored LinuxCNC `iniFindBool()`.
|
||||
|
||||
`createLinuxCncKinematicsSdk()` wraps standalone kinematics WASM modules built
|
||||
from vendored LinuxCNC `src/emc/kinematics` sources and exposes:
|
||||
|
||||
- `type()`
|
||||
- `switchable()`
|
||||
- `switchKinematics(switchkinsType)`
|
||||
- `forward(joints, { seedPose })`
|
||||
- `inverse(pose, jointCount, { seedJoints })`
|
||||
- `runProbe()`
|
||||
|
||||
The current standalone kinematics WASM modules are `trivkins`, `5axiskins`,
|
||||
`xyzac-trt`, `xyzbc-trt`, `corexy`, `rotate`, `rose`, `max`, `lineardelta`,
|
||||
`rotarydelta`, `scorbot`, `tripod`, `scara`, `puma`, `genser`, `genhex`, and
|
||||
`pentakins`. Use `supportedLinuxCncKinematicsModules()` to list supported
|
||||
module IDs and `linuxCncKinematicsWasmFile(moduleId)` to resolve the generated
|
||||
WASM filename. The optional seed buffers mirror LinuxCNC's in/out kinematics
|
||||
function contracts for iterative algorithms such as generic serial and
|
||||
hexapod/pentapod kinematics. The SDK only manages memory and calls the
|
||||
exported C ABI; forward/inverse math remains in vendored LinuxCNC kinematics
|
||||
source.
|
||||
|
||||
`createLinuxCncInterpSdk()` wraps the interpreter-core module built from
|
||||
vendored LinuxCNC RS274NGC sources and exposes:
|
||||
|
||||
|
||||
@@ -1,5 +1,10 @@
|
||||
export { createLinuxCncIniSdk } from "./linuxcnc-ini.js";
|
||||
export { createLinuxCncInterpSdk } from "./linuxcnc-interp.js";
|
||||
export {
|
||||
createLinuxCncKinematicsSdk,
|
||||
linuxCncKinematicsWasmFile,
|
||||
supportedLinuxCncKinematicsModules,
|
||||
} from "./linuxcnc-kinematics.js";
|
||||
export {
|
||||
VIRTUAL_HAL_AXES,
|
||||
VIRTUAL_HAL_AXISUI_PINS,
|
||||
|
||||
206
wasm-port/runtime/sdk/src/linuxcnc-kinematics.js
Normal file
206
wasm-port/runtime/sdk/src/linuxcnc-kinematics.js
Normal file
@@ -0,0 +1,206 @@
|
||||
import createLinuxCncTrivkinsKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_trivkins_kinematics.js";
|
||||
import createLinuxCnc5axiskinsKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_5axiskins_kinematics.js";
|
||||
import createLinuxCncXyzacTrtKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_xyzac_trt_kinematics.js";
|
||||
import createLinuxCncXyzbcTrtKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_xyzbc_trt_kinematics.js";
|
||||
import createLinuxCncCorexyKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_corexy_kinematics.js";
|
||||
import createLinuxCncRotateKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_rotate_kinematics.js";
|
||||
import createLinuxCncRoseKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_rose_kinematics.js";
|
||||
import createLinuxCncMaxKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_max_kinematics.js";
|
||||
import createLinuxCncLineardeltaKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_lineardelta_kinematics.js";
|
||||
import createLinuxCncRotarydeltaKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_rotarydelta_kinematics.js";
|
||||
import createLinuxCncScorbotKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_scorbot_kinematics.js";
|
||||
import createLinuxCncTripodKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_tripod_kinematics.js";
|
||||
import createLinuxCncScaraKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_scara_kinematics.js";
|
||||
import createLinuxCncPumaKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_puma_kinematics.js";
|
||||
import createLinuxCncGenserKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_genser_kinematics.js";
|
||||
import createLinuxCncGenhexKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_genhex_kinematics.js";
|
||||
import createLinuxCncPentakinsKinematicsModule from "../../../build/wasm/kinematics/linuxcnc_pentakins_kinematics.js";
|
||||
|
||||
export const LINUXCNC_KINEMATICS_MODULES = [
|
||||
{ id: "trivkins", wasmFile: "linuxcnc_trivkins_kinematics.wasm", factory: createLinuxCncTrivkinsKinematicsModule },
|
||||
{ id: "5axiskins", wasmFile: "linuxcnc_5axiskins_kinematics.wasm", factory: createLinuxCnc5axiskinsKinematicsModule },
|
||||
{ id: "xyzac-trt", wasmFile: "linuxcnc_xyzac_trt_kinematics.wasm", factory: createLinuxCncXyzacTrtKinematicsModule },
|
||||
{ id: "xyzbc-trt", wasmFile: "linuxcnc_xyzbc_trt_kinematics.wasm", factory: createLinuxCncXyzbcTrtKinematicsModule },
|
||||
{ id: "corexy", wasmFile: "linuxcnc_corexy_kinematics.wasm", factory: createLinuxCncCorexyKinematicsModule },
|
||||
{ id: "rotate", wasmFile: "linuxcnc_rotate_kinematics.wasm", factory: createLinuxCncRotateKinematicsModule },
|
||||
{ id: "rose", wasmFile: "linuxcnc_rose_kinematics.wasm", factory: createLinuxCncRoseKinematicsModule },
|
||||
{ id: "max", wasmFile: "linuxcnc_max_kinematics.wasm", factory: createLinuxCncMaxKinematicsModule },
|
||||
{ id: "lineardelta", wasmFile: "linuxcnc_lineardelta_kinematics.wasm", factory: createLinuxCncLineardeltaKinematicsModule },
|
||||
{ id: "rotarydelta", wasmFile: "linuxcnc_rotarydelta_kinematics.wasm", factory: createLinuxCncRotarydeltaKinematicsModule },
|
||||
{ id: "scorbot", wasmFile: "linuxcnc_scorbot_kinematics.wasm", factory: createLinuxCncScorbotKinematicsModule },
|
||||
{ id: "tripod", wasmFile: "linuxcnc_tripod_kinematics.wasm", factory: createLinuxCncTripodKinematicsModule },
|
||||
{ id: "scara", wasmFile: "linuxcnc_scara_kinematics.wasm", factory: createLinuxCncScaraKinematicsModule },
|
||||
{ id: "puma", wasmFile: "linuxcnc_puma_kinematics.wasm", factory: createLinuxCncPumaKinematicsModule },
|
||||
{ id: "genser", wasmFile: "linuxcnc_genser_kinematics.wasm", factory: createLinuxCncGenserKinematicsModule },
|
||||
{ id: "genhex", wasmFile: "linuxcnc_genhex_kinematics.wasm", factory: createLinuxCncGenhexKinematicsModule },
|
||||
{ id: "pentakins", wasmFile: "linuxcnc_pentakins_kinematics.wasm", factory: createLinuxCncPentakinsKinematicsModule },
|
||||
];
|
||||
|
||||
const MODULES_BY_ID = new Map(LINUXCNC_KINEMATICS_MODULES.map((entry) => [entry.id, entry]));
|
||||
|
||||
function requireWasmFunction(mod, functionName) {
|
||||
const fn = mod[`_${functionName}`];
|
||||
if (typeof fn !== "function") {
|
||||
throw new Error(`linuxcnc kinematics WASM missing ${functionName}; rebuild wasm-port/tools/build_kinematics_wasm.sh`);
|
||||
}
|
||||
return fn;
|
||||
}
|
||||
|
||||
function writeDoubleArray(mod, values) {
|
||||
const bytes = values.length * 8;
|
||||
const ptr = mod._malloc(bytes);
|
||||
mod.HEAPF64.set(values, ptr / 8);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
function readDoubleArray(mod, ptr, length) {
|
||||
return Array.from(mod.HEAPF64.subarray(ptr / 8, ptr / 8 + length));
|
||||
}
|
||||
|
||||
function poseValuesFromObject(pose = {}) {
|
||||
return [
|
||||
Number(pose.x ?? pose.tran?.x ?? 0),
|
||||
Number(pose.y ?? pose.tran?.y ?? 0),
|
||||
Number(pose.z ?? pose.tran?.z ?? 0),
|
||||
Number(pose.a ?? 0),
|
||||
Number(pose.b ?? 0),
|
||||
Number(pose.c ?? 0),
|
||||
Number(pose.u ?? 0),
|
||||
Number(pose.v ?? 0),
|
||||
Number(pose.w ?? 0),
|
||||
];
|
||||
}
|
||||
|
||||
function poseObjectFromValues(values) {
|
||||
return {
|
||||
x: values[0],
|
||||
y: values[1],
|
||||
z: values[2],
|
||||
a: values[3],
|
||||
b: values[4],
|
||||
c: values[5],
|
||||
u: values[6],
|
||||
v: values[7],
|
||||
w: values[8],
|
||||
};
|
||||
}
|
||||
|
||||
export function supportedLinuxCncKinematicsModules() {
|
||||
return LINUXCNC_KINEMATICS_MODULES.map((entry) => entry.id);
|
||||
}
|
||||
|
||||
export function linuxCncKinematicsWasmFile(moduleId) {
|
||||
return MODULES_BY_ID.get(moduleId)?.wasmFile || null;
|
||||
}
|
||||
|
||||
export async function createLinuxCncKinematicsSdk({
|
||||
moduleId = "xyzac-trt",
|
||||
moduleOptions = {},
|
||||
} = {}) {
|
||||
const entry = MODULES_BY_ID.get(moduleId);
|
||||
if (!entry) {
|
||||
throw new Error(`unsupported LinuxCNC kinematics module: ${moduleId}`);
|
||||
}
|
||||
|
||||
const mod = await entry.factory(moduleOptions);
|
||||
requireWasmFunction(mod, "lckins_init")();
|
||||
|
||||
return {
|
||||
apiName: "linuxcnc-kinematics-wasm-sdk",
|
||||
moduleId,
|
||||
module: mod,
|
||||
wasmFile: entry.wasmFile,
|
||||
|
||||
hasWasmFunction(functionName) {
|
||||
return typeof mod[`_${functionName}`] === "function";
|
||||
},
|
||||
|
||||
type() {
|
||||
return requireWasmFunction(mod, "lckins_type")();
|
||||
},
|
||||
|
||||
switchable() {
|
||||
return requireWasmFunction(mod, "lckins_switchable")();
|
||||
},
|
||||
|
||||
switchKinematics(switchkinsType) {
|
||||
return requireWasmFunction(mod, "lckins_switch")(Number(switchkinsType) || 0);
|
||||
},
|
||||
|
||||
forward(joints, options = {}) {
|
||||
const jointValues = Array.from(joints, Number);
|
||||
const jointsPtr = writeDoubleArray(mod, jointValues);
|
||||
const posePtr = writeDoubleArray(mod, poseValuesFromObject(options.seedPose));
|
||||
const fflagsPtr = mod._malloc(8);
|
||||
const iflagsPtr = mod._malloc(8);
|
||||
mod.HEAPU32[fflagsPtr / 4] = 0;
|
||||
mod.HEAPU32[iflagsPtr / 4] = 0;
|
||||
try {
|
||||
const rc = requireWasmFunction(mod, "lckins_forward")(
|
||||
jointsPtr,
|
||||
jointValues.length,
|
||||
posePtr,
|
||||
fflagsPtr,
|
||||
iflagsPtr,
|
||||
);
|
||||
return {
|
||||
rc,
|
||||
pose: poseObjectFromValues(readDoubleArray(mod, posePtr, 9)),
|
||||
fflags: mod.HEAPU32[fflagsPtr / 4],
|
||||
iflags: mod.HEAPU32[iflagsPtr / 4],
|
||||
};
|
||||
} finally {
|
||||
mod._free(jointsPtr);
|
||||
mod._free(posePtr);
|
||||
mod._free(fflagsPtr);
|
||||
mod._free(iflagsPtr);
|
||||
}
|
||||
},
|
||||
|
||||
inverse(pose, jointCount = 5, options = {}) {
|
||||
const posePtr = writeDoubleArray(mod, poseValuesFromObject(pose));
|
||||
const jointsPtr = mod._malloc(jointCount * 8);
|
||||
const iflagsPtr = mod._malloc(8);
|
||||
const fflagsPtr = mod._malloc(8);
|
||||
const seedJoints = Array.isArray(options.seedJoints)
|
||||
? options.seedJoints.map(Number)
|
||||
: [];
|
||||
mod.HEAPF64.fill(0, jointsPtr / 8, jointsPtr / 8 + jointCount);
|
||||
mod.HEAPF64.set(seedJoints.slice(0, jointCount), jointsPtr / 8);
|
||||
mod.HEAPU32[iflagsPtr / 4] = 0;
|
||||
mod.HEAPU32[fflagsPtr / 4] = 0;
|
||||
try {
|
||||
const rc = requireWasmFunction(mod, "lckins_inverse")(
|
||||
posePtr,
|
||||
jointsPtr,
|
||||
jointCount,
|
||||
iflagsPtr,
|
||||
fflagsPtr,
|
||||
);
|
||||
return {
|
||||
rc,
|
||||
joints: readDoubleArray(mod, jointsPtr, jointCount),
|
||||
iflags: mod.HEAPU32[iflagsPtr / 4],
|
||||
fflags: mod.HEAPU32[fflagsPtr / 4],
|
||||
};
|
||||
} finally {
|
||||
mod._free(posePtr);
|
||||
mod._free(jointsPtr);
|
||||
mod._free(iflagsPtr);
|
||||
mod._free(fflagsPtr);
|
||||
}
|
||||
},
|
||||
|
||||
runProbe() {
|
||||
const resultPtr = requireWasmFunction(mod, "lckins_run_probe")();
|
||||
if (!resultPtr) {
|
||||
throw new Error("lckins_run_probe returned null");
|
||||
}
|
||||
try {
|
||||
return mod.UTF8ToString(resultPtr);
|
||||
} finally {
|
||||
requireWasmFunction(mod, "lckins_free_string")(resultPtr);
|
||||
}
|
||||
},
|
||||
};
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user