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

Implement WebAssembly runtime and kinematics coverage

Browse Source
This commit is contained in:
wangdequan
2026-08-21 06:04:48 +08:00
parent 5dd614c335
commit 554b72dae8
112 changed files with 47148 additions and 807 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
wasm-core/src/sim_hal.c
View File

@@ -21,8 +21,11 @@ typedef struct {
uint32_t thread;
uint32_t calls;
uint32_t fail;
uint32_t transform_kind;
} function_entry;
enum { SIM_HAL_TRANSFORM_NONE = 0, SIM_HAL_TRANSFORM_COREXY = 1 };
typedef struct {
uint32_t tick;
pin_entry pins[SIM_HAL_PIN_CAPACITY];
@@ -136,6 +139,73 @@ int32_t sim_hal_register_function(const char *thread, const char *name) {
return SIM_HAL_ERR_FULL;
}
static int32_t corexy_pin_names(const char *component, char names[8][SIM_HAL_NAME_CAPACITY]) {
static const char *suffixes[8] = {
".j0-motor-pos-cmd", ".j1-motor-pos-cmd", ".alpha-cmd", ".beta-cmd",
".alpha-fb", ".beta-fb", ".j0-motor-pos-fb", ".j1-motor-pos-fb",
};
uint32_t index;
if (!valid_name(component)) return SIM_HAL_ERR_ARGUMENT;
for (index = 0; index < 8; ++index) {
size_t prefix = strlen(component);
size_t suffix = strlen(suffixes[index]);
if (prefix + suffix >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
memcpy(names[index], component, prefix);
memcpy(names[index] + prefix, suffixes[index], suffix + 1);
}
return SIM_HAL_OK;
}
int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread) {
char names[8][SIM_HAL_NAME_CAPACITY];
uint32_t index;
int32_t result;
if (component == NULL || thread == NULL) return SIM_HAL_ERR_ARGUMENT;
result = corexy_pin_names(component, names);
if (result != SIM_HAL_OK) return result;
for (index = 0; index < 8; ++index) {
if (find_pin(names[index]) >= 0) return SIM_HAL_ERR_DUPLICATE;
}
if (find_function(component) >= 0) return SIM_HAL_ERR_DUPLICATE;
for (index = 0; index < 8; ++index) {
result = sim_hal_register_pin(names[index], SIM_HAL_FLOAT);
if (result != SIM_HAL_OK) return result;
}
result = sim_hal_register_function(thread, component);
if (result != SIM_HAL_OK) return result;
state.functions[find_function(component)].transform_kind = SIM_HAL_TRANSFORM_COREXY;
return SIM_HAL_OK;
}
static int32_t float_pin(const char *name, float *value) {
sim_hal_value pin = {0};
int32_t result = sim_hal_get_pin(name, &pin);
if (result != SIM_HAL_OK) return result;
if (pin.type != SIM_HAL_FLOAT) return SIM_HAL_ERR_TYPE;
*value = pin.value.real;
return SIM_HAL_OK;
}
static int32_t set_float_pin(const char *name, float value) {
sim_hal_value pin = {SIM_HAL_FLOAT, {.real = value}};
return sim_hal_set_pin(name, pin);
}
int32_t sim_hal_corexy_transform(const char *component) {
char names[8][SIM_HAL_NAME_CAPACITY];
float j0_cmd, j1_cmd, alpha_fb, beta_fb;
int32_t result = corexy_pin_names(component, names);
if (result != SIM_HAL_OK) return result;
result = float_pin(names[0], &j0_cmd); if (result != SIM_HAL_OK) return result;
result = float_pin(names[1], &j1_cmd); if (result != SIM_HAL_OK) return result;
result = float_pin(names[4], &alpha_fb); if (result != SIM_HAL_OK) return result;
result = float_pin(names[5], &beta_fb); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[2], 0.5f * (j0_cmd + j1_cmd)); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[3], 0.5f * (j0_cmd - j1_cmd)); if (result != SIM_HAL_OK) return result;
result = set_float_pin(names[6], alpha_fb + beta_fb); if (result != SIM_HAL_OK) return result;
return set_float_pin(names[7], alpha_fb - beta_fb);
}
int32_t sim_hal_set_function_failure(const char *name, uint32_t fail) {
int index = find_function(name);
if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
@@ -152,6 +222,10 @@ int32_t sim_hal_step(uint32_t ticks) {
function_entry *function = &state.functions[function_index];
if (!function->used || tick % state.threads[function->thread].period != 0U) continue;
if (function->fail) return SIM_HAL_ERR_FUNCTION;
if (function->transform_kind == SIM_HAL_TRANSFORM_COREXY) {
int32_t result = sim_hal_corexy_transform(function->name);
if (result != SIM_HAL_OK) return SIM_HAL_ERR_FUNCTION;
}
function->calls += 1U;
}
}

