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

Extend simulation runtime coverage and kinematics support

Browse Source
This commit is contained in:
wangdequan
2026-08-21 10:57:10 +08:00
parent 554b72dae8
commit 2ef2d82db3
71 changed files with 24232 additions and 1867 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wasm-core/src/sim_event.c
View File

@@ -72,11 +72,21 @@ uint32_t sim_event_export(uint8_t *out, uint32_t capacity) {
}
int32_t sim_event_import(const uint8_t *data, uint32_t len) {
sim_event_state candidate;
uint32_t index;
if (data == NULL || len != sizeof(state)) return SIM_EVENT_ERR_ARGUMENT;
memcpy(&state, data, sizeof(state));
if (state.count > SIM_EVENT_CAPACITY) {
sim_event_reset();
return SIM_EVENT_ERR_ARGUMENT;
memcpy(&candidate, data, sizeof(candidate));
if (candidate.count > SIM_EVENT_CAPACITY) return SIM_EVENT_ERR_ARGUMENT;
if (candidate.count == 0U && (candidate.last_seq != 0U || candidate.last_tick != 0U)) return SIM_EVENT_ERR_ARGUMENT;
for (index = 0; index < candidate.count; ++index) {
const sim_event_record *record = &candidate.records[index];
size_t kind_len = 0U;
if (record->seq != index + 1U) return SIM_EVENT_ERR_SEQUENCE;
if (index > 0U && record->tick < candidate.records[index - 1U].tick) return SIM_EVENT_ERR_TICK;
while (kind_len < SIM_EVENT_KIND_CAPACITY && record->kind[kind_len] != '\0') ++kind_len;
if (kind_len == 0U || kind_len >= SIM_EVENT_KIND_CAPACITY) return SIM_EVENT_ERR_ARGUMENT;
}
if (candidate.count > 0U && (candidate.last_seq != candidate.records[candidate.count - 1U].seq || candidate.last_tick != candidate.records[candidate.count - 1U].tick)) return SIM_EVENT_ERR_ARGUMENT;
memcpy(&state, &candidate, sizeof(state));
return SIM_EVENT_OK;
}

561
wasm-core/src/sim_io.c Normal file
View File

@@ -0,0 +1,561 @@
#include "sim_io.h"
#include "sim_hal.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
static uint64_t io_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 double normalized(double value) {
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}
static int finite_pose(const sim_posemath_pose *pose) {
const double *values;
uint32_t index;
if (pose == 0) return 0;
values = &pose->tran.x;
for (index = 0; index < 9; ++index) if (!isfinite(values[index])) return 0;
return 1;
}
static int valid_io(const sim_io *io) {
return io != 0 && io->state.spindle_count > 0 &&
io->state.spindle_count <= SIM_IO_MAX_SPINDLES;
}
static int valid_spindle(const sim_io *io, uint32_t spindle) {
return valid_io(io) && spindle < io->state.spindle_count &&
io->state.spindles[spindle].registered;
}
static void set_last(sim_io *io, int result) {
if (io != 0) io->state.last_error = result;
}
static int register_pin(const char *name, uint32_t type) {
int result = sim_hal_register_pin(name, type);
if (result == SIM_HAL_OK) return SIM_IO_OK;
if (result == SIM_HAL_ERR_DUPLICATE) {
sim_hal_value existing = {0};
if (sim_hal_get_pin(name, &existing) == SIM_HAL_OK && existing.type == type) return SIM_IO_OK;
}
return SIM_IO_ERR_PIN;
}
static int register_io_pins(sim_io *io) {
static const struct { const char *name; uint32_t type; } fixed[] = {
{"motion.probe-input", SIM_HAL_BIT},
{"iocontrol.0.user-enable-out", SIM_HAL_BIT},
{"iocontrol.0.emc-enable-in", SIM_HAL_BIT},
{"iocontrol.0.user-request-enable", SIM_HAL_BIT},
{"iocontrol.0.coolant-mist", SIM_HAL_BIT},
{"iocontrol.0.coolant-flood", SIM_HAL_BIT},
{"iocontrol.0.tool-prepare", SIM_HAL_BIT},
{"iocontrol.0.tool-prep-number", SIM_HAL_S32},
{"iocontrol.0.tool-prep-pocket", SIM_HAL_S32},
{"iocontrol.0.tool-prep-index", SIM_HAL_S32},
{"iocontrol.0.tool-number", SIM_HAL_S32},
{"iocontrol.0.tool-prepared", SIM_HAL_BIT},
{"iocontrol.0.tool-change", SIM_HAL_BIT},
{"iocontrol.0.tool-changed", SIM_HAL_BIT},
{"iocontrol.0.toolchanger-fault", SIM_HAL_BIT},
{"iocontrol.0.toolchanger-fault-ack", SIM_HAL_BIT},
{"iocontrol.0.toolchanger-reason", SIM_HAL_S32},
{"iocontrol.0.toolchanger-faulted", SIM_HAL_BIT},
{"iocontrol.0.toolchanger-clear-fault", SIM_HAL_BIT},
{"iocontrol.0.emc-abort", SIM_HAL_BIT},
{"iocontrol.0.emc-abort-ack", SIM_HAL_BIT},
{"iocontrol.0.emc-reason", SIM_HAL_S32},
{"iocontrol.0.start-change", SIM_HAL_BIT},
{"iocontrol.0.start-change-ack", SIM_HAL_BIT},
{"iocontrol.0.state", SIM_HAL_S32},
};
uint32_t index;
char name[64];
int result;
result = sim_hal_register_thread("sim-io-servo", 1);
if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
result = sim_hal_register_function("sim-io-servo", "sim-io");
if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
for (index = 0; index < sizeof(fixed) / sizeof(fixed[0]); ++index) {
result = register_pin(fixed[index].name, fixed[index].type);
if (result != SIM_IO_OK) return result;
}
for (index = 0; index < io->state.spindle_count; ++index) {
static const struct { const char *suffix; uint32_t type; } spindle_pins[] = {
{"on", SIM_HAL_BIT}, {"forward", SIM_HAL_BIT}, {"reverse", SIM_HAL_BIT},
{"brake", SIM_HAL_BIT}, {"speed-out", SIM_HAL_FLOAT},
{"speed-out-abs", SIM_HAL_FLOAT}, {"speed-out-rps", SIM_HAL_FLOAT},
{"speed-out-rps-abs", SIM_HAL_FLOAT}, {"speed-cmd-rps", SIM_HAL_FLOAT},
{"inhibit", SIM_HAL_BIT}, {"amp-fault-in", SIM_HAL_BIT},
{"orient-angle", SIM_HAL_FLOAT}, {"orient-mode", SIM_HAL_S32},
{"orient", SIM_HAL_BIT}, {"locked", SIM_HAL_BIT},
{"is-oriented", SIM_HAL_BIT}, {"orient-fault", SIM_HAL_S32},
{"revs", SIM_HAL_FLOAT}, {"speed-in", SIM_HAL_FLOAT},
{"at-speed", SIM_HAL_BIT}, {"index-enable", SIM_HAL_BIT},
};
uint32_t pin;
for (pin = 0; pin < sizeof(spindle_pins) / sizeof(spindle_pins[0]); ++pin) {
int written = snprintf(name, sizeof(name), "spindle.%u.%s", index, spindle_pins[pin].suffix);
if (written < 0 || (size_t)written >= sizeof(name)) return SIM_IO_ERR_PIN;
result = register_pin(name, spindle_pins[pin].type);
if (result != SIM_IO_OK) return result;
}
}
io->pins_registered = 1;
return SIM_IO_OK;
}
static void set_bit(const char *name, uint32_t value) {
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
}
static void set_s32(const char *name, int32_t value) {
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_S32, {.s32 = value}});
}
static void set_float(const char *name, double value) {
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_FLOAT, {.real = (float)value}});
}
static void mirror_pins(sim_io *io) {
char name[64];
uint32_t index;
sim_io_state *state = &io->state;
set_bit("motion.probe-input", state->probe.input);
set_bit("iocontrol.0.user-enable-out", state->estop);
set_bit("iocontrol.0.emc-enable-in", state->ready);
set_bit("iocontrol.0.user-request-enable", 0);
set_bit("iocontrol.0.coolant-mist", 0);
set_bit("iocontrol.0.coolant-flood", 0);
set_bit("iocontrol.0.tool-prepare", state->tool_prepare);
set_s32("iocontrol.0.tool-prep-number", state->prepared_tool);
set_s32("iocontrol.0.tool-prep-pocket", state->prepared_pocket);
set_s32("iocontrol.0.tool-prep-index", state->prepared_pocket);
set_s32("iocontrol.0.tool-number", state->current_tool);
set_bit("iocontrol.0.tool-prepared", state->tool_prepared);
set_bit("iocontrol.0.tool-change", state->tool_change);
set_bit("iocontrol.0.tool-changed", state->tool_changed);
set_bit("iocontrol.0.toolchanger-fault", state->toolchanger_fault);
set_bit("iocontrol.0.toolchanger-fault-ack", state->toolchanger_fault ? 0 : 1);
set_s32("iocontrol.0.toolchanger-reason", state->toolchanger_reason);
set_bit("iocontrol.0.toolchanger-faulted", state->fault);
set_bit("iocontrol.0.toolchanger-clear-fault", 0);
set_bit("iocontrol.0.emc-abort", state->tool_state == SIM_IO_TOOL_ABORTED);
set_bit("iocontrol.0.emc-abort-ack", state->tool_state != SIM_IO_TOOL_ABORTED);
set_s32("iocontrol.0.emc-reason", state->fault_reason);
set_bit("iocontrol.0.start-change", state->tool_change);
set_bit("iocontrol.0.start-change-ack", state->tool_state != SIM_IO_TOOL_CHANGING);
set_s32("iocontrol.0.state", (int32_t)state->tool_state);
for (index = 0; index < state->spindle_count; ++index) {
sim_io_spindle_state *spindle = &state->spindles[index];
snprintf(name, sizeof(name), "spindle.%u.on", index); set_bit(name, spindle->enabled && spindle->speed_rpm != 0.0);
snprintf(name, sizeof(name), "spindle.%u.forward", index); set_bit(name, spindle->forward);
snprintf(name, sizeof(name), "spindle.%u.reverse", index); set_bit(name, spindle->reverse);
snprintf(name, sizeof(name), "spindle.%u.brake", index); set_bit(name, spindle->brake);
snprintf(name, sizeof(name), "spindle.%u.speed-out", index); set_float(name, spindle->speed_rpm);
snprintf(name, sizeof(name), "spindle.%u.speed-out-abs", index); set_float(name, fabs(spindle->speed_rpm));
snprintf(name, sizeof(name), "spindle.%u.speed-out-rps", index); set_float(name, spindle->speed_rpm / 60.0);
snprintf(name, sizeof(name), "spindle.%u.speed-out-rps-abs", index); set_float(name, fabs(spindle->speed_rpm) / 60.0);
snprintf(name, sizeof(name), "spindle.%u.speed-cmd-rps", index); set_float(name, spindle->command_rpm / 60.0);
snprintf(name, sizeof(name), "spindle.%u.inhibit", index); set_bit(name, spindle->inhibit);
snprintf(name, sizeof(name), "spindle.%u.amp-fault-in", index); set_bit(name, spindle->amp_fault);
snprintf(name, sizeof(name), "spindle.%u.orient-angle", index); set_float(name, spindle->orient_angle);
snprintf(name, sizeof(name), "spindle.%u.orient-mode", index); set_s32(name, spindle->orient_mode);
snprintf(name, sizeof(name), "spindle.%u.orient", index); set_bit(name, spindle->orient);
snprintf(name, sizeof(name), "spindle.%u.locked", index); set_bit(name, spindle->locked);
snprintf(name, sizeof(name), "spindle.%u.is-oriented", index); set_bit(name, spindle->is_oriented);
snprintf(name, sizeof(name), "spindle.%u.orient-fault", index); set_s32(name, spindle->orient_fault);
snprintf(name, sizeof(name), "spindle.%u.revs", index); set_float(name, spindle->revs);
snprintf(name, sizeof(name), "spindle.%u.speed-in", index); set_float(name, spindle->speed_rpm);
snprintf(name, sizeof(name), "spindle.%u.at-speed", index); set_bit(name, spindle->at_speed);
snprintf(name, sizeof(name), "spindle.%u.index-enable", index); set_bit(name, spindle->index_enable);
}
}
static void state_defaults(sim_io *io) {
uint32_t index;
sim_io_state old = io->state;
memset(&io->state, 0, sizeof(io->state));
io->state.spindle_count = old.spindle_count;
io->state.tool_capacity = old.tool_capacity;
io->state.ready = 1;
io->state.current_tool = 0;
io->state.prepared_tool = -1;
io->state.prepared_pocket = -1;
io->state.toolchanger_reason = 0;
io->state.tool_state = SIM_IO_TOOL_IDLE;
io->state.last_error = SIM_IO_OK;
io->state.probe.state = SIM_IO_PROBE_IDLE;
for (index = 0; index < io->state.spindle_count; ++index) {
io->state.spindles[index].registered = 1;
io->state.spindles[index].max_rpm = 100000.0;
io->state.spindles[index].accel_rpm_s = old.spindles[index].accel_rpm_s;
io->state.spindles[index].tolerance_rpm = old.spindles[index].tolerance_rpm;
io->state.spindles[index].at_speed = 1;
}
}
int sim_io_init(sim_io *io, uint32_t spindle_count, uint32_t tool_capacity,
double cycle_time, double spindle_accel_rpm_s,
double spindle_tolerance_rpm) {
uint32_t index;
int result;
if (io == 0) return SIM_IO_ERR_ARGUMENT;
if (spindle_count == 0 || spindle_count > SIM_IO_MAX_SPINDLES ||
tool_capacity == 0 || tool_capacity > SIM_IO_MAX_TOOLS ||
!isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 ||
!isfinite(spindle_accel_rpm_s) || spindle_accel_rpm_s <= 0.0 ||
!isfinite(spindle_tolerance_rpm) || spindle_tolerance_rpm < 0.0)
return SIM_IO_ERR_RANGE;
memset(io, 0, sizeof(*io));
io->cycle_time = cycle_time;
io->state.spindle_count = spindle_count;
io->state.tool_capacity = tool_capacity;
for (index = 0; index < spindle_count; ++index) {
io->state.spindles[index].accel_rpm_s = spindle_accel_rpm_s;
io->state.spindles[index].tolerance_rpm = spindle_tolerance_rpm;
}
result = register_io_pins(io);
if (result != SIM_IO_OK) return result;
state_defaults(io);
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_reset(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
state_defaults(io);
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_register_pins(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
return register_io_pins(io);
}
int sim_io_spindle_command(sim_io *io, uint32_t spindle, double rpm, uint32_t enabled) {
sim_io_spindle_state *state;
if (!valid_spindle(io, spindle) || enabled > 1U) return SIM_IO_ERR_ARGUMENT;
if (!isfinite(rpm)) { set_last(io, SIM_IO_ERR_NONFINITE); return SIM_IO_ERR_NONFINITE; }
if (fabs(rpm) > io->state.spindles[spindle].max_rpm) { set_last(io, SIM_IO_ERR_LIMIT); return SIM_IO_ERR_LIMIT; }
if (io->state.estop) { set_last(io, SIM_IO_ERR_ESTOP); return SIM_IO_ERR_ESTOP; }
if (!io->state.ready) { set_last(io, SIM_IO_ERR_NOT_READY); return SIM_IO_ERR_NOT_READY; }
state = &io->state.spindles[spindle];
state->command_rpm = enabled && !state->inhibit && !state->amp_fault ? rpm : 0.0;
state->enabled = enabled ? 1U : 0U;
set_last(io, SIM_IO_OK);
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_set_inhibit(sim_io *io, uint32_t spindle, uint32_t inhibit) {
if (!valid_spindle(io, spindle) || inhibit > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.spindles[spindle].inhibit = inhibit;
if (inhibit) io->state.spindles[spindle].command_rpm = 0.0;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_set_fault(sim_io *io, uint32_t spindle, uint32_t fault) {
if (!valid_spindle(io, spindle) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.spindles[spindle].amp_fault = fault;
if (fault) {
io->state.spindles[spindle].command_rpm = 0.0;
io->state.spindles[spindle].enabled = 0;
}
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_set_index_enable(sim_io *io, uint32_t spindle, uint32_t enable) {
if (!valid_spindle(io, spindle) || enable > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.spindles[spindle].index_enable = enable;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_set_feedback(sim_io *io, uint32_t spindle, double rpm) {
if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
if (!isfinite(rpm)) return SIM_IO_ERR_NONFINITE;
io->state.spindles[spindle].speed_rpm = rpm;
io->state.spindles[spindle].at_speed = fabs(rpm - io->state.spindles[spindle].command_rpm) <= io->state.spindles[spindle].tolerance_rpm;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_orient(sim_io *io, uint32_t spindle, double angle, int32_t mode) {
sim_io_spindle_state *state;
if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
if (!isfinite(angle) || mode < 0 || mode > 2) return SIM_IO_ERR_RANGE;
if (io->state.estop) return SIM_IO_ERR_ESTOP;
state = &io->state.spindles[spindle];
if (fabs(state->speed_rpm) > state->tolerance_rpm || state->command_rpm != 0.0) return SIM_IO_ERR_BUSY;
state->orient = 1; state->locked = 0; state->is_oriented = 0;
state->orient_angle = angle; state->orient_mode = mode; state->orient_fault = 0;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_spindle_set_oriented(sim_io *io, uint32_t spindle, uint32_t oriented, int32_t fault) {
sim_io_spindle_state *state;
if (!valid_spindle(io, spindle) || oriented > 1U) return SIM_IO_ERR_ARGUMENT;
state = &io->state.spindles[spindle];
if (!state->orient) return SIM_IO_ERR_STATE;
state->is_oriented = oriented;
state->orient_fault = fault;
if (fault) { state->orient = 0; state->locked = 0; }
else if (oriented) { state->orient = 0; state->locked = 1; }
mirror_pins(io);
return SIM_IO_OK;
}
static int spindle_stopped(const sim_io *io) {
uint32_t index;
for (index = 0; index < io->state.spindle_count; ++index)
if (fabs(io->state.spindles[index].speed_rpm) > io->state.spindles[index].tolerance_rpm || io->state.spindles[index].command_rpm != 0.0) return 0;
return 1;
}
int sim_io_tool_prepare(sim_io *io, int32_t tool, int32_t pocket) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (tool < 0 || pocket <= 0 || (uint32_t)pocket >= io->state.tool_capacity) return SIM_IO_ERR_RANGE;
if (io->state.estop) return SIM_IO_ERR_ESTOP;
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
if (io->state.fault) return SIM_IO_ERR_FAULT;
io->state.prepared_tool = tool; io->state.prepared_pocket = pocket;
io->state.tool_prepare = 1; io->state.tool_prepared = 0; io->state.tool_changed = 0;
io->state.tool_state = SIM_IO_TOOL_PREPARING;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_set_prepared(sim_io *io, uint32_t prepared) {
if (!valid_io(io) || prepared > 1U) return SIM_IO_ERR_ARGUMENT;
if (io->state.tool_state != SIM_IO_TOOL_PREPARING) return SIM_IO_ERR_STATE;
if (!prepared) return SIM_IO_OK;
io->state.tool_prepare = 0; io->state.tool_prepared = 1; io->state.tool_state = SIM_IO_TOOL_PREPARED;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_change(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (io->state.estop) return SIM_IO_ERR_ESTOP;
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
if (io->state.fault) return SIM_IO_ERR_FAULT;
if (io->state.tool_state != SIM_IO_TOOL_PREPARED || !io->state.tool_prepared) return SIM_IO_ERR_STATE;
if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
io->state.tool_change = 1; io->state.tool_changed = 0; io->state.tool_state = SIM_IO_TOOL_CHANGING;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_set_changed(sim_io *io, uint32_t changed) {
if (!valid_io(io) || changed > 1U) return SIM_IO_ERR_ARGUMENT;
if (io->state.tool_state != SIM_IO_TOOL_CHANGING) return SIM_IO_ERR_STATE;
if (!changed) return SIM_IO_OK;
io->state.tool_change = 0; io->state.tool_changed = 1; io->state.tool_prepared = 0;
io->state.current_tool = io->state.prepared_tool; io->state.tool_state = SIM_IO_TOOL_READY;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_abort(sim_io *io, int32_t reason) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
io->state.tool_prepare = 0; io->state.tool_change = 0; io->state.tool_prepared = 0;
io->state.tool_changed = 0; io->state.toolchanger_reason = reason;
io->state.tool_state = SIM_IO_TOOL_ABORTED;
io->state.last_error = SIM_IO_ERR_RECOVERY;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_set_fault(sim_io *io, uint32_t fault, int32_t reason) {
if (!valid_io(io) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.toolchanger_fault = fault; io->state.toolchanger_reason = reason;
io->state.fault = fault; io->state.fault_reason = reason;
if (fault) { io->state.tool_state = SIM_IO_TOOL_FAULT; io->state.tool_prepare = 0; io->state.tool_change = 0; }
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_tool_clear_fault(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
io->state.fault = 0; io->state.fault_reason = 0;
if (io->state.tool_state == SIM_IO_TOOL_FAULT || io->state.tool_state == SIM_IO_TOOL_ABORTED) io->state.tool_state = SIM_IO_TOOL_IDLE;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_probe_arm(sim_io *io, uint32_t probe_type) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (io->state.estop) return SIM_IO_ERR_ESTOP;
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
io->state.probe.probe_type = probe_type; io->state.probe.state = SIM_IO_PROBE_ARMED;
io->state.probe.tripped = 0; io->state.probe.tick = io->state.tick;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_probe_update(sim_io *io, const sim_posemath_pose *pose, uint32_t input) {
uint32_t when_clears;
if (!valid_io(io) || pose == 0) return SIM_IO_ERR_ARGUMENT;
if (!finite_pose(pose)) return SIM_IO_ERR_NONFINITE;
if (input > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.probe.input = input;
when_clears = (io->state.probe.probe_type & 2U) != 0;
if (io->state.probe.state == SIM_IO_PROBE_ARMED && ((input != 0U) ^ when_clears)) {
io->state.probe.position = *pose; io->state.probe.tripped = 1;
io->state.probe.state = SIM_IO_PROBE_TRIPPED; io->state.probe.tick = io->state.tick;
}
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_probe_disarm(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (io->state.probe.state == SIM_IO_PROBE_ARMED) io->state.probe.state = SIM_IO_PROBE_IDLE;
io->state.probe.probe_type = 0; io->state.probe.tripped = 0;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_set_estop(sim_io *io, uint32_t estop) {
uint32_t index;
if (!valid_io(io) || estop > 1U) return SIM_IO_ERR_ARGUMENT;
io->state.estop = estop;
if (estop) {
io->state.ready = 0; io->state.tool_prepare = 0; io->state.tool_change = 0;
io->state.tool_prepared = 0; io->state.tool_changed = 0;
io->state.tool_state = SIM_IO_TOOL_ABORTED;
for (index = 0; index < io->state.spindle_count; ++index) {
io->state.spindles[index].command_rpm = 0.0; io->state.spindles[index].enabled = 0;
io->state.spindles[index].brake = 1;
}
io->state.last_error = SIM_IO_ERR_ESTOP;
}
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_recover(sim_io *io) {
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (io->state.estop) return SIM_IO_ERR_ESTOP;
if (io->state.fault || io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
io->state.ready = 1; io->state.recovery_epoch += 1; io->state.tool_state = SIM_IO_TOOL_IDLE;
io->state.last_error = SIM_IO_OK;
mirror_pins(io);
return SIM_IO_OK;
}
int sim_io_step(sim_io *io, uint32_t ticks) {
uint32_t tick, index;
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
if (ticks == 0) return SIM_IO_OK;
if (ticks > 1000000U || UINT32_MAX - io->state.tick < ticks) return SIM_IO_ERR_RANGE;
if (sim_hal_step(ticks) != SIM_HAL_OK) return SIM_IO_ERR_PIN;
for (tick = 0; tick < ticks; ++tick) {
io->state.tick += 1;
for (index = 0; index < io->state.spindle_count; ++index) {
sim_io_spindle_state *spindle = &io->state.spindles[index];
double target = spindle->command_rpm;
double delta = spindle->accel_rpm_s * io->cycle_time;
if (spindle->amp_fault || spindle->inhibit || !io->state.ready || io->state.estop) target = 0.0;
if (spindle->speed_rpm < target) spindle->speed_rpm = fmin(target, spindle->speed_rpm + delta);
else if (spindle->speed_rpm > target) spindle->speed_rpm = fmax(target, spindle->speed_rpm - delta);
spindle->forward = spindle->speed_rpm > spindle->tolerance_rpm;
spindle->reverse = spindle->speed_rpm < -spindle->tolerance_rpm;
spindle->brake = fabs(target - spindle->speed_rpm) > spindle->tolerance_rpm && target == 0.0;
spindle->at_speed = fabs(spindle->speed_rpm - target) <= spindle->tolerance_rpm;
spindle->revs += spindle->speed_rpm * io->cycle_time / 60.0;
spindle->tick = io->state.tick;
if (spindle->index_enable && floor(spindle->revs) != floor(spindle->revs - spindle->speed_rpm * io->cycle_time / 60.0)) {
spindle->index_enable = 0; spindle->revs = spindle->revs - floor(spindle->revs);
}
if (spindle->orient && spindle->at_speed && spindle->speed_rpm == 0.0) {
spindle->orient = 0; spindle->locked = 1; spindle->is_oriented = 1;
}
}
}
mirror_pins(io);
set_last(io, SIM_IO_OK);
return SIM_IO_OK;
}
int sim_io_read_state(const sim_io *io, sim_io_state *state) {
if (!valid_io(io) || state == 0) return SIM_IO_ERR_ARGUMENT;
*state = io->state;
return SIM_IO_OK;
}
uint64_t sim_io_state_hash(const sim_io *io) {
unsigned char bytes[8192];
size_t offset = 0;
uint32_t index;
if (!valid_io(io)) return 0;
#define APPEND(value) do { memcpy(bytes + offset, &(value), sizeof(value)); offset += sizeof(value); } while (0)
APPEND(io->state.ready); APPEND(io->state.estop); APPEND(io->state.fault); APPEND(io->state.fault_reason);
APPEND(io->state.tick); APPEND(io->state.recovery_epoch); APPEND(io->state.spindle_count); APPEND(io->state.tool_capacity);
for (index = 0; index < io->state.spindle_count; ++index) {
sim_io_spindle_state spindle = io->state.spindles[index];
spindle.command_rpm = normalized(spindle.command_rpm); spindle.speed_rpm = normalized(spindle.speed_rpm);
spindle.max_rpm = normalized(spindle.max_rpm); spindle.accel_rpm_s = normalized(spindle.accel_rpm_s);
spindle.tolerance_rpm = normalized(spindle.tolerance_rpm); spindle.orient_angle = normalized(spindle.orient_angle);
spindle.revs = normalized(spindle.revs); APPEND(spindle);
}
APPEND(io->state.tool_state); APPEND(io->state.current_tool); APPEND(io->state.prepared_tool); APPEND(io->state.prepared_pocket);
APPEND(io->state.tool_prepare); APPEND(io->state.tool_prepared); APPEND(io->state.tool_change); APPEND(io->state.tool_changed);
APPEND(io->state.toolchanger_fault); APPEND(io->state.toolchanger_reason); APPEND(io->state.probe.input);
APPEND(io->state.probe.probe_type); APPEND(io->state.probe.state); APPEND(io->state.probe.tripped); APPEND(io->state.probe.tick);
{ sim_posemath_pose pose = io->state.probe.position; double *values = &pose.tran.x; for (index = 0; index < 9; ++index) values[index] = normalized(values[index]); APPEND(pose); }
APPEND(io->state.last_error);
#undef APPEND
return io_fnv1a64(bytes, offset);
}
uint32_t sim_io_canonical(const sim_io *io, char *output, uint32_t capacity) {
char discard[1];
char *destination = output == 0 ? discard : output;
uint32_t destination_capacity = output == 0 ? 1 : capacity;
int written;
uint32_t index;
if (!valid_io(io)) return 0;
written = snprintf(destination, destination_capacity,
"{\"estop\":%u,\"fault\":%u,\"fault_reason\":%d,\"last_error\":%d,\"probe\":{\"input\":%u,\"position\":[%.9g,%.9g,%.9g],\"state\":%u,\"tick\":%u,\"tripped\":%u},\"ready\":%u,\"recovery_epoch\":%u,\"spindles\":[",
io->state.estop, io->state.fault, io->state.fault_reason, io->state.last_error,
io->state.probe.input, io->state.probe.position.tran.x, io->state.probe.position.tran.y,
io->state.probe.position.tran.z, io->state.probe.state, io->state.probe.tick,
io->state.probe.tripped, io->state.ready, io->state.recovery_epoch);
if (written < 0) return 0;
for (index = 0; index < io->state.spindle_count; ++index) {
const sim_io_spindle_state *spindle = &io->state.spindles[index];
int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
"%s{\"at_speed\":%u,\"brake\":%u,\"command_rpm\":%.9g,\"forward\":%u,\"reverse\":%u,\"speed_rpm\":%.9g,\"tick\":%u}",
index ? "," : "", spindle->at_speed, spindle->brake, spindle->command_rpm,
spindle->forward, spindle->reverse, spindle->speed_rpm, spindle->tick);
if (part < 0) return 0; written += part;
}
{ int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
"],\"tick\":%u,\"tool\":{\"changed\":%u,\"current\":%d,\"prepared\":%u,\"prepared_pocket\":%d,\"prepared_tool\":%d,\"prepare\":%u,\"state\":%u}}",
io->state.tick, io->state.tool_changed, io->state.current_tool, io->state.tool_prepared,
io->state.prepared_pocket, io->state.prepared_tool, io->state.tool_prepare, io->state.tool_state);
if (part < 0) return 0; written += part; }
return (uint32_t)written + 1;
}

36
wasm-core/src/sim_kinematics.c
View File

@@ -19,6 +19,14 @@ static double hash_normalize(double value) {
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}
/* The five-leg Newton solve can differ by a few ulps between native and
* wasm32 math libraries. Keep the public pose/info boundary deterministic
* without changing the convergence test itself. */
static double pentapod_canonical(double value) {
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
}
static int finite_values(const double *values, size_t count) {
size_t index;
if (values == 0) return 0;
@@ -168,6 +176,7 @@ static int valid_config(const sim_kinematics_config *config) {
}
if (config->type == SIM_KIN_PENTAPOD) {
if (kinematics_count != SIM_KIN_PENTA_STRUTS || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B')) return SIM_KIN_ERR_CONFIGURATION;
if (!isfinite(config->penta_tool_offset)) return SIM_KIN_ERR_NONFINITE;
if (config->penta_iteration_limit > 100000u || config->penta_convergence_tolerance < 0.0 || !isfinite(config->penta_convergence_tolerance) || config->penta_max_error < 0.0 || !isfinite(config->penta_max_error) || !finite_values(config->penta_initial_pose, 5)) return SIM_KIN_ERR_CONFIGURATION;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (!finite_values(config->penta_base[index], 3) || !isfinite(config->penta_effector_radius[index]) || !isfinite(config->penta_effector_z[index]) || ((config->penta_effector_radius[index] < 0.0) || (config->penta_effector_radius[index] == 0.0 && config->penta_effector_z[index] != 0.0))) return SIM_KIN_ERR_CONFIGURATION;
}
@@ -269,7 +278,10 @@ int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config
static const double effector_z[SIM_KIN_PENTA_STRUTS] = {87.0, 114.5, 140.5, 232.5, 258.5};
int all_zero = 1;
size_t index;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0) all_zero = 0;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0 ||
kinematics->config.penta_effector_radius[index] != 0.0 || kinematics->config.penta_effector_z[index] != 0.0) all_zero = 0;
}
if (all_zero) { memcpy(kinematics->config.penta_base, base, sizeof(base)); memcpy(kinematics->config.penta_effector_radius, radius, sizeof(radius)); memcpy(kinematics->config.penta_effector_z, effector_z, sizeof(effector_z)); }
if (kinematics->config.penta_iteration_limit == 0) kinematics->config.penta_iteration_limit = 120;
if (kinematics->config.penta_convergence_tolerance == 0.0) kinematics->config.penta_convergence_tolerance = 1e-9;
@@ -808,7 +820,10 @@ static int pentapod_lengths(const sim_kinematics *k, const double q[5], double l
PmRotationMatrix rotation, inverse;
if (pmRpyMatConvert(&rpy, &rotation) != 0 || pmMatInv(&rotation, &inverse) != 0) return SIM_KIN_ERR_SINGULAR;
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] - q[2]};
/* pentakins_read_hal_pins applies tool-offset to both ends of each
* strut; preserve that world-frame translation before rotating into
* the effector frame. */
PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] + k->config.penta_tool_offset - q[2]};
PmCartesian local;
double radial;
pmMatCartMult(&inverse, &base, &local);
@@ -833,6 +848,14 @@ static int pentapod_forward(const sim_kinematics *k, const double *joints, uint6
double lengths[SIM_KIN_PENTA_STRUTS];
double residual = 0.0;
uint32_t iteration;
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
/* LinuxCNC's pentakins rejects non-positive strut lengths before
* entering Newton iteration. */
if (joints[index] <= 0.0) {
if (info != 0) { info->iterations = 0; info->residual = 0.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_ERR_RANGE;
}
}
for (size_t index = 0; index < 5; ++index) q[index] = k->config.penta_initial_pose[index];
if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
for (iteration = 1; iteration <= k->config.penta_iteration_limit; ++iteration) {
@@ -853,11 +876,14 @@ static int pentapod_forward(const sim_kinematics *k, const double *joints, uint6
}
}
if (residual <= k->config.penta_convergence_tolerance) {
pose->tran.x = q[0]; pose->tran.y = q[1]; pose->tran.z = q[2]; pose->a = q[3] * RAD_TO_DEG; pose->b = q[4] * RAD_TO_DEG; pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
pose->tran.x = pentapod_canonical(q[0]); pose->tran.y = pentapod_canonical(q[1]); pose->tran.z = pentapod_canonical(q[2]); pose->a = pentapod_canonical(q[3] * RAD_TO_DEG); pose->b = pentapod_canonical(q[4] * RAD_TO_DEG); pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
if (info != 0) { info->iterations = iteration; info->residual = nearbyint(residual * 1000000000.0) / 1000000000.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_OK;
}
if (residual > k->config.penta_max_error) return SIM_KIN_ERR_RANGE;
if (residual > k->config.penta_max_error) {
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_ERR_RANGE;
}
if (solve_linear5(jacobian, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
for (int column = 0; column < 5; ++column) q[column] -= delta[column];
}

109
wasm-core/src/sim_kinematics_feedback.c Normal file
View File

@@ -0,0 +1,109 @@
#include "sim_kinematics_feedback.h"
#include <math.h>
#include <string.h>
static uint64_t feedback_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 void state_zero(sim_kinematics_feedback_state *state) {
memset(state, 0, sizeof(*state));
state->last_error = SIM_KIN_FEEDBACK_OK;
state->kinematics_error = SIM_KIN_OK;
}
int sim_kinematics_feedback_init(sim_kinematics_feedback *feedback,
sim_kinematics *kinematics,
uint32_t joint_count) {
uint32_t configured_count;
if (feedback == 0 || kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (!kinematics->configured) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
configured_count = kinematics->config.kinematics_joint_count == 0 ?
kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
if (joint_count == 0 || joint_count != configured_count || joint_count > SIM_KIN_MAX_JOINTS)
return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
memset(feedback, 0, sizeof(*feedback));
feedback->kinematics = kinematics;
feedback->state.joint_count = joint_count;
state_zero(&feedback->state);
feedback->state.joint_count = joint_count;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_reset(sim_kinematics_feedback *feedback) {
uint32_t joint_count;
if (feedback == 0 || feedback->kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
joint_count = feedback->state.joint_count;
state_zero(&feedback->state);
feedback->state.joint_count = joint_count;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_update(sim_kinematics_feedback *feedback,
const double *joints, uint32_t joint_count,
uint64_t inverse_flags,
sim_kinematics_result_info *info) {
sim_posemath_pose pose;
sim_kinematics_result_info local_info;
uint32_t index;
int result;
if (feedback == 0 || feedback->kinematics == 0 || joints == 0)
return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (joint_count != feedback->state.joint_count) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
for (index = 0; index < joint_count; ++index)
if (!isfinite(joints[index])) {
feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_NONFINITE;
feedback->state.kinematics_error = SIM_KIN_ERR_NONFINITE;
return feedback->state.last_error;
}
memset(&pose, 0, sizeof(pose));
result = sim_kinematics_forward_ex(feedback->kinematics, joints, joint_count,
inverse_flags, &pose, &local_info);
if (result != SIM_KIN_OK) {
feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_FORWARD;
feedback->state.kinematics_error = result;
if (info != 0) *info = local_info;
return feedback->state.last_error;
}
feedback->state.pose = pose;
memcpy(feedback->state.joints, joints, sizeof(double) * joint_count);
feedback->state.forward_flags = local_info.forward_flags;
feedback->state.inverse_flags = local_info.inverse_flags;
feedback->state.tick += 1;
feedback->state.valid = 1;
feedback->state.last_error = SIM_KIN_FEEDBACK_OK;
feedback->state.kinematics_error = SIM_KIN_OK;
if (info != 0) *info = local_info;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_read(const sim_kinematics_feedback *feedback,
sim_kinematics_feedback_state *state) {
if (feedback == 0 || state == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (!feedback->state.valid) return SIM_KIN_FEEDBACK_ERR_STATE;
*state = feedback->state;
return SIM_KIN_FEEDBACK_OK;
}
uint64_t sim_kinematics_feedback_state_hash(const sim_kinematics_feedback *feedback) {
unsigned char bytes[sizeof(sim_kinematics_feedback_state)];
sim_kinematics_feedback_state state;
size_t offset = 0;
uint32_t index;
if (feedback == 0) return 0;
state = feedback->state;
for (index = 0; index < 9; ++index) {
double *pose_value = &state.pose.tran.x;
pose_value[index] = isfinite(pose_value[index]) ? nearbyint(pose_value[index] * 1000000000.0) / 1000000000.0 : pose_value[index];
if (index < state.joint_count) state.joints[index] = isfinite(state.joints[index]) ? nearbyint(state.joints[index] * 1000000000.0) / 1000000000.0 : state.joints[index];
}
memcpy(bytes + offset, &state, sizeof(state)); offset += sizeof(state);
return feedback_fnv1a64(bytes, offset);
}

138
wasm-core/src/sim_kinematics_home_limit.c Normal file
View File

@@ -0,0 +1,138 @@
#include "sim_kinematics_home_limit.h"
#include <math.h>
#include <string.h>
static uint64_t home_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 void state_zero(sim_kinematics_home_limit_state *state) {
memset(state, 0, sizeof(*state));
state->last_error = SIM_KIN_HOME_OK;
state->limit_error = SIM_KIN_OK;
state->kinematics_error = SIM_KIN_OK;
}
int sim_kinematics_home_limit_init(sim_kinematics_home_limit *home_limit,
sim_kinematics *kinematics,
uint32_t joint_count) {
uint32_t configured_count;
if (home_limit == 0 || kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (!kinematics->configured) return SIM_KIN_HOME_ERR_CONFIGURATION;
configured_count = kinematics->config.kinematics_joint_count == 0 ?
kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
if (joint_count == 0 || joint_count != configured_count || joint_count > SIM_KIN_MAX_JOINTS)
return SIM_KIN_HOME_ERR_CONFIGURATION;
memset(home_limit, 0, sizeof(*home_limit));
home_limit->kinematics = kinematics;
home_limit->state.joint_count = joint_count;
state_zero(&home_limit->state);
home_limit->state.joint_count = joint_count;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_reset(sim_kinematics_home_limit *home_limit) {
uint32_t joint_count;
if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
joint_count = home_limit->state.joint_count;
state_zero(&home_limit->state);
home_limit->state.joint_count = joint_count;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_home(sim_kinematics_home_limit *home_limit,
uint32_t home_sequence) {
sim_posemath_pose pose;
double joints[SIM_KIN_MAX_JOINTS] = {0};
int result;
if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (home_sequence > 0xffffu) return SIM_KIN_HOME_ERR_SEQUENCE;
result = sim_kinematics_home(home_limit->kinematics, joints,
home_limit->state.joint_count, &pose);
if (result != SIM_KIN_OK) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
home_limit->state.kinematics_error = result;
return home_limit->state.last_error;
}
memcpy(home_limit->state.joints, joints, sizeof(double) * home_limit->state.joint_count);
home_limit->state.pose = pose;
home_limit->state.home_sequence = home_sequence;
home_limit->state.homed = 1;
home_limit->state.tick += 1;
home_limit->state.last_error = SIM_KIN_HOME_OK;
home_limit->state.limit_error = SIM_KIN_OK;
home_limit->state.kinematics_error = SIM_KIN_OK;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_check(sim_kinematics_home_limit *home_limit,
const double *joints,
uint32_t joint_count) {
sim_posemath_pose pose;
uint32_t index;
int result;
if (home_limit == 0 || joints == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (!home_limit->state.homed) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_UNHOMED;
return home_limit->state.last_error;
}
if (joint_count != home_limit->state.joint_count) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_CONFIGURATION;
return home_limit->state.last_error;
}
for (index = 0; index < joint_count; ++index) {
if (!isfinite(joints[index])) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_NONFINITE;
return home_limit->state.last_error;
}
if (home_limit->kinematics->config.limit_enabled[index] &&
(joints[index] < home_limit->kinematics->config.joint_min[index] ||
joints[index] > home_limit->kinematics->config.joint_max[index])) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_LIMIT;
home_limit->state.limit_error = SIM_KIN_ERR_RANGE;
return home_limit->state.last_error;
}
}
result = sim_kinematics_forward(home_limit->kinematics, joints, joint_count, &pose);
if (result != SIM_KIN_OK) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
home_limit->state.kinematics_error = result;
return home_limit->state.last_error;
}
memcpy(home_limit->state.joints, joints, sizeof(double) * joint_count);
home_limit->state.pose = pose;
home_limit->state.tick += 1;
home_limit->state.last_error = SIM_KIN_HOME_OK;
home_limit->state.limit_error = SIM_KIN_OK;
home_limit->state.kinematics_error = SIM_KIN_OK;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_read(const sim_kinematics_home_limit *home_limit,
sim_kinematics_home_limit_state *state) {
if (home_limit == 0 || state == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
*state = home_limit->state;
return SIM_KIN_HOME_OK;
}
uint64_t sim_kinematics_home_limit_state_hash(const sim_kinematics_home_limit *home_limit) {
sim_kinematics_home_limit_state state;
unsigned char bytes[sizeof(state)];
uint32_t index;
if (home_limit == 0) return 0;
state = home_limit->state;
for (index = 0; index < 9; ++index) {
double *pose_value = &state.pose.tran.x;
pose_value[index] = isfinite(pose_value[index]) ? nearbyint(pose_value[index] * 1000000000.0) / 1000000000.0 : pose_value[index];
if (index < state.joint_count) state.joints[index] = isfinite(state.joints[index]) ? nearbyint(state.joints[index] * 1000000000.0) / 1000000000.0 : state.joints[index];
}
memcpy(bytes, &state, sizeof(state));
return home_fnv1a64(bytes, sizeof(bytes));
}

184
wasm-core/src/sim_kinematics_motion.c Normal file
View File

@@ -0,0 +1,184 @@
#include "sim_kinematics_motion.h"
#include <math.h>
#include <string.h>
static uint64_t motion_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 double normalized(double value) {
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}
static void set_error(sim_kinematics_motion *adapter, int error,
int planner_error, int kinematics_error) {
adapter->last_error = error;
adapter->planner_error = planner_error;
adapter->kinematics_error = kinematics_error;
}
int sim_kinematics_motion_reset(sim_kinematics_motion *adapter) {
if (adapter == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
if (adapter->kinematics == 0 || !adapter->kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
if (sim_tp_reset(&adapter->planner) != SIM_POSEMATH_OK) return SIM_KIN_MOTION_ERR_PLANNER;
adapter->head = 0;
adapter->count = 0;
adapter->tick = 0;
adapter->paused = 0;
memset(&adapter->last_pose, 0, sizeof(adapter->last_pose));
memset(adapter->last_joints, 0, sizeof(adapter->last_joints));
set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
return SIM_KIN_MOTION_OK;
}
int sim_kinematics_motion_init(sim_kinematics_motion *adapter,
sim_kinematics *kinematics, double cycle_time,
double velocity_limit, double acceleration_limit) {
uint32_t joint_count;
int result;
if (adapter == 0 || kinematics == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
if (!kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
joint_count = kinematics->config.kinematics_joint_count == 0 ?
kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
if (joint_count == 0 || joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_MOTION_ERR_CONFIGURATION;
memset(adapter, 0, sizeof(*adapter));
result = sim_tp_init(&adapter->planner, cycle_time, velocity_limit, acceleration_limit);
if (result != SIM_POSEMATH_OK) return SIM_KIN_MOTION_ERR_CONFIGURATION;
adapter->kinematics = kinematics;
adapter->joint_count = joint_count;
return sim_kinematics_motion_reset(adapter);
}
int sim_kinematics_motion_enqueue(sim_kinematics_motion *adapter,
const sim_posemath_segment *segment,
uint32_t *id_out) {
int result;
if (adapter == 0 || segment == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
result = sim_tp_enqueue(&adapter->planner, segment, id_out);
if (result != SIM_POSEMATH_OK) {
set_error(adapter, result == SIM_POSEMATH_ERR_NONFINITE ? SIM_KIN_MOTION_ERR_NONFINITE : SIM_KIN_MOTION_ERR_PLANNER, result, SIM_KIN_OK);
return adapter->last_error;
}
set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
return SIM_KIN_MOTION_OK;
}
int sim_kinematics_motion_pause(sim_kinematics_motion *adapter, uint32_t paused) {
if (adapter == 0 || paused > 1) return SIM_KIN_MOTION_ERR_ARGUMENT;
adapter->paused = paused;
return SIM_KIN_MOTION_OK;
}
int sim_kinematics_motion_step(sim_kinematics_motion *adapter) {
sim_tp planner_snapshot;
sim_kinematics kinematics_snapshot;
sim_tp_state planner_state;
sim_kinematics_motion_sample sample;
sim_kinematics_result_info info;
uint32_t index;
int result;
if (adapter == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
if (adapter->kinematics == 0 || !adapter->kinematics->configured) return SIM_KIN_MOTION_ERR_CONFIGURATION;
if (adapter->paused) {
set_error(adapter, SIM_KIN_MOTION_ERR_STATE, SIM_POSEMATH_OK, SIM_KIN_OK);
return adapter->last_error;
}
if (adapter->count >= SIM_KIN_MOTION_QUEUE_CAPACITY) {
set_error(adapter, SIM_KIN_MOTION_ERR_CAPACITY, SIM_POSEMATH_OK, SIM_KIN_OK);
return adapter->last_error;
}
planner_snapshot = adapter->planner;
kinematics_snapshot = *adapter->kinematics;
result = sim_tp_step(&adapter->planner);
if (result != SIM_POSEMATH_OK) {
adapter->planner = planner_snapshot;
set_error(adapter, SIM_KIN_MOTION_ERR_PLANNER, result, SIM_KIN_OK);
return adapter->last_error;
}
if (sim_tp_read_state(&adapter->planner, &planner_state) != SIM_POSEMATH_OK) {
adapter->planner = planner_snapshot;
set_error(adapter, SIM_KIN_MOTION_ERR_PLANNER, SIM_POSEMATH_ERR_STATE, SIM_KIN_OK);
return adapter->last_error;
}
memset(&sample, 0, sizeof(sample));
sample.pose = planner_state.position;
result = sim_kinematics_inverse_ex(adapter->kinematics, &sample.pose,
adapter->joint_count, 0, sample.joints,
&info);
if (result != SIM_KIN_OK) {
adapter->planner = planner_snapshot;
*adapter->kinematics = kinematics_snapshot;
set_error(adapter, result == SIM_KIN_ERR_NONFINITE ? SIM_KIN_MOTION_ERR_NONFINITE : SIM_KIN_MOTION_ERR_INVERSE, SIM_POSEMATH_OK, result);
return adapter->last_error;
}
sample.joint_count = adapter->joint_count;
sample.tick = planner_state.tick;
sample.planner_id = planner_state.active_id;
index = (adapter->head + adapter->count) % SIM_KIN_MOTION_QUEUE_CAPACITY;
adapter->samples[index] = sample;
adapter->count += 1;
adapter->tick = sample.tick;
adapter->last_pose = sample.pose;
memcpy(adapter->last_joints, sample.joints, sizeof(sample.joints));
set_error(adapter, SIM_KIN_MOTION_OK, SIM_POSEMATH_OK, SIM_KIN_OK);
return SIM_KIN_MOTION_OK;
}
int sim_kinematics_motion_peek(const sim_kinematics_motion *adapter,
sim_kinematics_motion_sample *sample) {
if (adapter == 0 || sample == 0) return SIM_KIN_MOTION_ERR_ARGUMENT;
if (adapter->count == 0) return SIM_KIN_MOTION_ERR_STATE;
*sample = adapter->samples[adapter->head];
return SIM_KIN_MOTION_OK;
}
int sim_kinematics_motion_pop(sim_kinematics_motion *adapter,
sim_kinematics_motion_sample *sample) {
int result = sim_kinematics_motion_peek(adapter, sample);
if (result != SIM_KIN_MOTION_OK) return result;
adapter->head = (adapter->head + 1) % SIM_KIN_MOTION_QUEUE_CAPACITY;
adapter->count -= 1;
return SIM_KIN_MOTION_OK;
}
uint32_t sim_kinematics_motion_depth(const sim_kinematics_motion *adapter) {
return adapter == 0 ? 0 : adapter->count;
}
uint64_t sim_kinematics_motion_state_hash(const sim_kinematics_motion *adapter) {
unsigned char bytes[16384];
size_t offset = 0;
uint64_t planner_hash;
uint64_t kinematics_hash;
uint32_t sample_index;
if (adapter == 0 || adapter->kinematics == 0) return 0;
planner_hash = sim_tp_state_hash(&adapter->planner);
kinematics_hash = sim_kinematics_state_hash(adapter->kinematics, adapter->last_joints,
adapter->joint_count, &adapter->last_pose);
#define APPEND(value) do { memcpy(bytes + offset, &(value), sizeof(value)); offset += sizeof(value); } while (0)
APPEND(adapter->head); APPEND(adapter->count); APPEND(adapter->tick);
APPEND(adapter->joint_count); APPEND(adapter->paused); APPEND(adapter->last_error);
APPEND(adapter->planner_error); APPEND(adapter->kinematics_error);
APPEND(planner_hash); APPEND(kinematics_hash);
for (sample_index = 0; sample_index < adapter->count; ++sample_index) {
const sim_kinematics_motion_sample *sample = &adapter->samples[(adapter->head + sample_index) % SIM_KIN_MOTION_QUEUE_CAPACITY];
sim_posemath_pose pose = sample->pose;
double joints[SIM_KIN_MAX_JOINTS] = {0};
double *pose_values = &pose.tran.x;
uint32_t joint_index;
for (joint_index = 0; joint_index < 9; ++joint_index) pose_values[joint_index] = normalized(pose_values[joint_index]);
for (joint_index = 0; joint_index < sample->joint_count; ++joint_index) joints[joint_index] = normalized(sample->joints[joint_index]);
memcpy(bytes + offset, &pose, sizeof(pose)); offset += sizeof(pose);
memcpy(bytes + offset, joints, sizeof(joints)); offset += sizeof(joints);
APPEND(sample->joint_count); APPEND(sample->tick); APPEND(sample->planner_id);
}
#undef APPEND
return motion_fnv1a64(bytes, offset);
}

124
wasm-core/src/sim_kinematics_registry.c Normal file
View File

@@ -0,0 +1,124 @@
#include "sim_kinematics.h"
#include <string.h>
#define COMMON (SIM_KIN_CAP_FORWARD | SIM_KIN_CAP_INVERSE | SIM_KIN_CAP_SWITCHABLE)
#define BASE_PARAMS (SIM_KIN_PARAM_MAPPING | SIM_KIN_PARAM_LIMITS)
static const sim_kinematics_capability registry[] = {
{1, SIM_KIN_TRIVKINS, "trivkins", "", 1, 9, COMMON | SIM_KIN_CAP_IDENTITY | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS},
{1, SIM_KIN_XYZAC_TRT, "xyzac-trt-kins", "XYZAC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
{1, SIM_KIN_XYZBC_TRT, "xyzbc-trt-kins", "XYZBC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
{1, SIM_KIN_XYZBCW, "xyzbcw", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
{1, SIM_KIN_COREXY, "corexykins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
{1, SIM_KIN_ROSE, "rosekins", "XYZ", 3, 3, COMMON, BASE_PARAMS},
{1, SIM_KIN_MAXKINS, "maxkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
{1, SIM_KIN_ROTATEKINS, "rotatekins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
{1, SIM_KIN_5AXISKINS, "5axiskins", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
{1, SIM_KIN_SCARA, "scarakins", "XYZCAB", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_PUMA, "pumakins", "XYZABC", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_SCORBOT, "scorbot-kins", "XYZAB", 5, 5, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_GENSER, "genserkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_DH | SIM_KIN_PARAM_SOLVER},
{1, SIM_KIN_LINEAR_DELTA, "lineardeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_ROTARY_DELTA, "rotarydeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_TRIPOD, "tripodkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
{1, SIM_KIN_GENHEX, "genhexkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
{1, SIM_KIN_PENTAPOD, "pentakins", "XYZAB", 5, 5, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
};
static uint64_t registry_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 path_like(const char *name) {
size_t index;
if (name == 0 || name[0] == '\0') return 0;
for (index = 0; name[index] != '\0'; ++index)
if (name[index] == '/' || name[index] == '\\') return 1;
return 0;
}
static int has_coordinate_casefold(const char *coordinates, char wanted) {
size_t index;
for (index = 0; coordinates[index] != '\0'; ++index) {
char value = coordinates[index];
if (value >= 'a' && value <= 'z') value = (char)(value - 'a' + 'A');
if (value == wanted) return 1;
}
return 0;
}
uint32_t sim_kinematics_registry_count(void) {
return (uint32_t)(sizeof(registry) / sizeof(registry[0]));
}
int sim_kinematics_registry_get(uint32_t index,
sim_kinematics_capability *capability) {
if (capability == 0) return SIM_KIN_ERR_ARGUMENT;
if (index >= sim_kinematics_registry_count()) return SIM_KIN_ERR_RANGE;
*capability = registry[index];
return SIM_KIN_OK;
}
int sim_kinematics_registry_find(const char *name,
sim_kinematics_capability *capability) {
uint32_t index;
if (name == 0 || capability == 0) return SIM_KIN_ERR_ARGUMENT;
if (path_like(name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
for (index = 0; index < sim_kinematics_registry_count(); ++index) {
if (strcmp(name, registry[index].name) == 0) {
*capability = registry[index];
return SIM_KIN_OK;
}
}
return SIM_KIN_ERR_UNKNOWN_TYPE;
}
int sim_kinematics_registry_require(const char *name, uint32_t capabilities,
uint64_t parameter_mask,
sim_kinematics_capability *capability) {
sim_kinematics_capability found;
int result = sim_kinematics_registry_find(name, &found);
if (result != SIM_KIN_OK) return result;
if ((found.capabilities & capabilities) != capabilities ||
(found.parameter_mask & parameter_mask) != parameter_mask) return SIM_KIN_ERR_CAPABILITY;
if (capability != 0) *capability = found;
return SIM_KIN_OK;
}
int sim_kinematics_registry_validate_config(const char *name,
const sim_kinematics_config *config,
sim_kinematics_capability *capability) {
sim_kinematics_capability found;
sim_kinematics instance;
const char *coordinates;
uint32_t joint_count;
size_t index;
int result;
if (config == 0) return SIM_KIN_ERR_ARGUMENT;
result = sim_kinematics_registry_find(name, &found);
if (result != SIM_KIN_OK) return result;
if (config->type != found.type) return SIM_KIN_ERR_CONFIGURATION;
joint_count = config->kinematics_joint_count == 0 ? config->joint_count : config->kinematics_joint_count;
if (joint_count < found.min_joints || joint_count > found.max_joints) return SIM_KIN_ERR_CONFIGURATION;
coordinates = config->coordinates[0] == '\0' ? "XYZABCUVW" : config->coordinates;
for (index = 0; found.required_coordinates[index] != '\0'; ++index)
if (!has_coordinate_casefold(coordinates, found.required_coordinates[index])) return SIM_KIN_ERR_CONFIGURATION;
result = sim_kinematics_init(&instance, config);
if (result != SIM_KIN_OK) return result;
if (capability != 0) *capability = found;
return SIM_KIN_OK;
}
uint64_t sim_kinematics_registry_state_hash(void) {
return registry_fnv1a64((const unsigned char *)registry, sizeof(registry));
}
#undef BASE_PARAMS
#undef COMMON

185
wasm-core/src/sim_switchkins.c Normal file
View File

@@ -0,0 +1,185 @@
#include "sim_kinematics.h"
#include <math.h>
#include <string.h>
static uint64_t switch_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_mode(int mode) {
return mode >= 0 && mode < (int)SIM_KIN_SWITCH_MAX_TYPES;
}
static void switch_info(sim_kinematics_result_info *info) {
if (info != 0) memset(info, 0, sizeof(*info));
}
int sim_kinematics_switch_init(sim_kinematics_switch *switcher, int default_mode,
uint8_t fwd_iterates_mask) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(default_mode)) return SIM_KIN_ERR_INVALID_MODE;
memset(switcher, 0, sizeof(*switcher));
switcher->active_mode = default_mode;
switcher->pending_mode = default_mode;
switcher->fwd_iterates_mask = fwd_iterates_mask & 0x07u;
return SIM_KIN_OK;
}
int sim_kinematics_switch_register(sim_kinematics_switch *switcher, int mode,
sim_kinematics *module, uint8_t fwd_iterates) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (module == 0 || !module->configured) return SIM_KIN_ERR_MISSING_MODULE;
switcher->modules[mode] = module;
switcher->registered[mode] = 1;
if (fwd_iterates) switcher->fwd_iterates_mask |= (uint8_t)(1u << mode);
else switcher->fwd_iterates_mask &= (uint8_t)~(1u << mode);
return SIM_KIN_OK;
}
int sim_kinematics_switch_begin(sim_kinematics_switch *switcher, int mode) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
switcher->pending_mode = mode;
switcher->switching = (uint8_t)(mode != switcher->active_mode);
return SIM_KIN_OK;
}
int sim_kinematics_switch_abort(sim_kinematics_switch *switcher) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
switcher->pending_mode = switcher->active_mode;
switcher->switching = 0;
switcher->abort_count += 1;
return SIM_KIN_OK;
}
int sim_kinematics_switch_commit(sim_kinematics_switch *switcher, double *joints,
uint32_t joint_count, sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_kinematics *target;
double remapped[SIM_KIN_MAX_JOINTS] = {0};
sim_kinematics_result_info local_info;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
target = switcher->modules[switcher->pending_mode];
if (target == 0 || !switcher->registered[switcher->pending_mode]) {
switcher->switching = 0;
switcher->pending_mode = switcher->active_mode;
return SIM_KIN_ERR_MISSING_MODULE;
}
if (joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_ERR_CONFIGURATION;
result = sim_kinematics_inverse_ex(target, pose, joint_count, 0, remapped,
&local_info);
if (result != SIM_KIN_OK) {
switcher->switching = 0;
switcher->pending_mode = switcher->active_mode;
if (info != 0) *info = local_info;
return result;
}
memcpy(joints, remapped, sizeof(double) * joint_count);
switcher->active_mode = switcher->pending_mode;
switcher->pending_mode = switcher->active_mode;
switcher->switching = 0;
switcher->generation += 1;
switcher->switch_count += 1;
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
if (info != 0) *info = local_info;
return SIM_KIN_OK;
}
int sim_kinematics_switch_forward(sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_kinematics *module;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
module = switcher->modules[switcher->active_mode];
if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
result = sim_kinematics_forward_ex(module, joints, joint_count, 0, pose, info);
if (result == SIM_KIN_OK) {
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
}
return result;
}
int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info) {
sim_kinematics *module;
int result;
switch_info(info);
if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
module = switcher->modules[switcher->active_mode];
if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
result = sim_kinematics_inverse_ex(module, pose, joint_count, 0, joints, info);
if (result == SIM_KIN_OK) {
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
}
return result;
}
int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_posemath_pose current_pose;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
if (mode == switcher->active_mode) return sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
result = sim_kinematics_switch_forward(switcher, joints, joint_count, &current_pose, info);
if (result != SIM_KIN_OK) return result;
result = sim_kinematics_switch_begin(switcher, mode);
if (result != SIM_KIN_OK) return result;
result = sim_kinematics_switch_commit(switcher, joints, joint_count, &current_pose, info);
if (result == SIM_KIN_OK) *pose = current_pose;
return result;
}
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher) {
return switcher == 0 ? SIM_KIN_ERR_ARGUMENT : switcher->active_mode;
}
uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose) {
unsigned char bytes[256];
size_t offset = 0;
uint64_t module_hash;
if (switcher == 0 || joints == 0 || pose == 0 || joint_count > SIM_KIN_MAX_JOINTS) return 0;
if (!valid_mode(switcher->active_mode) || switcher->modules[switcher->active_mode] == 0) return 0;
module_hash = sim_kinematics_state_hash(switcher->modules[switcher->active_mode], joints, joint_count, pose);
memcpy(bytes + offset, &switcher->active_mode, sizeof(switcher->active_mode)); offset += sizeof(switcher->active_mode);
memcpy(bytes + offset, &switcher->pending_mode, sizeof(switcher->pending_mode)); offset += sizeof(switcher->pending_mode);
memcpy(bytes + offset, &switcher->switching, sizeof(switcher->switching)); offset += sizeof(switcher->switching);
memcpy(bytes + offset, &switcher->generation, sizeof(switcher->generation)); offset += sizeof(switcher->generation);
memcpy(bytes + offset, &switcher->switch_count, sizeof(switcher->switch_count)); offset += sizeof(switcher->switch_count);
memcpy(bytes + offset, &switcher->abort_count, sizeof(switcher->abort_count)); offset += sizeof(switcher->abort_count);
memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
return switch_fnv1a64(bytes, offset);
}

98
wasm-core/src/sim_userkins.c Normal file
View File

@@ -0,0 +1,98 @@
#include "sim_kinematics.h"
#include <string.h>
static uint64_t user_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_static_name(const char *name) {
size_t index;
if (name == 0 || name[0] == '\0') return 0;
for (index = 0; index < SIM_KIN_USER_NAME_CAPACITY && name[index] != '\0'; ++index) {
unsigned char value = (unsigned char)name[index];
if (value < 0x21 || value > 0x7e || value == '/' || value == '\\') return 0;
}
return index < SIM_KIN_USER_NAME_CAPACITY;
}
int sim_kinematics_user_init(sim_kinematics_user *user,
const sim_kinematics_user_descriptor *descriptor) {
sim_kinematics_config config;
uint32_t required = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
SIM_KIN_USER_CAP_STATIC;
int result;
if (user == 0 || descriptor == 0) return SIM_KIN_ERR_ARGUMENT;
if (descriptor->abi_version != SIM_KIN_USER_ABI_VERSION) return SIM_KIN_ERR_ABI_VERSION;
if ((descriptor->capabilities & required) != required ||
(descriptor->capabilities & ~required) != 0) return SIM_KIN_ERR_CAPABILITY;
if (!valid_static_name(descriptor->name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
config = descriptor->config;
if (descriptor->implementation == SIM_KIN_USER_IDENTITY) config.type = SIM_KIN_TRIVKINS;
else if (descriptor->implementation == SIM_KIN_USER_GENSER) config.type = SIM_KIN_GENSER;
else return SIM_KIN_ERR_MISSING_MODULE;
result = sim_kinematics_init(&user->module, &config);
if (result != SIM_KIN_OK) return result;
user->descriptor = *descriptor;
user->descriptor.config = user->module.config;
user->forward_calls = 0;
user->inverse_calls = 0;
user->initialized = 1;
return SIM_KIN_OK;
}
int sim_kinematics_user_bind_switch(sim_kinematics_user *user,
sim_kinematics_switch *switcher, int mode,
uint8_t fwd_iterates) {
if (user == 0 || switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
return sim_kinematics_switch_register(switcher, mode, &user->module, fwd_iterates);
}
int sim_kinematics_user_forward(sim_kinematics_user *user, const double *joints,
uint32_t joint_count, sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
if (user == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
user->forward_calls += 1;
return sim_kinematics_forward_ex(&user->module, joints, joint_count, 0, pose, info);
}
int sim_kinematics_user_inverse(sim_kinematics_user *user,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info) {
if (user == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
if (!user->initialized) return SIM_KIN_ERR_MISSING_MODULE;
user->inverse_calls += 1;
return sim_kinematics_inverse_ex(&user->module, pose, joint_count, 0, joints, info);
}
int sim_kinematics_user_reject_dynamic(const char *module_path) {
return module_path == 0 ? SIM_KIN_ERR_ARGUMENT : SIM_KIN_ERR_DYNAMIC_MODULE;
}
uint64_t sim_kinematics_user_state_hash(const sim_kinematics_user *user,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose) {
unsigned char bytes[sizeof(uint32_t) * 5 + sizeof(int) + SIM_KIN_USER_NAME_CAPACITY + sizeof(uint64_t)];
uint64_t module_hash;
size_t offset = 0;
if (user == 0 || joints == 0 || pose == 0 || !user->initialized) return 0;
module_hash = sim_kinematics_state_hash(&user->module, joints, joint_count, pose);
memcpy(bytes + offset, &user->descriptor.abi_version, sizeof(user->descriptor.abi_version)); offset += sizeof(user->descriptor.abi_version);
memcpy(bytes + offset, &user->descriptor.capabilities, sizeof(user->descriptor.capabilities)); offset += sizeof(user->descriptor.capabilities);
memcpy(bytes + offset, &user->descriptor.implementation, sizeof(user->descriptor.implementation)); offset += sizeof(user->descriptor.implementation);
memcpy(bytes + offset, user->descriptor.name, sizeof(user->descriptor.name)); offset += sizeof(user->descriptor.name);
memcpy(bytes + offset, &user->initialized, sizeof(user->initialized)); offset += sizeof(user->initialized);
memcpy(bytes + offset, &user->forward_calls, sizeof(user->forward_calls)); offset += sizeof(user->forward_calls);
memcpy(bytes + offset, &user->inverse_calls, sizeof(user->inverse_calls)); offset += sizeof(user->inverse_calls);
memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
return user_fnv1a64(bytes, offset);
}