继续完成 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:
2026-06-21 16:44:29 +08:00
parent a6eda3fbff
commit 626bcfe8e3
101 changed files with 101586 additions and 770 deletions

View 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);
}

View File

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