257
wasm-core/src/sim_interpreter.c Normal file
View File

@@ -0,0 +1,257 @@
#include "sim_interpreter.h"
#include <ctype.h>
#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <string.h>
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) {
hash ^= (uint64_t)bytes[index];
hash *= UINT64_C(1099511628211);
}
return hash;
}
static int finite_state(const sim_interpreter_state *state) {
const double *values;
size_t index;
if (state == 0) return 0;
values = &state->x;
for (index = 0; index < 11; ++index) if (!isfinite(values[index])) return 0;
return 1;
}
static int parse_number(const char **cursor, const char *end, double *output) {
const char *start = *cursor;
int sign = 1;
int digits = 0;
int exponent_sign = 1;
int exponent = 0;
long double value = 0.0L;
long double scale = 1.0L;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
if (**cursor == '-') sign = -1;
++*cursor;
}
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0');
++*cursor;
digits = 1;
}
if (*cursor < end && **cursor == '.') {
++*cursor;
while (*cursor < end && isdigit((unsigned char)**cursor)) {
value = value * 10.0L + (long double)(**cursor - '0');
scale *= 10.0L;
++*cursor;
digits = 1;
}
}
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
++*cursor;
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
if (**cursor == '-') exponent_sign = -1;
++*cursor;
}
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
while (*cursor < end && isdigit((unsigned char)**cursor)) {
if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0');
++*cursor;
}
}
value = (long double)sign * value / scale;
if (exponent) value *= powl(10.0L, (long double)(exponent_sign * exponent));
*output = (double)value;
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
}
static int code_is(double value, int integer_code) {
return isfinite(value) && fabs(value - (double)integer_code) <= 1e-12;
}
static int axis_index(char letter) {
switch (letter) {
case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
case 'A': return 3; case 'B': return 4; case 'C': return 5;
case 'U': return 6; case 'V': return 7; case 'W': return 8;
default: return -1;
}
}
static double *axis_pointer(sim_interpreter_state *state, int index) {
return &state->x + index;
}
static int apply_g(sim_interpreter_state *state, double code, int *groups) {
if (code_is(code, 0)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 0; return SIM_INTERP_OK; }
if (code_is(code, 1)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 1; return SIM_INTERP_OK; }
if (code_is(code, 2) || code_is(code, 3)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 17) || code_is(code, 18) || code_is(code, 19)) { if (++groups[1] > 1) return SIM_INTERP_ERR_STATE; state->plane = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 20) || code_is(code, 21)) { if (++groups[2] > 1) return SIM_INTERP_ERR_STATE; state->units = (int)code; return SIM_INTERP_OK; }
if (code_is(code, 90) || code_is(code, 91)) { if (++groups[3] > 1) return SIM_INTERP_ERR_STATE; state->distance_mode = (int)code; return SIM_INTERP_OK; }
return SIM_INTERP_ERR_UNKNOWN_CODE;
}
static int apply_m(sim_interpreter_state *state, double code, int *groups) {
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) {
if (++groups[4] > 1) return SIM_INTERP_ERR_STATE;
state->spindle_mode = (int)code;
return SIM_INTERP_OK;
}
if (code_is(code, 2) || code_is(code, 30)) {
if (++groups[5] > 1) return SIM_INTERP_ERR_STATE;
state->program_end = 1;
return SIM_INTERP_OK;
}
return SIM_INTERP_ERR_UNKNOWN_CODE;
}
static uint64_t state_hash(const sim_interpreter_state *state) {
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7];
size_t offset = 0;
const double *values = &state->x;
size_t index;
for (index = 0; index < 11; ++index) { memcpy(bytes + offset, values + index, sizeof(double)); offset += sizeof(double); }
memcpy(bytes + offset, &state->line, sizeof(uint32_t)); offset += sizeof(uint32_t);
memcpy(bytes + offset, &state->units, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->distance_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->plane, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->motion_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->spindle_mode, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->program_end, sizeof(int)); offset += sizeof(int);
memcpy(bytes + offset, &state->status, sizeof(int)); offset += sizeof(int);
return fnv1a64(bytes, offset);
}
static int make_canonical(sim_interpreter *interpreter) {
int written;
const sim_interpreter_state *s = &interpreter->state;
written = snprintf(interpreter->canonical, sizeof(interpreter->canonical),
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, s->motion_mode, s->plane, s->program_end,
s->speed, s->spindle_mode, s->status, s->units, s->u, s->v, s->w, s->x, s->y, s->z);
if (written < 0 || (size_t)written >= sizeof(interpreter->canonical)) return SIM_INTERP_ERR_BUFFER;
interpreter->canonical_length = (uint32_t)written;
return SIM_INTERP_OK;
}
int sim_interpreter_reset(sim_interpreter *interpreter) {
if (interpreter == 0) return SIM_INTERP_ERR_ARGUMENT;
memset(interpreter, 0, sizeof(*interpreter));
interpreter->state.units = SIM_INTERP_UNITS_MM;
interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE;
interpreter->state.plane = 17;
interpreter->state.motion_mode = 1;
interpreter->state.spindle_mode = 5;
interpreter->state.status = SIM_INTERP_OK;
return make_canonical(interpreter);
}
int sim_interpreter_init(sim_interpreter *interpreter) {
return sim_interpreter_reset(interpreter);
}
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length) {
sim_interpreter_state next;
const char *cursor;
const char *end;
int groups[6] = {0};
int seen_axis[9] = {0};
double axis_values[9] = {0};
int seen_feed = 0;
int seen_speed = 0;
double feed_value = 0.0;
double speed_value = 0.0;
int seen_line = 0;
int result = SIM_INTERP_OK;
if (interpreter == 0 || block == 0 || length == 0) return SIM_INTERP_ERR_ARGUMENT;
if (!finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
if (interpreter->state.program_end) return SIM_INTERP_ERR_STATE;
next = interpreter->state;
cursor = block;
end = block + length;
while (cursor < end) {
char letter;
double value;
int index;
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
if (cursor >= end) break;
if (*cursor == ';') break;
if (*cursor == '(') {
int closed = 0;
++cursor;
while (cursor < end) { if (*cursor++ == ')') { closed = 1; break; } }
if (!closed) return SIM_INTERP_ERR_SYNTAX;
continue;
}
letter = (char)toupper((unsigned char)*cursor++);
if (letter == '%') continue;
if (letter < 'A' || letter > 'Z') return SIM_INTERP_ERR_SYNTAX;
result = parse_number(&cursor, end, &value);
if (result != SIM_INTERP_OK) return result;
switch (letter) {
case 'G': result = apply_g(&next, value, groups); break;
case 'M': result = apply_m(&next, value, groups); break;
case 'N': if (seen_line++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0 || value > 4294967295.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else next.line = (uint32_t)value; break;
case 'F': if (seen_feed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else feed_value = value; break;
case 'S': if (seen_speed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else speed_value = value; break;
default:
index = axis_index(letter);
if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE;
else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE;
else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE;
if (result == SIM_INTERP_OK) axis_values[index] = value;
break;
}
if (result != SIM_INTERP_OK) return result;
}
/* Modal words apply to the whole block, independent of their textual order. */
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0);
if (seen_speed) next.speed = speed_value;
{
size_t axis;
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) {
double value = axis_values[axis];
double *destination = axis_pointer(&next, (int)axis);
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) value *= 25.4;
if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += value;
else *destination = value;
}
}
if (!finite_state(&next)) return SIM_INTERP_ERR_NONFINITE;
next.line = interpreter->state.line + 1;
next.status = SIM_INTERP_OK;
interpreter->state = next;
interpreter->block_hash = fnv1a64((const unsigned char *)block, length);
result = make_canonical(interpreter);
if (result != SIM_INTERP_OK) { interpreter->state.status = result; return result; }
return SIM_INTERP_OK;
}
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) {
if (interpreter == 0 || state == 0) return SIM_INTERP_ERR_ARGUMENT;
*state = interpreter->state;
return SIM_INTERP_OK;
}
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity) {
if (interpreter == 0) return 0;
if (output == 0 || capacity == 0) return interpreter->canonical_length + 1;
if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1;
memcpy(output, interpreter->canonical, interpreter->canonical_length + 1);
return interpreter->canonical_length + 1;
}
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) {
if (interpreter == 0) return 0;
return state_hash(&interpreter->state);
}
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) {
return interpreter == 0 ? 0 : interpreter->block_hash;
}

