继续完成 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
|
||||
|
||||
Reference in New Issue
Block a user