1181
wasm-core/src/sim_kinematics.c Normal file
View File

File diff suppressed because it is too large Load Diff

154
wasm-core/src/sim_motion_queue.c Normal file
View File

@@ -0,0 +1,154 @@
#include "sim_motion_queue.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
return hash;
}
static int valid_config(double cycle_time, double velocity_limit) {
return isfinite(cycle_time) && isfinite(velocity_limit) && cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9;
}
static double segment_length(const sim_posemath_segment *segment) {
const double dx = segment->end.tran.x - segment->start.tran.x;
const double dy = segment->end.tran.y - segment->start.tran.y;
const double dz = segment->end.tran.z - segment->start.tran.z;
double value = dx * dx + dy * dy + dz * dz;
return isfinite(value) && value > 0.0 ? sqrt(value) : 0.0;
}
static int poses_match(const sim_posemath_pose *left, const sim_posemath_pose *right) {
const double *a = &left->tran.x;
const double *b = &right->tran.x;
size_t index;
for (index = 0; index < 9; ++index) if (fabs(a[index] - b[index]) > 1e-12) return 0;
return 1;
}
static void zero_state(sim_motion_state *state) {
memset(state, 0, sizeof(*state));
state->status = SIM_MOTION_OK;
}
int sim_motion_reset(sim_motion_queue *queue) {
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
zero_state(&queue->state);
return SIM_MOTION_OK;
}
int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_limit) {
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
if (!valid_config(cycle_time, velocity_limit)) return SIM_MOTION_ERR_RANGE;
queue->cycle_time = cycle_time; queue->velocity_limit = velocity_limit;
return sim_motion_reset(queue);
}
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
uint32_t index;
int result;
if (queue == 0 || command == 0) return SIM_MOTION_ERR_ARGUMENT;
result = sim_posemath_segment_validate(&command->segment);
if (result == SIM_POSEMATH_ERR_MOTION_TYPE) return SIM_MOTION_ERR_MOTION_TYPE;
if (result != SIM_POSEMATH_OK) return result == SIM_POSEMATH_ERR_NONFINITE ? SIM_MOTION_ERR_NONFINITE : SIM_MOTION_ERR_GEOMETRY;
if (queue->count >= SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_CAPACITY;
if (command->segment.velocity > queue->velocity_limit) return SIM_MOTION_ERR_RANGE;
if (queue->count > 0) {
index = (queue->head + queue->count - 1) % SIM_MOTION_QUEUE_CAPACITY;
if (!poses_match(&queue->commands[index].segment.end, &command->segment.start)) return SIM_MOTION_ERR_STATE;
} else if (queue->state.tick > 0 && !poses_match(&queue->state.position, &command->segment.start)) {
return SIM_MOTION_ERR_STATE;
}
index = (queue->head + queue->count) % SIM_MOTION_QUEUE_CAPACITY;
queue->commands[index] = *command;
queue->commands[index].id = queue->next_id++;
if (id_out != 0) *id_out = queue->commands[index].id;
queue->count++;
return SIM_MOTION_OK;
}
int sim_motion_step(sim_motion_queue *queue) {
sim_motion_command *command;
double length;
double delta;
int result;
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
command = &queue->commands[queue->head];
length = segment_length(&command->segment);
if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->state.active_id = command->id; queue->state.active_tag = command->tag; queue->state.motion_type = command->segment.motion_type;
delta = command->segment.velocity * queue->cycle_time / length;
if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
queue->state.progress += delta;
if (queue->state.progress >= 1.0) {
queue->state.progress = 1.0;
result = sim_posemath_segment_point(&command->segment, 1.0, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
queue->head = (queue->head + 1) % SIM_MOTION_QUEUE_CAPACITY;
queue->count--;
queue->state.progress = 0.0;
queue->state.active_id = 0;
queue->state.active_tag = 0;
queue->state.motion_type = 0;
} else {
result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
}
queue->state.velocity = command->segment.velocity;
queue->tick++;
queue->state.tick = queue->tick;
queue->state.queue_depth = queue->count;
queue->state.status = SIM_MOTION_OK;
return SIM_MOTION_OK;
}
int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state) {
if (queue == 0 || state == 0) return SIM_MOTION_ERR_ARGUMENT;
*state = queue->state;
state->queue_depth = queue->count;
return SIM_MOTION_OK;
}
uint32_t sim_motion_queue_depth(const sim_motion_queue *queue) {
return queue == 0 ? 0 : queue->count;
}
uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
sim_motion_state state;
unsigned char bytes[sizeof(sim_motion_state)];
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
memset(bytes, 0, sizeof(bytes));
memcpy(bytes, &state.position, sizeof(state.position));
memcpy(bytes + sizeof(state.position), &state.velocity, sizeof(double));
memcpy(bytes + sizeof(state.position) + sizeof(double), &state.progress, sizeof(double));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2, &state.tick, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t), &state.queue_depth, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 2, &state.active_id, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 3, &state.active_tag, sizeof(uint32_t));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4, &state.motion_type, sizeof(int));
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int), &state.status, sizeof(int));
return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
}
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
sim_motion_state state;
char discard[1];
char *destination = output == 0 ? discard : output;
uint32_t destination_capacity = output == 0 ? 1 : capacity;
int written;
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
written = snprintf(destination, destination_capacity,
"{\"active_id\":%u,\"active_tag\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
state.active_id, state.active_tag, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
state.progress, state.queue_depth, state.status, state.tick, state.velocity);
if (written < 0) return 0;
if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
return (uint32_t)written + 1;
}

221
wasm-core/src/sim_posemath.c
View File

@@ -3,11 +3,25 @@
#include "posemath.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
static int finite_cart(const sim_posemath_cartesian *value) {
return value != 0 && isfinite(value->x) && isfinite(value->y) && isfinite(value->z);
}
static int finite_pose(const sim_posemath_pose *value) {
return value != 0 && finite_cart(&value->tran) && isfinite(value->a) &&
isfinite(value->b) && isfinite(value->c) && isfinite(value->u) &&
isfinite(value->v) && isfinite(value->w);
}
static int finite_segment(const sim_posemath_segment *value) {
return value != 0 && finite_pose(&value->start) && finite_pose(&value->end) &&
finite_cart(&value->center) && finite_cart(&value->normal) &&
isfinite(value->velocity) && isfinite(value->acceleration);
}
static PmCartesian to_pm(const sim_posemath_cartesian *value) {
PmCartesian result = {value->x, value->y, value->z};
return result;
@@ -27,7 +41,8 @@ int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_car
PmCartesian source;
PmCartesian result;
double magnitude_squared = 0.0;
if (!finite_cart(input) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (input == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!finite_cart(input)) return SIM_POSEMATH_ERR_NONFINITE;
source = to_pm(input);
if (pmCartMagSq(&source, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
@@ -39,7 +54,8 @@ int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_car
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output) {
PmCartesian lhs;
PmCartesian rhs;
if (!finite_cart(left) || !finite_cart(right) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (left == 0 || right == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!finite_cart(left) || !finite_cart(right)) return SIM_POSEMATH_ERR_NONFINITE;
lhs = to_pm(left);
rhs = to_pm(right);
if (pmCartCartDot(&lhs, &rhs, output) != 0 || !isfinite(*output)) return SIM_POSEMATH_ERR_NONFINITE;
@@ -52,11 +68,206 @@ int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posem
PmCartesian finish;
PmCartesian result;
PmCartesian delta;
if (!finite_cart(start) || !finite_cart(end) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!isfinite(fraction) || fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
double magnitude_squared = 0.0;
if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!finite_cart(start) || !finite_cart(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
begin = to_pm(start);
finish = to_pm(end);
if (pmCartCartSub(&finish, &begin, &delta) != 0 || pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
if (pmCartCartSub(&finish, &begin, &delta) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
if (pmCartMagSq(&delta, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
if (pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result)) return SIM_POSEMATH_ERR_DEGENERATE;
if (!isfinite(result.x) || !isfinite(result.y) || !isfinite(result.z)) return SIM_POSEMATH_ERR_NONFINITE;
from_pm(&result, output);
return SIM_POSEMATH_OK;
}
static int finite_pose_value(const sim_posemath_pose *value) {
return finite_pose(value);
}
int sim_posemath_pose_is_finite(const sim_posemath_pose *value) {
return finite_pose_value(value);
}
int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
double fraction, sim_posemath_pose *output) {
if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!finite_pose_value(start) || !finite_pose_value(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
output->tran.x = start->tran.x + (end->tran.x - start->tran.x) * fraction;
output->tran.y = start->tran.y + (end->tran.y - start->tran.y) * fraction;
output->tran.z = start->tran.z + (end->tran.z - start->tran.z) * fraction;
output->a = start->a + (end->a - start->a) * fraction;
output->b = start->b + (end->b - start->b) * fraction;
output->c = start->c + (end->c - start->c) * fraction;
output->u = start->u + (end->u - start->u) * fraction;
output->v = start->v + (end->v - start->v) * fraction;
output->w = start->w + (end->w - start->w) * fraction;
return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
}
int sim_posemath_segment_validate(const sim_posemath_segment *segment) {
double length_sq;
double normal_sq;
if (segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!finite_segment(segment)) return SIM_POSEMATH_ERR_NONFINITE;
if (segment->motion_type < SIM_POSEMATH_LINEAR || segment->motion_type > SIM_POSEMATH_SPHERICAL) return SIM_POSEMATH_ERR_MOTION_TYPE;
if (segment->velocity < 0.0 || segment->acceleration < 0.0 || segment->velocity > 1e9 || segment->acceleration > 1e12) return SIM_POSEMATH_ERR_RANGE;
length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
(segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
if (!isfinite(length_sq) || length_sq <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
if (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
normal_sq = segment->normal.x * segment->normal.x + segment->normal.y * segment->normal.y + segment->normal.z * segment->normal.z;
if (!isfinite(normal_sq) || normal_sq <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
if (segment->turn < -1000000 || segment->turn > 1000000) return SIM_POSEMATH_ERR_RANGE;
}
return SIM_POSEMATH_OK;
}
int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
sim_posemath_pose *output) {
int result = sim_posemath_segment_validate(segment);
if (result != SIM_POSEMATH_OK) return result;
if (output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
if (segment->motion_type != SIM_POSEMATH_CIRCULAR)
return sim_posemath_pose_line_point(&segment->start, &segment->end, fraction, output);
{
PmCartesian start = to_pm(&segment->start.tran);
PmCartesian end = to_pm(&segment->end.tran);
PmCartesian center = to_pm(&segment->center);
PmCartesian normal = to_pm(&segment->normal);
PmCircle circle;
PmCartesian point;
if (pmCircleInit(&circle, &start, &end, &center, &normal, segment->turn) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
if (pmCirclePoint(&circle, circle.angle * fraction, &point) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
output->tran.x = point.x; output->tran.y = point.y; output->tran.z = point.z;
output->a = segment->start.a + (segment->end.a - segment->start.a) * fraction;
output->b = segment->start.b + (segment->end.b - segment->start.b) * fraction;
output->c = segment->start.c + (segment->end.c - segment->start.c) * fraction;
output->u = segment->start.u + (segment->end.u - segment->start.u) * fraction;
output->v = segment->start.v + (segment->end.v - segment->start.v) * fraction;
output->w = segment->start.w + (segment->end.w - segment->start.w) * fraction;
return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
}
}
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
uint64_t hash = UINT64_C(1469598103934665603);
size_t index;
for (index = 0; index < length; ++index) {
hash ^= (uint64_t)bytes[index];
hash *= UINT64_C(1099511628211);
}
return hash;
}
uint64_t sim_posemath_hash_state(const sim_tp_state *state) {
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) * 3 + sizeof(int) * 2];
size_t offset = 0;
const double *doubles;
const uint32_t *uints;
const int *ints;
size_t index;
if (state == 0) return 0;
doubles = (const double *)&state->position;
for (index = 0; index < 11; ++index) {
memcpy(bytes + offset, doubles + index, sizeof(double));
offset += sizeof(double);
}
uints = &state->tick;
for (index = 0; index < 3; ++index) {
memcpy(bytes + offset, uints + index, sizeof(uint32_t));
offset += sizeof(uint32_t);
}
ints = &state->motion_type;
for (index = 0; index < 2; ++index) {
memcpy(bytes + offset, ints + index, sizeof(int));
offset += sizeof(int);
}
return fnv1a64(bytes, offset);
}
static int valid_tp_config(double cycle_time, double velocity_limit, double acceleration_limit) {
return isfinite(cycle_time) && isfinite(velocity_limit) && isfinite(acceleration_limit) &&
cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9 &&
acceleration_limit >= 0.0 && acceleration_limit <= 1e12;
}
int sim_tp_reset(sim_tp *tp) {
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
tp->head = 0; tp->count = 0; tp->next_id = 1; tp->active_id = 0; tp->tick = 0;
tp->progress = 0.0; tp->current_velocity = 0.0; tp->motion_type = 0; tp->status = SIM_POSEMATH_OK;
tp->position.tran.x = 0.0; tp->position.tran.y = 0.0; tp->position.tran.z = 0.0;
tp->position.a = tp->position.b = tp->position.c = tp->position.u = tp->position.v = tp->position.w = 0.0;
return SIM_POSEMATH_OK;
}
int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit) {
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (!valid_tp_config(cycle_time, velocity_limit, acceleration_limit)) return SIM_POSEMATH_ERR_RANGE;
tp->cycle_time = cycle_time; tp->velocity_limit = velocity_limit; tp->acceleration_limit = acceleration_limit;
return sim_tp_reset(tp);
}
int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out) {
uint32_t index;
int result;
if (tp == 0 || segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
result = sim_posemath_segment_validate(segment);
if (result != SIM_POSEMATH_OK) return result;
if (tp->count >= SIM_TP_QUEUE_CAPACITY) return SIM_POSEMATH_ERR_CAPACITY;
if (segment->velocity > tp->velocity_limit || segment->acceleration > tp->acceleration_limit) return SIM_POSEMATH_ERR_RANGE;
index = (tp->head + tp->count) % SIM_TP_QUEUE_CAPACITY;
tp->queue[index] = *segment; tp->ids[index] = tp->next_id++;
if (id_out != 0) *id_out = tp->ids[index];
tp->count++;
return SIM_POSEMATH_OK;
}
int sim_tp_step(sim_tp *tp) {
sim_posemath_segment *segment;
double length_sq;
double step;
int result;
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
if (tp->count == 0) { tp->tick++; tp->status = SIM_POSEMATH_ERR_STATE; return SIM_POSEMATH_ERR_STATE; }
segment = &tp->queue[tp->head];
result = sim_posemath_segment_validate(segment);
if (result != SIM_POSEMATH_OK) { tp->status = result; return result; }
if (tp->active_id == 0) { tp->active_id = tp->ids[tp->head]; tp->motion_type = segment->motion_type; }
length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
(segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
step = segment->velocity * tp->cycle_time / sqrt(length_sq);
if (!isfinite(step) || step < 0.0) { tp->status = SIM_POSEMATH_ERR_NONFINITE; return tp->status; }
if (step < 1e-15) step = 1e-15;
tp->progress += step;
if (tp->progress >= 1.0) {
tp->progress = 1.0;
result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
tp->head = (tp->head + 1) % SIM_TP_QUEUE_CAPACITY; tp->count--; tp->active_id = 0; tp->motion_type = 0; tp->progress = 0.0;
} else {
result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
}
tp->current_velocity = segment->velocity; tp->tick++; tp->status = result;
return result;
}
int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state) {
if (tp == 0 || state == 0) return SIM_POSEMATH_ERR_ARGUMENT;
state->position = tp->position; state->velocity = tp->current_velocity; state->progress = tp->progress;
state->tick = tp->tick; state->queue_depth = tp->count; state->active_id = tp->active_id;
state->motion_type = tp->motion_type; state->status = tp->status;
return SIM_POSEMATH_OK;
}
uint64_t sim_tp_state_hash(const sim_tp *tp) {
sim_tp_state state;
if (sim_tp_read_state(tp, &state) != SIM_POSEMATH_OK) return 0;
return sim_posemath_hash_state(&state);
}