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

90
wasm-core/CMakeLists.txt
View File

@@ -6,7 +6,7 @@ set(CMAKE_C_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_hal.c src/sim_posemath.c src/sim_interpreter.c src/sim_motion_queue.c src/sim_kinematics.c)
add_library(sim_core STATIC src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_hal.c src/sim_posemath.c src/sim_interpreter.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
target_include_directories(sim_core PUBLIC include)
target_compile_definitions(sim_core PRIVATE SIM_API_BUILD=1)
@@ -70,6 +70,48 @@ if(NOT EMSCRIPTEN)
target_include_directories(sim_kinematics_test PRIVATE include)
target_compile_options(sim_kinematics_test PRIVATE -UNDEBUG)
add_test(NAME sim_kinematics_test COMMAND sim_kinematics_test)
add_executable(sim_switchkins_test tests/sim_switchkins_test.c)
target_link_libraries(sim_switchkins_test PRIVATE sim_core)
target_include_directories(sim_switchkins_test PRIVATE include)
target_compile_options(sim_switchkins_test PRIVATE -UNDEBUG)
add_test(NAME sim_switchkins_test COMMAND sim_switchkins_test)
add_executable(sim_userkins_test tests/sim_userkins_test.c)
target_link_libraries(sim_userkins_test PRIVATE sim_core)
target_include_directories(sim_userkins_test PRIVATE include)
target_compile_options(sim_userkins_test PRIVATE -UNDEBUG)
add_test(NAME sim_userkins_test COMMAND sim_userkins_test)
add_executable(sim_kinematics_registry_test tests/sim_kinematics_registry_test.c)
target_link_libraries(sim_kinematics_registry_test PRIVATE sim_core)
target_include_directories(sim_kinematics_registry_test PRIVATE include)
target_compile_options(sim_kinematics_registry_test PRIVATE -UNDEBUG)
add_test(NAME sim_kinematics_registry_test COMMAND sim_kinematics_registry_test)
add_executable(sim_kinematics_motion_test tests/sim_kinematics_motion_test.c)
target_link_libraries(sim_kinematics_motion_test PRIVATE sim_core)
target_include_directories(sim_kinematics_motion_test PRIVATE include)
target_compile_options(sim_kinematics_motion_test PRIVATE -UNDEBUG)
add_test(NAME sim_kinematics_motion_test COMMAND sim_kinematics_motion_test)
add_executable(sim_kinematics_feedback_test tests/sim_kinematics_feedback_test.c)
target_link_libraries(sim_kinematics_feedback_test PRIVATE sim_core)
target_include_directories(sim_kinematics_feedback_test PRIVATE include)
target_compile_options(sim_kinematics_feedback_test PRIVATE -UNDEBUG)
add_test(NAME sim_kinematics_feedback_test COMMAND sim_kinematics_feedback_test)
add_executable(sim_kinematics_home_limit_test tests/sim_kinematics_home_limit_test.c)
target_link_libraries(sim_kinematics_home_limit_test PRIVATE sim_core)
target_include_directories(sim_kinematics_home_limit_test PRIVATE include)
target_compile_options(sim_kinematics_home_limit_test PRIVATE -UNDEBUG)
add_test(NAME sim_kinematics_home_limit_test COMMAND sim_kinematics_home_limit_test)
add_executable(sim_io_test tests/sim_io_test.c)
target_link_libraries(sim_io_test PRIVATE sim_core)
target_include_directories(sim_io_test PRIVATE include)
target_compile_options(sim_io_test PRIVATE -UNDEBUG)
add_test(NAME sim_io_test COMMAND sim_io_test)
endif()
if(EMSCRIPTEN)
@@ -85,7 +127,49 @@ if(EMSCRIPTEN)
target_compile_options(sim_kinematics_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_kinematics_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_hal.c src/sim_posemath.c src/sim_interpreter.c src/sim_motion_queue.c src/sim_kinematics.c)
add_executable(sim_switchkins_wasm_test tests/sim_switchkins_test.c)
target_link_libraries(sim_switchkins_wasm_test PRIVATE sim_core)
target_include_directories(sim_switchkins_wasm_test PRIVATE include)
target_compile_options(sim_switchkins_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_switchkins_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_userkins_wasm_test tests/sim_userkins_test.c)
target_link_libraries(sim_userkins_wasm_test PRIVATE sim_core)
target_include_directories(sim_userkins_wasm_test PRIVATE include)
target_compile_options(sim_userkins_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_userkins_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_kinematics_registry_wasm_test tests/sim_kinematics_registry_test.c)
target_link_libraries(sim_kinematics_registry_wasm_test PRIVATE sim_core)
target_include_directories(sim_kinematics_registry_wasm_test PRIVATE include)
target_compile_options(sim_kinematics_registry_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_kinematics_registry_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_kinematics_motion_wasm_test tests/sim_kinematics_motion_test.c)
target_link_libraries(sim_kinematics_motion_wasm_test PRIVATE sim_core)
target_include_directories(sim_kinematics_motion_wasm_test PRIVATE include)
target_compile_options(sim_kinematics_motion_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_kinematics_motion_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_kinematics_feedback_wasm_test tests/sim_kinematics_feedback_test.c)
target_link_libraries(sim_kinematics_feedback_wasm_test PRIVATE sim_core)
target_include_directories(sim_kinematics_feedback_wasm_test PRIVATE include)
target_compile_options(sim_kinematics_feedback_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_kinematics_feedback_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_kinematics_home_limit_wasm_test tests/sim_kinematics_home_limit_test.c)
target_link_libraries(sim_kinematics_home_limit_wasm_test PRIVATE sim_core)
target_include_directories(sim_kinematics_home_limit_wasm_test PRIVATE include)
target_compile_options(sim_kinematics_home_limit_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_kinematics_home_limit_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_io_wasm_test tests/sim_io_test.c)
target_link_libraries(sim_io_wasm_test PRIVATE sim_core)
target_include_directories(sim_io_wasm_test PRIVATE include)
target_compile_options(sim_io_wasm_test PRIVATE -UNDEBUG)
target_link_options(sim_io_wasm_test PRIVATE -sSINGLE_FILE=1)
add_executable(sim_core_module src/sim_api.c src/sim_protocol.c src/sim_event.c src/sim_hal.c src/sim_posemath.c src/sim_interpreter.c src/sim_motion_queue.c src/sim_kinematics.c src/sim_switchkins.c src/sim_userkins.c src/sim_kinematics_registry.c src/sim_kinematics_motion.c src/sim_kinematics_feedback.c src/sim_kinematics_home_limit.c src/sim_io.c)
target_include_directories(sim_core_module PRIVATE
include
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src
@@ -97,6 +181,6 @@ if(EMSCRIPTEN)
set_target_properties(sim_core_module PROPERTIES
OUTPUT_NAME "linuxcnc-sim-core"
SUFFIX ".js"
LINK_FLAGS "-sMODULARIZE=1 -sEXPORT_ES6=1 -sENVIRONMENT=web,worker -sEXPORTED_FUNCTIONS=['_malloc','_free','_sim_create','_sim_load_workspace','_sim_command','_sim_step','_sim_read_snapshot','_sim_read_event','_sim_export_state','_sim_import_state','_sim_destroy','_sim_protocol_reset','_sim_protocol_accept','_sim_protocol_pause','_sim_protocol_resume','_sim_protocol_state_value','_sim_protocol_last_seq','_sim_protocol_tick','_sim_protocol_set_tick','_sim_event_reset','_sim_event_append','_sim_event_count','_sim_event_cursor','_sim_event_read','_sim_event_export','_sim_event_import','_sim_hal_reset','_sim_hal_register_pin','_sim_hal_set_pin','_sim_hal_get_pin','_sim_hal_connect','_sim_hal_register_thread','_sim_hal_register_function','_sim_hal_set_function_failure','_sim_hal_step','_sim_hal_tick','_sim_hal_function_calls','_sim_hal_pin_count','_sim_hal_thread_count','_sim_hal_function_count','_sim_posemath_normalize','_sim_posemath_dot','_sim_posemath_line_point','_sim_posemath_is_finite','_sim_posemath_pose_is_finite','_sim_posemath_pose_line_point','_sim_posemath_segment_validate','_sim_posemath_segment_point','_sim_posemath_hash_state','_sim_tp_init','_sim_tp_reset','_sim_tp_enqueue','_sim_tp_step','_sim_tp_read_state','_sim_tp_state_hash','_sim_interpreter_init','_sim_interpreter_reset','_sim_interpreter_execute','_sim_interpreter_read_state','_sim_interpreter_canonical','_sim_interpreter_state_hash','_sim_interpreter_block_hash','_sim_motion_init','_sim_motion_reset','_sim_motion_enqueue','_sim_motion_step','_sim_motion_read_state','_sim_motion_queue_depth','_sim_motion_state_hash','_sim_motion_canonical','_sim_kinematics_init','_sim_kinematics_forward','_sim_kinematics_forward_ex','_sim_kinematics_inverse','_sim_kinematics_inverse_ex','_sim_kinematics_home','_sim_kinematics_validate_pose','_sim_kinematics_state_hash'] -sEXPORTED_RUNTIME_METHODS=['ccall','cwrap']"
LINK_FLAGS "-sMODULARIZE=1 -sEXPORT_ES6=1 -sENVIRONMENT=web,worker -sEXPORTED_FUNCTIONS=['_malloc','_free','_sim_create','_sim_load_workspace','_sim_command','_sim_step','_sim_read_snapshot','_sim_read_event','_sim_export_state','_sim_import_state','_sim_destroy','_sim_protocol_reset','_sim_protocol_accept','_sim_protocol_pause','_sim_protocol_resume','_sim_protocol_state_value','_sim_protocol_last_seq','_sim_protocol_tick','_sim_protocol_set_tick','_sim_event_reset','_sim_event_append','_sim_event_count','_sim_event_cursor','_sim_event_read','_sim_event_export','_sim_event_import','_sim_hal_reset','_sim_hal_register_pin','_sim_hal_set_pin','_sim_hal_get_pin','_sim_hal_connect','_sim_hal_register_thread','_sim_hal_register_function','_sim_hal_set_function_failure','_sim_hal_step','_sim_hal_tick','_sim_hal_function_calls','_sim_hal_pin_count','_sim_hal_thread_count','_sim_hal_function_count','_sim_posemath_normalize','_sim_posemath_dot','_sim_posemath_line_point','_sim_posemath_is_finite','_sim_posemath_pose_is_finite','_sim_posemath_pose_line_point','_sim_posemath_segment_validate','_sim_posemath_segment_point','_sim_posemath_hash_state','_sim_tp_init','_sim_tp_reset','_sim_tp_enqueue','_sim_tp_step','_sim_tp_read_state','_sim_tp_state_hash','_sim_interpreter_init','_sim_interpreter_reset','_sim_interpreter_execute','_sim_interpreter_read_state','_sim_interpreter_canonical','_sim_interpreter_state_hash','_sim_interpreter_block_hash','_sim_motion_init','_sim_motion_reset','_sim_motion_enqueue','_sim_motion_step','_sim_motion_read_state','_sim_motion_queue_depth','_sim_motion_state_hash','_sim_motion_canonical','_sim_kinematics_init','_sim_kinematics_forward','_sim_kinematics_forward_ex','_sim_kinematics_inverse','_sim_kinematics_inverse_ex','_sim_kinematics_home','_sim_kinematics_validate_pose','_sim_kinematics_state_hash','_sim_kinematics_switch_init','_sim_kinematics_switch_register','_sim_kinematics_switch_begin','_sim_kinematics_switch_abort','_sim_kinematics_switch_commit','_sim_kinematics_switch_select','_sim_kinematics_switch_forward','_sim_kinematics_switch_inverse','_sim_kinematics_switch_active','_sim_kinematics_switch_state_hash','_sim_kinematics_user_init','_sim_kinematics_user_bind_switch','_sim_kinematics_user_forward','_sim_kinematics_user_inverse','_sim_kinematics_user_reject_dynamic','_sim_kinematics_user_state_hash','_sim_kinematics_registry_count','_sim_kinematics_registry_get','_sim_kinematics_registry_find','_sim_kinematics_registry_require','_sim_kinematics_registry_validate_config','_sim_kinematics_registry_state_hash','_sim_kinematics_motion_init','_sim_kinematics_motion_reset','_sim_kinematics_motion_enqueue','_sim_kinematics_motion_pause','_sim_kinematics_motion_step','_sim_kinematics_motion_peek','_sim_kinematics_motion_pop','_sim_kinematics_motion_depth','_sim_kinematics_motion_state_hash','_sim_kinematics_feedback_init','_sim_kinematics_feedback_reset','_sim_kinematics_feedback_update','_sim_kinematics_feedback_read','_sim_kinematics_feedback_state_hash','_sim_kinematics_home_limit_init','_sim_kinematics_home_limit_reset','_sim_kinematics_home_limit_home','_sim_kinematics_home_limit_check','_sim_kinematics_home_limit_read','_sim_kinematics_home_limit_state_hash','_sim_io_init','_sim_io_reset','_sim_io_register_pins','_sim_io_spindle_command','_sim_io_spindle_set_inhibit','_sim_io_spindle_set_fault','_sim_io_spindle_set_index_enable','_sim_io_spindle_set_feedback','_sim_io_spindle_orient','_sim_io_spindle_set_oriented','_sim_io_tool_prepare','_sim_io_tool_set_prepared','_sim_io_tool_change','_sim_io_tool_set_changed','_sim_io_tool_abort','_sim_io_tool_set_fault','_sim_io_tool_clear_fault','_sim_io_probe_arm','_sim_io_probe_update','_sim_io_probe_disarm','_sim_io_set_estop','_sim_io_recover','_sim_io_step','_sim_io_read_state','_sim_io_state_hash','_sim_io_canonical'] -sEXPORTED_RUNTIME_METHODS=['ccall','cwrap']"
)
endif()

2
wasm-core/include/sim_hal.h
View File

@@ -8,7 +8,7 @@ extern "C" {
#endif
#define SIM_HAL_NAME_CAPACITY 48U
#define SIM_HAL_PIN_CAPACITY 64U
#define SIM_HAL_PIN_CAPACITY 256U
#define SIM_HAL_THREAD_CAPACITY 16U
#define SIM_HAL_FUNCTION_CAPACITY 64U

151
wasm-core/include/sim_io.h Normal file
View File

@@ -0,0 +1,151 @@
#ifndef LINUXCNC_WEB_WASM_SIM_IO_H
#define LINUXCNC_WEB_WASM_SIM_IO_H
#include "sim_posemath.h"
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
#define SIM_IO_MAX_SPINDLES 8u
#define SIM_IO_MAX_TOOLS 1001u
enum sim_io_result {
SIM_IO_OK = 0,
SIM_IO_ERR_ARGUMENT = -900,
SIM_IO_ERR_RANGE = -901,
SIM_IO_ERR_STATE = -902,
SIM_IO_ERR_NOT_READY = -903,
SIM_IO_ERR_ESTOP = -904,
SIM_IO_ERR_FAULT = -905,
SIM_IO_ERR_LIMIT = -906,
SIM_IO_ERR_NONFINITE = -907,
SIM_IO_ERR_PIN = -908,
SIM_IO_ERR_CAPACITY = -909,
SIM_IO_ERR_BUSY = -910,
SIM_IO_ERR_PROBE = -911,
SIM_IO_ERR_TIMEOUT = -912,
SIM_IO_ERR_RECOVERY = -913,
};
enum sim_io_tool_state {
SIM_IO_TOOL_IDLE = 0,
SIM_IO_TOOL_PREPARING = 1,
SIM_IO_TOOL_PREPARED = 2,
SIM_IO_TOOL_CHANGING = 3,
SIM_IO_TOOL_READY = 4,
SIM_IO_TOOL_ABORTED = 5,
SIM_IO_TOOL_FAULT = 6,
};
enum sim_io_probe_status {
SIM_IO_PROBE_IDLE = 0,
SIM_IO_PROBE_ARMED = 1,
SIM_IO_PROBE_TRIPPED = 2,
SIM_IO_PROBE_FAILED = 3,
};
typedef struct sim_io_spindle_state {
uint32_t registered;
uint32_t enabled;
uint32_t inhibit;
uint32_t amp_fault;
uint32_t at_speed;
uint32_t brake;
uint32_t forward;
uint32_t reverse;
uint32_t index_enable;
uint32_t orient;
uint32_t locked;
uint32_t is_oriented;
int32_t orient_mode;
int32_t orient_fault;
double command_rpm;
double speed_rpm;
double max_rpm;
double accel_rpm_s;
double tolerance_rpm;
double orient_angle;
double revs;
uint32_t tick;
} sim_io_spindle_state;
typedef struct sim_io_probe_state {
uint32_t input;
uint32_t probe_type;
uint32_t state;
uint32_t tripped;
uint32_t tick;
sim_posemath_pose position;
} sim_io_probe_state;
typedef struct sim_io_state {
uint32_t ready;
uint32_t estop;
uint32_t fault;
int32_t fault_reason;
uint32_t tick;
uint32_t recovery_epoch;
uint32_t spindle_count;
uint32_t tool_capacity;
sim_io_spindle_state spindles[SIM_IO_MAX_SPINDLES];
uint32_t tool_state;
int32_t current_tool;
int32_t prepared_tool;
int32_t prepared_pocket;
uint32_t tool_prepare;
uint32_t tool_prepared;
uint32_t tool_change;
uint32_t tool_changed;
uint32_t toolchanger_fault;
int32_t toolchanger_reason;
sim_io_probe_state probe;
int32_t last_error;
} sim_io_state;
typedef struct sim_io {
sim_io_state state;
double cycle_time;
uint32_t pins_registered;
} sim_io;
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);
int sim_io_reset(sim_io *io);
int sim_io_register_pins(sim_io *io);
int sim_io_spindle_command(sim_io *io, uint32_t spindle, double rpm,
uint32_t enabled);
int sim_io_spindle_set_inhibit(sim_io *io, uint32_t spindle, uint32_t inhibit);
int sim_io_spindle_set_fault(sim_io *io, uint32_t spindle, uint32_t fault);
int sim_io_spindle_set_index_enable(sim_io *io, uint32_t spindle, uint32_t enable);
int sim_io_spindle_set_feedback(sim_io *io, uint32_t spindle, double rpm);
int sim_io_spindle_orient(sim_io *io, uint32_t spindle, double angle,
int32_t mode);
int sim_io_spindle_set_oriented(sim_io *io, uint32_t spindle,
uint32_t oriented, int32_t fault);
int sim_io_tool_prepare(sim_io *io, int32_t tool, int32_t pocket);
int sim_io_tool_set_prepared(sim_io *io, uint32_t prepared);
int sim_io_tool_change(sim_io *io);
int sim_io_tool_set_changed(sim_io *io, uint32_t changed);
int sim_io_tool_abort(sim_io *io, int32_t reason);
int sim_io_tool_set_fault(sim_io *io, uint32_t fault, int32_t reason);
int sim_io_tool_clear_fault(sim_io *io);
int sim_io_probe_arm(sim_io *io, uint32_t probe_type);
int sim_io_probe_update(sim_io *io, const sim_posemath_pose *pose,
uint32_t input);
int sim_io_probe_disarm(sim_io *io);
int sim_io_set_estop(sim_io *io, uint32_t estop);
int sim_io_recover(sim_io *io);
int sim_io_step(sim_io *io, uint32_t ticks);
int sim_io_read_state(const sim_io *io, sim_io_state *state);
uint64_t sim_io_state_hash(const sim_io *io);
uint32_t sim_io_canonical(const sim_io *io, char *output, uint32_t capacity);
#ifdef __cplusplus
}
#endif
#endif

134
wasm-core/include/sim_kinematics.h
View File

@@ -16,6 +16,12 @@ enum sim_kinematics_result {
SIM_KIN_ERR_SINGULAR = -704,
SIM_KIN_ERR_UNKNOWN_TYPE = -705,
SIM_KIN_ERR_UNSUPPORTED = -706,
SIM_KIN_ERR_INVALID_MODE = -707,
SIM_KIN_ERR_MISSING_MODULE = -708,
SIM_KIN_ERR_SWITCH_INTERRUPTED = -709,
SIM_KIN_ERR_ABI_VERSION = -710,
SIM_KIN_ERR_CAPABILITY = -711,
SIM_KIN_ERR_DYNAMIC_MODULE = -712,
};
enum sim_kinematics_type {
@@ -60,6 +66,34 @@ enum sim_kinematics_mode {
#define SIM_KIN_MAX_JOINTS 9u
#define SIM_KIN_GENHEX_STRUTS 6u
#define SIM_KIN_PENTA_STRUTS 5u
#define SIM_KIN_SWITCH_MAX_TYPES 3u
#define SIM_KIN_USER_ABI_VERSION 1u
#define SIM_KIN_USER_CAP_FORWARD UINT32_C(1)
#define SIM_KIN_USER_CAP_INVERSE UINT32_C(2)
#define SIM_KIN_USER_CAP_STATIC UINT32_C(4)
#define SIM_KIN_USER_NAME_CAPACITY 32u
#define SIM_KIN_REGISTRY_ABI_VERSION 1u
#define SIM_KIN_REGISTRY_NAME_CAPACITY 32u
#define SIM_KIN_CAP_FORWARD UINT32_C(1)
#define SIM_KIN_CAP_INVERSE UINT32_C(2)
#define SIM_KIN_CAP_IDENTITY UINT32_C(4)
#define SIM_KIN_CAP_ITERATIVE UINT32_C(8)
#define SIM_KIN_CAP_SWITCHABLE UINT32_C(16)
#define SIM_KIN_CAP_EXTRA_JOINTS UINT32_C(32)
#define SIM_KIN_CAP_DUPLICATE_COORDINATES UINT32_C(64)
#define SIM_KIN_PARAM_MAPPING UINT64_C(1)
#define SIM_KIN_PARAM_LIMITS UINT64_C(2)
#define SIM_KIN_PARAM_OFFSETS UINT64_C(4)
#define SIM_KIN_PARAM_TOOL UINT64_C(8)
#define SIM_KIN_PARAM_PIVOT UINT64_C(16)
#define SIM_KIN_PARAM_GEOMETRY UINT64_C(32)
#define SIM_KIN_PARAM_DH UINT64_C(64)
#define SIM_KIN_PARAM_SOLVER UINT64_C(128)
enum sim_kinematics_user_implementation {
SIM_KIN_USER_IDENTITY = 1,
SIM_KIN_USER_GENSER = 2,
};
typedef struct sim_kinematics_config {
int type;
@@ -149,6 +183,52 @@ typedef struct sim_kinematics_result_info {
uint32_t branch_id;
} sim_kinematics_result_info;
/* A bounded switchkins container matching LinuxCNC's three registered
* kinematics slots. The container owns switching state, not the modules. */
typedef struct sim_kinematics_switch {
sim_kinematics *modules[SIM_KIN_SWITCH_MAX_TYPES];
uint8_t registered[SIM_KIN_SWITCH_MAX_TYPES];
uint8_t fwd_iterates_mask;
uint8_t switching;
int active_mode;
int pending_mode;
uint32_t generation;
uint32_t switch_count;
uint32_t abort_count;
sim_posemath_pose last_pose[SIM_KIN_SWITCH_MAX_TYPES];
uint8_t have_last_pose[SIM_KIN_SWITCH_MAX_TYPES];
} sim_kinematics_switch;
/* User kinematics are selected from implementations already linked into the
* WASM module. No function pointers, host paths, or dynamic libraries enter
* this descriptor. */
typedef struct sim_kinematics_user_descriptor {
uint32_t abi_version;
uint32_t capabilities;
int implementation;
char name[SIM_KIN_USER_NAME_CAPACITY];
sim_kinematics_config config;
} sim_kinematics_user_descriptor;
typedef struct sim_kinematics_user {
sim_kinematics_user_descriptor descriptor;
sim_kinematics module;
uint32_t initialized;
uint32_t forward_calls;
uint32_t inverse_calls;
} sim_kinematics_user;
typedef struct sim_kinematics_capability {
uint32_t abi_version;
int type;
char name[SIM_KIN_REGISTRY_NAME_CAPACITY];
char required_coordinates[16];
uint32_t min_joints;
uint32_t max_joints;
uint32_t capabilities;
uint64_t parameter_mask;
} sim_kinematics_capability;
int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config *config);
int sim_kinematics_forward(const sim_kinematics *kinematics, const double *joints,
uint32_t joint_count, sim_posemath_pose *pose);
@@ -167,6 +247,60 @@ uint64_t sim_kinematics_state_hash(const sim_kinematics *kinematics,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose);
int sim_kinematics_switch_init(sim_kinematics_switch *switcher, int default_mode,
uint8_t fwd_iterates_mask);
int sim_kinematics_switch_register(sim_kinematics_switch *switcher, int mode,
sim_kinematics *module, uint8_t fwd_iterates);
int sim_kinematics_switch_begin(sim_kinematics_switch *switcher, int mode);
int sim_kinematics_switch_abort(sim_kinematics_switch *switcher);
int sim_kinematics_switch_commit(sim_kinematics_switch *switcher, double *joints,
uint32_t joint_count, sim_posemath_pose *pose,
sim_kinematics_result_info *info);
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);
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);
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);
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher);
uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose);
int sim_kinematics_user_init(sim_kinematics_user *user,
const sim_kinematics_user_descriptor *descriptor);
int sim_kinematics_user_bind_switch(sim_kinematics_user *user,
sim_kinematics_switch *switcher, int mode,
uint8_t 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);
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);
int sim_kinematics_user_reject_dynamic(const char *module_path);
uint64_t sim_kinematics_user_state_hash(const sim_kinematics_user *user,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose);
uint32_t sim_kinematics_registry_count(void);
int sim_kinematics_registry_get(uint32_t index,
sim_kinematics_capability *capability);
int sim_kinematics_registry_find(const char *name,
sim_kinematics_capability *capability);
int sim_kinematics_registry_require(const char *name, uint32_t capabilities,
uint64_t parameter_mask,
sim_kinematics_capability *capability);
int sim_kinematics_registry_validate_config(const char *name,
const sim_kinematics_config *config,
sim_kinematics_capability *capability);
uint64_t sim_kinematics_registry_state_hash(void);
#ifdef __cplusplus
}
#endif

52
wasm-core/include/sim_kinematics_feedback.h Normal file
View File

@@ -0,0 +1,52 @@
#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_FEEDBACK_H
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_FEEDBACK_H
#include "sim_kinematics.h"
#ifdef __cplusplus
extern "C" {
#endif
enum sim_kinematics_feedback_result {
SIM_KIN_FEEDBACK_OK = 0,
SIM_KIN_FEEDBACK_ERR_ARGUMENT = -820,
SIM_KIN_FEEDBACK_ERR_CONFIGURATION = -821,
SIM_KIN_FEEDBACK_ERR_NONFINITE = -822,
SIM_KIN_FEEDBACK_ERR_FORWARD = -823,
SIM_KIN_FEEDBACK_ERR_STATE = -824,
};
typedef struct sim_kinematics_feedback_state {
sim_posemath_pose pose;
uint64_t forward_flags;
uint64_t inverse_flags;
double joints[SIM_KIN_MAX_JOINTS];
uint32_t joint_count;
uint32_t tick;
uint32_t valid;
int last_error;
int kinematics_error;
} sim_kinematics_feedback_state;
typedef struct sim_kinematics_feedback {
sim_kinematics *kinematics;
sim_kinematics_feedback_state state;
} sim_kinematics_feedback;
int sim_kinematics_feedback_init(sim_kinematics_feedback *feedback,
sim_kinematics *kinematics,
uint32_t joint_count);
int sim_kinematics_feedback_reset(sim_kinematics_feedback *feedback);
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);
int sim_kinematics_feedback_read(const sim_kinematics_feedback *feedback,
sim_kinematics_feedback_state *state);
uint64_t sim_kinematics_feedback_state_hash(const sim_kinematics_feedback *feedback);
#ifdef __cplusplus
}
#endif
#endif

55
wasm-core/include/sim_kinematics_home_limit.h Normal file
View File

@@ -0,0 +1,55 @@
#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_HOME_LIMIT_H
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_HOME_LIMIT_H
#include "sim_kinematics.h"
#ifdef __cplusplus
extern "C" {
#endif
enum sim_kinematics_home_limit_result {
SIM_KIN_HOME_OK = 0,
SIM_KIN_HOME_ERR_ARGUMENT = -840,
SIM_KIN_HOME_ERR_CONFIGURATION = -841,
SIM_KIN_HOME_ERR_UNHOMED = -842,
SIM_KIN_HOME_ERR_LIMIT = -843,
SIM_KIN_HOME_ERR_FORWARD = -844,
SIM_KIN_HOME_ERR_NONFINITE = -845,
SIM_KIN_HOME_ERR_SEQUENCE = -846,
};
typedef struct sim_kinematics_home_limit_state {
sim_posemath_pose pose;
double joints[SIM_KIN_MAX_JOINTS];
uint32_t joint_count;
uint32_t homed;
uint32_t home_sequence;
uint32_t tick;
int last_error;
int limit_error;
int kinematics_error;
} sim_kinematics_home_limit_state;
typedef struct sim_kinematics_home_limit {
sim_kinematics *kinematics;
sim_kinematics_home_limit_state state;
} sim_kinematics_home_limit;
int sim_kinematics_home_limit_init(sim_kinematics_home_limit *home_limit,
sim_kinematics *kinematics,
uint32_t joint_count);
int sim_kinematics_home_limit_reset(sim_kinematics_home_limit *home_limit);
int sim_kinematics_home_limit_home(sim_kinematics_home_limit *home_limit,
uint32_t home_sequence);
int sim_kinematics_home_limit_check(sim_kinematics_home_limit *home_limit,
const double *joints,
uint32_t joint_count);
int sim_kinematics_home_limit_read(const sim_kinematics_home_limit *home_limit,
sim_kinematics_home_limit_state *state);
uint64_t sim_kinematics_home_limit_state_hash(const sim_kinematics_home_limit *home_limit);
#ifdef __cplusplus
}
#endif
#endif

68
wasm-core/include/sim_kinematics_motion.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_MOTION_H
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_MOTION_H
#include "sim_kinematics.h"
#include "sim_posemath.h"
#ifdef __cplusplus
extern "C" {
#endif
enum sim_kinematics_motion_result {
SIM_KIN_MOTION_OK = 0,
SIM_KIN_MOTION_ERR_ARGUMENT = -800,
SIM_KIN_MOTION_ERR_STATE = -801,
SIM_KIN_MOTION_ERR_CAPACITY = -802,
SIM_KIN_MOTION_ERR_PLANNER = -803,
SIM_KIN_MOTION_ERR_INVERSE = -804,
SIM_KIN_MOTION_ERR_NONFINITE = -805,
SIM_KIN_MOTION_ERR_CONFIGURATION = -806,
};
#define SIM_KIN_MOTION_QUEUE_CAPACITY 64u
typedef struct sim_kinematics_motion_sample {
sim_posemath_pose pose;
double joints[SIM_KIN_MAX_JOINTS];
uint32_t joint_count;
uint32_t tick;
uint32_t planner_id;
} sim_kinematics_motion_sample;
typedef struct sim_kinematics_motion {
sim_tp planner;
sim_kinematics *kinematics;
sim_kinematics_motion_sample samples[SIM_KIN_MOTION_QUEUE_CAPACITY];
uint32_t head;
uint32_t count;
uint32_t tick;
uint32_t joint_count;
uint32_t paused;
int last_error;
int planner_error;
int kinematics_error;
sim_posemath_pose last_pose;
double last_joints[SIM_KIN_MAX_JOINTS];
} sim_kinematics_motion;
int sim_kinematics_motion_init(sim_kinematics_motion *adapter,
sim_kinematics *kinematics, double cycle_time,
double velocity_limit, double acceleration_limit);
int sim_kinematics_motion_reset(sim_kinematics_motion *adapter);
int sim_kinematics_motion_enqueue(sim_kinematics_motion *adapter,
const sim_posemath_segment *segment,
uint32_t *id_out);
int sim_kinematics_motion_pause(sim_kinematics_motion *adapter, uint32_t paused);
int sim_kinematics_motion_step(sim_kinematics_motion *adapter);
int sim_kinematics_motion_peek(const sim_kinematics_motion *adapter,
sim_kinematics_motion_sample *sample);
int sim_kinematics_motion_pop(sim_kinematics_motion *adapter,
sim_kinematics_motion_sample *sample);
uint32_t sim_kinematics_motion_depth(const sim_kinematics_motion *adapter);
uint64_t sim_kinematics_motion_state_hash(const sim_kinematics_motion *adapter);
#ifdef __cplusplus
}
#endif
#endif

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

7
wasm-core/tests/sim_event_test.c
View File

@@ -24,5 +24,12 @@ int main(void) {
assert(sim_event_import(exported, export_size) == SIM_EVENT_OK);
assert(sim_event_count() == 1);
assert(sim_event_import(exported, export_size - 1) == SIM_EVENT_ERR_ARGUMENT);
((sim_event_record *)exported)->seq = 2;
assert(sim_event_import(exported, export_size) == SIM_EVENT_ERR_SEQUENCE);
assert(sim_event_cursor() == 1);
((sim_event_record *)exported)->seq = 1;
((sim_event_record *)exported)->kind[SIM_EVENT_KIND_CAPACITY - 1U] = 'x';
assert(sim_event_import(exported, export_size) == SIM_EVENT_ERR_ARGUMENT);
assert(sim_event_cursor() == 1);
return 0;
}

89
wasm-core/tests/sim_io_test.c Normal file
View File

@@ -0,0 +1,89 @@
#include "sim_io.h"
#include "sim_hal.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_posemath_pose pose(double x, double y, double z) {
sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
return value;
}
int main(void) {
sim_io io = {0};
sim_io_state state = {0};
sim_hal_value pin = {0};
char canonical[2048];
uint64_t hash;
uint64_t repeat_hash;
int invalid_pin_status;
int limit_status;
int spindle_step_status;
int tool_busy_status;
int probe_status;
int estop_command_status;
int estop_recover_status;
int recover_status;
int fault_clear_status;
sim_hal_reset();
assert(sim_io_init(&io, 2, 32, 0.1, 600.0, 5.0) == SIM_IO_OK);
assert(sim_hal_pin_count() >= 58 && sim_hal_function_count() == 1);
assert(sim_hal_get_pin("motion.probe-input", &pin) == SIM_HAL_OK && pin.type == SIM_HAL_BIT);
invalid_pin_status = sim_hal_set_pin("spindle.0.speed-out", (sim_hal_value){SIM_HAL_BIT, {.bit = 1}});
assert(invalid_pin_status == SIM_HAL_ERR_TYPE);
limit_status = sim_io_spindle_command(&io, 0, 100001.0, 1);
assert(limit_status == SIM_IO_ERR_LIMIT);
assert(sim_io_spindle_command(&io, 0, 1200.0, 1) == SIM_IO_OK);
spindle_step_status = sim_io_step(&io, 10);
assert(spindle_step_status == SIM_IO_OK && io.state.spindles[0].speed_rpm == 600.0);
assert(sim_io_step(&io, 10) == SIM_IO_OK && io.state.spindles[0].at_speed == 1);
assert(sim_io_tool_prepare(&io, 7, 3) == SIM_IO_OK);
assert(sim_io_tool_set_prepared(&io, 1) == SIM_IO_OK);
tool_busy_status = sim_io_tool_change(&io);
assert(tool_busy_status == SIM_IO_ERR_BUSY);
assert(sim_io_spindle_command(&io, 0, 0.0, 0) == SIM_IO_OK);
assert(sim_io_step(&io, 20) == SIM_IO_OK);
assert(sim_io_tool_change(&io) == SIM_IO_OK);
assert(sim_io_tool_set_changed(&io, 1) == SIM_IO_OK && io.state.current_tool == 7);
assert(sim_io_probe_arm(&io, 0) == SIM_IO_OK);
probe_status = sim_io_probe_update(&io, &(sim_posemath_pose){{1.0, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 1);
assert(probe_status == SIM_IO_OK && io.state.probe.state == SIM_IO_PROBE_TRIPPED);
assert(sim_io_probe_update(&io, &(sim_posemath_pose){{NAN, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 0) == SIM_IO_ERR_NONFINITE);
assert(sim_io_set_estop(&io, 1) == SIM_IO_OK);
estop_command_status = sim_io_spindle_command(&io, 0, 100.0, 1);
estop_recover_status = sim_io_recover(&io);
assert(estop_command_status == SIM_IO_ERR_ESTOP && estop_recover_status == SIM_IO_ERR_ESTOP);
assert(sim_io_set_estop(&io, 0) == SIM_IO_OK);
recover_status = sim_io_recover(&io);
assert(recover_status == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 1);
assert(sim_io_tool_set_fault(&io, 1, 42) == SIM_IO_OK);
fault_clear_status = sim_io_tool_clear_fault(&io);
assert(fault_clear_status == SIM_IO_ERR_FAULT);
assert(sim_io_tool_set_fault(&io, 0, 0) == SIM_IO_OK);
assert(sim_io_tool_clear_fault(&io) == SIM_IO_OK);
hash = sim_io_state_hash(&io);
repeat_hash = sim_io_state_hash(&io);
assert(hash == repeat_hash);
assert(sim_io_canonical(&io, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
assert(sim_io_canonical(&io, 0, 0) == strlen(canonical) + 1);
assert(sim_hal_get_pin("iocontrol.0.tool-number", &pin) == SIM_HAL_OK && pin.value.s32 == 7);
assert(sim_hal_get_pin("spindle.0.brake", &pin) == SIM_HAL_OK && pin.value.bit == 1);
printf("{\"schema\":\"io-samples.v1\",\"pin\":{\"invalid_type\":%d,\"count\":%u,\"functions\":%u},\"spindle\":{\"limit\":%d,\"step\":%d,\"speed\":%.3f,\"at_speed\":%u,\"tool_busy\":%d},\"tool\":{\"state\":%u,\"current\":%d,\"fault_clear\":%d},\"probe\":{\"status\":%d,\"state\":%u,\"position\":[%.3f,%.3f,%.3f]},\"estop\":{\"command\":%d,\"recover_before_clear\":%d,\"recover\":%d},\"state_hash\":%llu,\"repeat_hash\":%llu,\"canonical\":%s}\n",
invalid_pin_status, sim_hal_pin_count(), sim_hal_function_count(), limit_status,
spindle_step_status, io.state.spindles[0].speed_rpm, io.state.spindles[0].at_speed,
tool_busy_status, io.state.tool_state, io.state.current_tool, fault_clear_status,
probe_status, io.state.probe.state, io.state.probe.position.tran.x,
io.state.probe.position.tran.y, io.state.probe.position.tran.z,
estop_command_status, estop_recover_status, recover_status,
(unsigned long long)hash, (unsigned long long)repeat_hash, canonical);
(void)pose;
return 0;
}

65
wasm-core/tests/sim_kinematics_feedback_test.c Normal file
View File

@@ -0,0 +1,65 @@
#include "sim_kinematics_feedback.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_kinematics_config config_for(int type, const char *coordinates) {
sim_kinematics_config config = {0};
config.type = type;
config.joint_count = (uint32_t)strlen(coordinates);
strcpy(config.coordinates, coordinates);
return config;
}
int main(void) {
sim_kinematics kinematics = {0};
sim_kinematics singular = {0};
sim_kinematics_feedback feedback = {0};
sim_kinematics_feedback singular_feedback = {0};
sim_kinematics_feedback_state state = {0};
sim_kinematics_result_info info = {0};
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
double joints[3] = {1.0, 2.0, 3.0};
double bad_joints[3] = {1.0, NAN, 3.0};
double singular_joints[5] = {0, 0, 0, 90, 0};
uint64_t hash;
uint64_t repeat_hash;
uint32_t valid_tick;
int update_status;
int bad_status;
int singular_status;
int stale_read_status;
int count_status;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
assert(sim_kinematics_feedback_init(&feedback, &kinematics, 3) == SIM_KIN_FEEDBACK_OK);
stale_read_status = sim_kinematics_feedback_read(&feedback, &state);
update_status = sim_kinematics_feedback_update(&feedback, joints, 3, 0, &info);
assert(update_status == SIM_KIN_FEEDBACK_OK);
assert(sim_kinematics_feedback_read(&feedback, &state) == SIM_KIN_FEEDBACK_OK);
valid_tick = state.tick;
hash = sim_kinematics_feedback_state_hash(&feedback);
repeat_hash = sim_kinematics_feedback_state_hash(&feedback);
assert(state.valid && state.tick == 1 && fabs(state.pose.tran.x - 1.0) < 1e-12);
bad_status = sim_kinematics_feedback_update(&feedback, bad_joints, 3, 0, &info);
assert(bad_status == SIM_KIN_FEEDBACK_ERR_NONFINITE && feedback.state.tick == valid_tick);
count_status = sim_kinematics_feedback_update(&feedback, joints, 2, 0, &info);
assert(count_status == SIM_KIN_FEEDBACK_ERR_CONFIGURATION);
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
config.mode = SIM_KIN_MODE_INVERSE_ONLY;
assert(sim_kinematics_init(&singular, &config) == SIM_KIN_OK);
assert(sim_kinematics_feedback_init(&singular_feedback, &singular, 5) == SIM_KIN_FEEDBACK_OK);
singular_status = sim_kinematics_feedback_update(&singular_feedback, singular_joints, 5, 0, &info);
assert(singular_status == SIM_KIN_FEEDBACK_ERR_FORWARD && singular_feedback.state.valid == 0 && singular_feedback.state.kinematics_error == SIM_KIN_ERR_UNSUPPORTED);
printf("{\"schema\":\"kinematics-feedback-samples.v1\",\"nominal\":{\"status\":%d,\"tick\":%u,\"valid\":%u,\"pose\":[%.9f,%.9f,%.9f],\"flags\":[%llu,%llu],\"state_hash\":%llu,\"repeat_hash\":%llu},\"stale_read\":%d,\"nonfinite\":%d,\"count_error\":%d,\"singular\":{\"status\":%d,\"kinematics_error\":%d,\"valid\":%u}}\n",
update_status, state.tick, state.valid, state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
(unsigned long long)state.forward_flags, (unsigned long long)state.inverse_flags,
(unsigned long long)hash, (unsigned long long)repeat_hash, stale_read_status,
bad_status, count_status, singular_status, singular_feedback.state.kinematics_error,
singular_feedback.state.valid);
return 0;
}

57
wasm-core/tests/sim_kinematics_home_limit_test.c Normal file
View File

@@ -0,0 +1,57 @@
#include "sim_kinematics_home_limit.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_kinematics_config config_for(void) {
sim_kinematics_config config = {0};
config.type = SIM_KIN_TRIVKINS;
config.joint_count = 3;
strcpy(config.coordinates, "XYZ");
config.limit_enabled[0] = 1; config.joint_min[0] = -5.0; config.joint_max[0] = 5.0;
config.limit_enabled[1] = 1; config.joint_min[1] = -5.0; config.joint_max[1] = 5.0;
config.limit_enabled[2] = 1; config.joint_min[2] = -5.0; config.joint_max[2] = 5.0;
return config;
}
int main(void) {
sim_kinematics kinematics = {0};
sim_kinematics_home_limit home_limit = {0};
sim_kinematics_home_limit_state state = {0};
sim_kinematics_config config = config_for();
double nominal[3] = {1.0, 2.0, 3.0};
double outside[3] = {6.0, 2.0, 3.0};
double nonfinite[3] = {NAN, 2.0, 3.0};
uint64_t hash;
uint64_t repeat_hash;
int before_home;
int home_status;
int limit_status;
int nonfinite_status;
int sequence_status;
int count_status;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
assert(sim_kinematics_home_limit_init(&home_limit, &kinematics, 3) == SIM_KIN_HOME_OK);
before_home = sim_kinematics_home_limit_check(&home_limit, nominal, 3);
sequence_status = sim_kinematics_home_limit_home(&home_limit, 0x10000u);
home_status = sim_kinematics_home_limit_home(&home_limit, 7);
assert(home_status == SIM_KIN_HOME_OK && home_limit.state.homed == 1 && home_limit.state.home_sequence == 7);
limit_status = sim_kinematics_home_limit_check(&home_limit, outside, 3);
assert(limit_status == SIM_KIN_HOME_ERR_LIMIT && home_limit.state.joints[0] == 0.0);
nonfinite_status = sim_kinematics_home_limit_check(&home_limit, nonfinite, 3);
count_status = sim_kinematics_home_limit_check(&home_limit, nominal, 2);
assert(sim_kinematics_home_limit_check(&home_limit, nominal, 3) == SIM_KIN_HOME_OK);
assert(sim_kinematics_home_limit_read(&home_limit, &state) == SIM_KIN_HOME_OK);
hash = sim_kinematics_home_limit_state_hash(&home_limit);
repeat_hash = sim_kinematics_home_limit_state_hash(&home_limit);
assert(hash == repeat_hash && state.tick == 2);
printf("{\"schema\":\"kinematics-home-limit-samples.v1\",\"before_home\":%d,\"sequence_error\":%d,\"home_status\":%d,\"limit_status\":%d,\"nonfinite_status\":%d,\"count_status\":%d,\"state\":{\"homed\":%u,\"sequence\":%u,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"hash\":%llu,\"repeat_hash\":%llu}}\n",
before_home, sequence_status, home_status, limit_status, nonfinite_status,
count_status, state.homed, state.home_sequence, state.tick,
state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
state.joints[0], state.joints[1], state.joints[2],
(unsigned long long)hash, (unsigned long long)repeat_hash);
return 0;
}

107
wasm-core/tests/sim_kinematics_motion_test.c Normal file
View File

@@ -0,0 +1,107 @@
#include "sim_kinematics_motion.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_posemath_segment line(double x0, double x1, double velocity) {
sim_posemath_segment segment = {
{{x0, 0, 0}, 0, 0, 0, 0, 0, 0},
{{x1, 0, 0}, 0, 0, 0, 0, 0, 0},
{0, 0, 0}, {0, 0, 1}, velocity, 10.0, SIM_POSEMATH_LINEAR, 0
};
return segment;
}
static sim_kinematics_config config_for(int type, const char *coordinates) {
sim_kinematics_config config = {0};
config.type = type;
config.joint_count = (uint32_t)strlen(coordinates);
strcpy(config.coordinates, coordinates);
return config;
}
int main(void) {
sim_kinematics kinematics = {0};
sim_kinematics singular_kins = {0};
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
sim_kinematics_motion adapter = {0};
sim_kinematics_motion singular_adapter = {0};
sim_kinematics_motion_sample sample = {0};
sim_posemath_segment segment = line(0.0, 1.0, 1.0);
uint32_t id = 0;
uint32_t pause_tick;
uint64_t nominal_hash;
uint64_t nominal_repeat_hash;
uint64_t full_planner_before;
uint64_t full_planner_after;
uint64_t full_hash;
uint64_t singular_planner_before;
uint64_t singular_planner_after;
int pause_status;
int resume_status;
int full_status;
int singular_status;
int singular_kinematics_error;
int nonfinite_status;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
assert(sim_kinematics_motion_init(&adapter, &kinematics, 0.1, 10.0, 10.0) == SIM_KIN_MOTION_OK);
assert(sim_kinematics_motion_enqueue(&adapter, &segment, &id) == SIM_KIN_MOTION_OK && id == 1);
assert(sim_kinematics_motion_pause(&adapter, 1) == SIM_KIN_MOTION_OK);
pause_tick = adapter.planner.tick;
pause_status = sim_kinematics_motion_step(&adapter);
assert(pause_status == SIM_KIN_MOTION_ERR_STATE && adapter.planner.tick == pause_tick && adapter.count == 0);
assert(sim_kinematics_motion_pause(&adapter, 0) == SIM_KIN_MOTION_OK);
resume_status = sim_kinematics_motion_step(&adapter);
assert(resume_status == SIM_KIN_MOTION_OK);
assert(sim_kinematics_motion_peek(&adapter, &sample) == SIM_KIN_MOTION_OK);
assert(fabs(sample.pose.tran.x - 0.1) < 1e-12 && fabs(sample.joints[0] - 0.1) < 1e-12);
nominal_hash = sim_kinematics_motion_state_hash(&adapter);
nominal_repeat_hash = sim_kinematics_motion_state_hash(&adapter);
assert(sim_kinematics_motion_pop(&adapter, &sample) == SIM_KIN_MOTION_OK);
assert(sim_kinematics_motion_reset(&adapter) == SIM_KIN_MOTION_OK);
segment = line(0.0, 100.0, 1.0);
assert(sim_kinematics_motion_enqueue(&adapter, &segment, 0) == SIM_KIN_MOTION_OK);
for (uint32_t index = 0; index < SIM_KIN_MOTION_QUEUE_CAPACITY; ++index)
assert(sim_kinematics_motion_step(&adapter) == SIM_KIN_MOTION_OK);
full_planner_before = sim_tp_state_hash(&adapter.planner);
full_status = sim_kinematics_motion_step(&adapter);
full_planner_after = sim_tp_state_hash(&adapter.planner);
full_hash = sim_kinematics_motion_state_hash(&adapter);
assert(full_status == SIM_KIN_MOTION_ERR_CAPACITY);
assert(full_planner_before == full_planner_after && adapter.count == SIM_KIN_MOTION_QUEUE_CAPACITY);
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
assert(sim_kinematics_init(&singular_kins, &config) == SIM_KIN_OK);
assert(sim_kinematics_motion_init(&singular_adapter, &singular_kins, 1.0, 10.0, 10.0) == SIM_KIN_MOTION_OK);
segment = line(0.0, 1.0, 1.0);
segment.end.b = 90.0;
assert(sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0) == SIM_KIN_MOTION_OK);
singular_planner_before = sim_tp_state_hash(&singular_adapter.planner);
singular_status = sim_kinematics_motion_step(&singular_adapter);
singular_planner_after = sim_tp_state_hash(&singular_adapter.planner);
singular_kinematics_error = singular_adapter.kinematics_error;
assert(singular_status == SIM_KIN_MOTION_ERR_INVERSE);
assert(singular_kinematics_error == SIM_KIN_ERR_SINGULAR);
assert(singular_planner_before == singular_planner_after && singular_adapter.count == 0);
segment = line(0.0, 1.0, 1.0);
segment.end.tran.x = NAN;
nonfinite_status = sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0);
assert(nonfinite_status == SIM_KIN_MOTION_ERR_NONFINITE);
printf("{\"schema\":\"kinematics-motion-samples.v1\",\"nominal\":{\"pause_status\":%d,\"resume_status\":%d,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"state_hash\":%llu,\"repeat_hash\":%llu},\"capacity\":{\"status\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu,\"state_hash\":%llu},\"inverse_failure\":{\"status\":%d,\"kinematics_error\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu},\"nonfinite_status\":%d}\n",
pause_status, resume_status, sample.tick, sample.pose.tran.x,
sample.pose.tran.y, sample.pose.tran.z, sample.joints[0],
sample.joints[1], sample.joints[2],
(unsigned long long)nominal_hash, (unsigned long long)nominal_repeat_hash,
full_status, adapter.count, (unsigned long long)full_planner_before,
(unsigned long long)full_planner_after, (unsigned long long)full_hash,
singular_status, singular_kinematics_error, singular_adapter.count,
(unsigned long long)singular_planner_before,
(unsigned long long)singular_planner_after, nonfinite_status);
return 0;
}

63
wasm-core/tests/sim_kinematics_registry_test.c Normal file
View File

@@ -0,0 +1,63 @@
#include "sim_kinematics.h"
#include <assert.h>
#include <stdio.h>
#include <string.h>
int main(void) {
sim_kinematics_capability capability = {0};
sim_kinematics_config config = {0};
uint32_t count = sim_kinematics_registry_count();
uint32_t verified = 0;
uint64_t capability_union = 0;
uint64_t parameter_union = 0;
int lookup_status;
int iterative_require;
int capability_error;
int parameter_error;
int type_mismatch;
int coordinate_error;
int joint_error;
int unknown_name;
int path_name;
int range_error;
assert(count == 18);
printf("{\"schema\":\"kinematics-registry-samples.v1\",\"entries\":[");
for (uint32_t index = 0; index < count; ++index) {
assert(sim_kinematics_registry_get(index, &capability) == SIM_KIN_OK);
memset(&config, 0, sizeof(config));
config.type = capability.type;
config.joint_count = capability.min_joints;
strcpy(config.coordinates, capability.required_coordinates[0] == '\0' ? "X" : capability.required_coordinates);
if (sim_kinematics_registry_validate_config(capability.name, &config, 0) == SIM_KIN_OK) verified += 1;
capability_union |= capability.capabilities;
parameter_union |= capability.parameter_mask;
printf("%s{\"name\":\"%s\",\"type\":%d,\"required\":\"%s\",\"min\":%u,\"max\":%u,\"capabilities\":%u,\"parameters\":%llu}",
index == 0 ? "" : ",", capability.name, capability.type,
capability.required_coordinates, capability.min_joints,
capability.max_joints, capability.capabilities,
(unsigned long long)capability.parameter_mask);
}
lookup_status = sim_kinematics_registry_find("xyzac-trt-kins", &capability);
iterative_require = sim_kinematics_registry_require("genserkins", SIM_KIN_CAP_ITERATIVE, SIM_KIN_PARAM_DH, &capability);
capability_error = sim_kinematics_registry_require("trivkins", SIM_KIN_CAP_ITERATIVE, 0, 0);
parameter_error = sim_kinematics_registry_require("trivkins", 0, SIM_KIN_PARAM_TOOL, 0);
memset(&config, 0, sizeof(config));
config.type = SIM_KIN_TRIVKINS; config.joint_count = 5; strcpy(config.coordinates, "XYZAC");
type_mismatch = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
config.type = SIM_KIN_XYZAC_TRT; strcpy(config.coordinates, "XYZAQ");
coordinate_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
strcpy(config.coordinates, "XYZAC"); config.joint_count = 4;
joint_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
unknown_name = sim_kinematics_registry_find("missing-kins", &capability);
path_name = sim_kinematics_registry_find("plugins/user.so", &capability);
range_error = sim_kinematics_registry_get(count, &capability);
printf("],\"count\":%u,\"verified\":%u,\"capability_union\":%llu,\"parameter_union\":%llu,\"registry_hash\":%llu,\"lookup_status\":%d,\"iterative_require\":%d,\"capability_error\":%d,\"parameter_error\":%d,\"type_mismatch\":%d,\"coordinate_error\":%d,\"joint_error\":%d,\"unknown_name\":%d,\"path_name\":%d,\"range_error\":%d}\n",
count, verified, (unsigned long long)capability_union,
(unsigned long long)parameter_union,
(unsigned long long)sim_kinematics_registry_state_hash(), lookup_status,
iterative_require, capability_error, parameter_error, type_mismatch,
coordinate_error, joint_error, unknown_name, path_name, range_error);
return 0;
}

52
wasm-core/tests/sim_kinematics_test.c
View File

@@ -32,6 +32,11 @@ static double canonical(double value) {
return fabs(value) < 5e-10 ? 0.0 : value;
}
static double canonical_pentapod(double value) {
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
}
static void emit_coupled_samples(void) {
sim_kinematics kinematics = {0};
sim_kinematics_config config;
@@ -123,6 +128,11 @@ static void emit_coupled_samples(void) {
uint32_t penta_branch;
uint32_t penta_alt_branch;
uint32_t penta_iterations;
uint32_t penta_alt_iterations;
uint32_t penta_nonpositive_iterations;
uint32_t penta_iter_limit_iterations;
uint32_t penta_max_error_iterations;
uint32_t penta_singular_iterations;
double penta_residual;
sim_posemath_pose tripod_alt_pose = {0};
sim_posemath_pose genhex_alt_pose = {0};
@@ -341,21 +351,21 @@ static void emit_coupled_samples(void) {
genhex_singular = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
config.penta_initial_pose[0] = 0.0; config.penta_initial_pose[1] = 500.0; config.penta_initial_pose[2] = 300.0; config.penta_initial_pose[3] = 2.0 * 0.017453292519943295769; config.penta_initial_pose[4] = -3.0 * 0.017453292519943295769;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, 5, 0, output, &genser_info) == SIM_KIN_OK);
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 0.01, .b = -0.01}, 5, 0, output, &genser_info) == SIM_KIN_OK);
memcpy(penta_joints, output, sizeof(penta_joints));
assert(sim_kinematics_forward_ex(&kinematics, output, 5, 0, &pose, &genser_info) == SIM_KIN_OK);
penta_pose[0] = canonical(pose.tran.x); penta_pose[1] = canonical(pose.tran.y); penta_pose[2] = canonical(pose.tran.z); penta_pose[3] = canonical(pose.a); penta_pose[4] = canonical(pose.b);
penta_iterations = genser_info.iterations; penta_residual = genser_info.residual; penta_branch = genser_info.branch_id;
penta_pose[0] = canonical_pentapod(pose.tran.x); penta_pose[1] = canonical_pentapod(pose.tran.y); penta_pose[2] = canonical_pentapod(pose.tran.z); penta_pose[3] = canonical_pentapod(pose.a); penta_pose[4] = canonical_pentapod(pose.b);
pose.tran.x = penta_pose[0]; pose.tran.y = penta_pose[1]; pose.tran.z = penta_pose[2]; pose.a = penta_pose[3]; pose.b = penta_pose[4];
penta_iterations = genser_info.iterations; penta_residual = nearbyint(genser_info.residual * 1000000000.0) / 1000000000.0; penta_branch = genser_info.branch_id;
penta_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
penta_alt_status = sim_kinematics_forward_ex(&kinematics, output, 5, SIM_KIN_GENHEX_ALTERNATE_SEED, &penta_alt_pose, &genser_info); penta_alt_branch = genser_info.branch_id;
penta_nonpositive = sim_kinematics_forward(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0}, 5, &pose);
penta_alt_status = sim_kinematics_forward_ex(&kinematics, output, 5, SIM_KIN_GENHEX_ALTERNATE_SEED, &penta_alt_pose, &genser_info); penta_alt_branch = genser_info.branch_id; penta_alt_iterations = genser_info.iterations;
penta_nonpositive = sim_kinematics_forward_ex(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0}, 5, 0, &pose, &genser_info); penta_nonpositive_iterations = genser_info.iterations;
config.penta_initial_pose[3] = 0.0; config.penta_initial_pose[4] = 0.0;
config.penta_iteration_limit = 1; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_iter_limit_error = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
config.penta_iteration_limit = 120; config.penta_max_error = 0.000001; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_max_error_status = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
config.penta_iteration_limit = 1; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_iter_limit_error = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_iter_limit_iterations = genser_info.iterations;
config.penta_iteration_limit = 120; config.penta_max_error = 0.000001; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_max_error_status = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_max_error_iterations = genser_info.iterations;
config.penta_max_error = 1000.0; config.limit_enabled[0] = 1; config.joint_min[0] = 0.0; config.joint_max[0] = 1.0; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, output, 5);
config.limit_enabled[0] = 0; config.penta_base[0][0] = 1.0; config.penta_base[0][1] = 1.0; config.penta_base[0][2] = 1.0; for (int duplicate = 1; duplicate < 5; ++duplicate) memcpy(config.penta_base[duplicate], config.penta_base[0], sizeof(config.penta_base[0])); assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_singular = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
config.limit_enabled[0] = 0; config.penta_base[0][0] = 1.0; config.penta_base[0][1] = 1.0; config.penta_base[0][2] = 1.0; for (int duplicate = 1; duplicate < 5; ++duplicate) memcpy(config.penta_base[duplicate], config.penta_base[0], sizeof(config.penta_base[0])); assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_singular = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_singular_iterations = genser_info.iterations;
printf("{\"schema\":\"kinematics-coupled-samples.v1\","
"\"corexy\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
@@ -378,7 +388,7 @@ static void emit_coupled_samples(void) {
"\"rotarydelta\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"singular\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
"\"tripod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"no_solution\":%d,\"singular\":%d,\"limit_error\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
"\"genhex\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"nonpositive\":%d,\"iteration_limit\":%d,\"max_error\":%d,\"singular\":%d,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f},"
"\"pentapod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"nonpositive\":%d,\"iteration_limit\":%d,\"max_error\":%d,\"singular\":%d,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f}}}\n",
"\"pentapod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"alternate_iterations\":%u,\"nonpositive\":%d,\"nonpositive_iterations\":%u,\"iteration_limit\":%d,\"iteration_limit_iterations\":%u,\"max_error\":%d,\"max_error_iterations\":%u,\"singular\":%d,\"singular_iterations\":%u,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f}}}\n",
corexy_pose[0], corexy_pose[1], corexy_pose[2], corexy_joints[0], corexy_joints[1], corexy_joints[2], (unsigned long long)corexy_hash,
rose_pose[0], rose_pose[1], rose_pose[2], rose_joints[0], rose_joints[1], rose_joints[2], rose_branch, (unsigned long long)rose_hash,
max_pose[0], max_pose[1], max_pose[2], max_joints[0], max_joints[1], max_joints[2], max_joints[3], max_joints[4], max_joints[5], (unsigned long long)max_hash,
@@ -399,7 +409,7 @@ static void emit_coupled_samples(void) {
rdelta_alt_status, rdelta_alt_branch, rdelta_alt_pose.tran.x, rdelta_alt_pose.tran.y, rdelta_alt_pose.tran.z, rdelta_singular, rdelta_geometry_status, rdelta_branch, rdelta_iterations,
tripod_alt_status, tripod_alt_branch, tripod_alt_pose.tran.x, tripod_alt_pose.tran.y, tripod_alt_pose.tran.z, tripod_no_solution, tripod_singular, tripod_limit_error, tripod_geometry_status, tripod_branch, delta_info.iterations,
genhex_alt_status, genhex_alt_branch, genhex_alt_pose.tran.x, genhex_alt_pose.tran.y, genhex_alt_pose.tran.z, genhex_nonpositive, genhex_iter_limit_error, genhex_max_error_status, genhex_singular, genhex_limit_error, genhex_branch, genhex_iterations, genhex_residual,
penta_alt_status, penta_alt_branch, penta_alt_pose.tran.x, penta_alt_pose.tran.y, penta_alt_pose.tran.z, penta_nonpositive, penta_iter_limit_error, penta_max_error_status, penta_singular, penta_limit_error, penta_branch, penta_iterations, penta_residual);
penta_alt_status, penta_alt_branch, penta_alt_pose.tran.x, penta_alt_pose.tran.y, penta_alt_pose.tran.z, penta_alt_iterations, penta_nonpositive, penta_nonpositive_iterations, penta_iter_limit_error, penta_iter_limit_iterations, penta_max_error_status, penta_max_error_iterations, penta_singular, penta_singular_iterations, penta_limit_error, penta_branch, penta_iterations, penta_residual);
}
int sim_kinematics_test_run(void) {
@@ -580,6 +590,26 @@ int sim_kinematics_test_run(void) {
config.dh_alpha[0] = NAN;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
/* Pentapod owns XYZAB only: yaw is not mapped, and LinuxCNC applies the
* tool offset to both base and effector HAL geometry. */
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
config.penta_initial_pose[3] = 2.0 * 0.017453292519943295769;
config.penta_initial_pose[4] = -3.0 * 0.017453292519943295769;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
pose = (sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0, .c = 17.0};
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_OK);
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
assert(fabs(pose.c) < 1e-12);
config.penta_tool_offset = 25.0;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
assert(sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, joints, 5) == SIM_KIN_OK);
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
assert(fabs(pose.tran.z - 300.0) < 1e-9);
config.penta_tool_offset = NAN;
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
config = config_for(SIM_KIN_PENTAPOD, "XYZABC");
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
emit_coupled_samples();
return 0;
}

85
wasm-core/tests/sim_switchkins_test.c Normal file
View File

@@ -0,0 +1,85 @@
#include "sim_kinematics.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_kinematics_config config_for(int type, const char *coordinates) {
sim_kinematics_config config = {0};
config.type = type;
config.joint_count = (uint32_t)strlen(coordinates);
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
return config;
}
int main(void) {
sim_kinematics identity = {0};
sim_kinematics trt = {0};
sim_kinematics_config config;
sim_kinematics_switch switcher = {0};
sim_kinematics_result_info info = {0};
sim_posemath_pose initial = {0};
sim_posemath_pose switched = {0};
sim_posemath_pose returned = {0};
double joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
uint64_t initial_hash;
uint64_t switched_hash;
uint64_t returned_hash;
uint64_t returned_repeat_hash;
int forward_status;
int select_to_trt;
int select_to_identity;
int invalid_mode;
int missing_module;
int register_missing;
int begin_status;
int interrupted_begin;
int abort_status;
int interrupted_commit;
config = config_for(SIM_KIN_TRIVKINS, "XYZAC");
assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
assert(sim_kinematics_init(&trt, &config) == SIM_KIN_OK);
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
assert(sim_kinematics_switch_register(&switcher, 0, &identity, 0) == SIM_KIN_OK);
assert(sim_kinematics_switch_register(&switcher, 1, &trt, 0) == SIM_KIN_OK);
register_missing = sim_kinematics_switch_register(&switcher, 2, 0, 0);
forward_status = sim_kinematics_switch_forward(&switcher, joints, 5, &initial, &info);
assert(forward_status == SIM_KIN_OK);
initial_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &initial);
select_to_trt = sim_kinematics_switch_select(&switcher, 1, joints, 5, &switched, &info);
assert(select_to_trt == SIM_KIN_OK);
assert(fabs(switched.tran.x - initial.tran.x) < 1e-9);
assert(fabs(switched.tran.y - initial.tran.y) < 1e-9);
assert(fabs(switched.tran.z - initial.tran.z) < 1e-9);
switched_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &switched);
select_to_identity = sim_kinematics_switch_select(&switcher, 0, joints, 5, &returned, &info);
assert(select_to_identity == SIM_KIN_OK);
assert(fabs(returned.tran.x - initial.tran.x) < 1e-9);
assert(fabs(returned.tran.y - initial.tran.y) < 1e-9);
assert(fabs(returned.tran.z - initial.tran.z) < 1e-9);
returned_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
returned_repeat_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
invalid_mode = sim_kinematics_switch_select(&switcher, 3, joints, 5, &returned, &info);
missing_module = sim_kinematics_switch_select(&switcher, 2, joints, 5, &returned, &info);
begin_status = sim_kinematics_switch_begin(&switcher, 1);
interrupted_begin = sim_kinematics_switch_begin(&switcher, 0);
abort_status = sim_kinematics_switch_abort(&switcher);
assert(sim_kinematics_switch_begin(&switcher, 1) == SIM_KIN_OK);
abort_status = sim_kinematics_switch_abort(&switcher);
interrupted_commit = sim_kinematics_switch_commit(&switcher, joints, 5, &returned, &info);
assert(sim_kinematics_switch_active(&switcher) == 0);
printf("{\"schema\":\"switchkins-samples.v1\",\"registered\":[%u,%u,%u],\"initial_mode\":0,\"active_mode\":%d,\"forward_status\":%d,\"switch_to_trt\":%d,\"switch_to_identity\":%d,\"initial_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"switched_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"returned_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"initial_hash\":%llu,\"switched_hash\":%llu,\"returned_hash\":%llu,\"returned_repeat_hash\":%llu,\"invalid_mode\":%d,\"missing_module\":%d,\"register_missing\":%d,\"begin_status\":%d,\"interrupted_begin\":%d,\"abort_status\":%d,\"interrupted_commit\":%d,\"generation\":%u,\"switch_count\":%u,\"abort_count\":%u}\n",
switcher.registered[0], switcher.registered[1], switcher.registered[2], switcher.active_mode,
forward_status, select_to_trt, select_to_identity,
initial.tran.x, initial.tran.y, initial.tran.z, initial.a, initial.b, initial.c,
switched.tran.x, switched.tran.y, switched.tran.z, switched.a, switched.b, switched.c,
returned.tran.x, returned.tran.y, returned.tran.z, returned.a, returned.b, returned.c,
(unsigned long long)initial_hash, (unsigned long long)switched_hash, (unsigned long long)returned_hash, (unsigned long long)returned_repeat_hash,
invalid_mode, missing_module, register_missing, begin_status, interrupted_begin, abort_status,
interrupted_commit, switcher.generation, switcher.switch_count, switcher.abort_count);
return 0;
}

126
wasm-core/tests/sim_userkins_test.c Normal file
View File

@@ -0,0 +1,126 @@
#include "sim_kinematics.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static sim_kinematics_config config_for(const char *coordinates) {
sim_kinematics_config config = {0};
config.joint_count = (uint32_t)strlen(coordinates);
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
return config;
}
static sim_kinematics_user_descriptor identity_descriptor(void) {
sim_kinematics_user_descriptor descriptor = {0};
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
SIM_KIN_USER_CAP_STATIC;
descriptor.implementation = SIM_KIN_USER_IDENTITY;
strcpy(descriptor.name, "userk.identity");
descriptor.config = config_for("XYZAC");
return descriptor;
}
static sim_kinematics_user_descriptor genser_descriptor(void) {
sim_kinematics_user_descriptor descriptor = {0};
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
SIM_KIN_USER_CAP_STATIC;
descriptor.implementation = SIM_KIN_USER_GENSER;
strcpy(descriptor.name, "ugenser.static");
descriptor.config = config_for("XYZABC");
descriptor.config.dh_joint_count = 6;
descriptor.config.max_iterations = 200;
descriptor.config.convergence_tolerance = 1e-7;
descriptor.config.dh_a[1] = 85.0;
descriptor.config.dh_a[2] = 380.0;
descriptor.config.dh_a[3] = 100.0;
descriptor.config.dh_alpha[1] = -1.5707963267948966;
descriptor.config.dh_alpha[3] = -1.5707963267948966;
descriptor.config.dh_alpha[4] = 1.5707963267948966;
descriptor.config.dh_alpha[5] = -1.5707963267948966;
descriptor.config.dh_d[0] = 350.0;
descriptor.config.dh_d[3] = 425.0;
descriptor.config.dh_d[5] = 235.0;
return descriptor;
}
int main(void) {
sim_kinematics_user identity = {0};
sim_kinematics_user genser = {0};
sim_kinematics_user uninitialized = {0};
sim_kinematics_user_descriptor descriptor = identity_descriptor();
sim_kinematics_result_info info = {0};
sim_kinematics base = {0};
sim_kinematics_config base_config = config_for("XYZAC");
sim_kinematics_switch switcher = {0};
sim_posemath_pose identity_pose = {0};
sim_posemath_pose genser_pose = {0};
double identity_joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
double identity_roundtrip[5] = {0};
double genser_joints[6] = {0.0, 10.0, -20.0, 30.0, 15.0, -10.0};
double genser_roundtrip[6] = {0};
uint64_t identity_hash;
uint64_t identity_repeat_hash;
uint64_t genser_hash;
uint64_t genser_repeat_hash;
int switch_to_user;
int abi_error;
int capability_error;
int path_name_error;
int missing_implementation;
int dynamic_path_error;
int uninitialized_error;
assert(sim_kinematics_user_init(&identity, &descriptor) == SIM_KIN_OK);
assert(sim_kinematics_user_forward(&identity, identity_joints, 5, &identity_pose, &info) == SIM_KIN_OK);
assert(sim_kinematics_user_inverse(&identity, &identity_pose, identity_roundtrip, 5, &info) == SIM_KIN_OK);
assert(memcmp(identity_joints, identity_roundtrip, sizeof(identity_joints)) == 0);
identity_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
identity_repeat_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
base_config.type = SIM_KIN_TRIVKINS;
assert(sim_kinematics_init(&base, &base_config) == SIM_KIN_OK);
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
assert(sim_kinematics_switch_register(&switcher, 0, &base, 0) == SIM_KIN_OK);
assert(sim_kinematics_user_bind_switch(&identity, &switcher, 2, 0) == SIM_KIN_OK);
switch_to_user = sim_kinematics_switch_select(&switcher, 2, identity_roundtrip, 5, &identity_pose, &info);
assert(switch_to_user == SIM_KIN_OK && sim_kinematics_switch_active(&switcher) == 2);
descriptor = genser_descriptor();
assert(sim_kinematics_user_init(&genser, &descriptor) == SIM_KIN_OK);
assert(sim_kinematics_user_forward(&genser, genser_joints, 6, &genser_pose, &info) == SIM_KIN_OK);
assert(sim_kinematics_user_inverse(&genser, &genser_pose, genser_roundtrip, 6, &info) == SIM_KIN_OK);
assert(sim_kinematics_user_forward(&genser, genser_roundtrip, 6, &genser_pose, &info) == SIM_KIN_OK);
genser_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
genser_repeat_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
descriptor = identity_descriptor();
descriptor.abi_version += 1;
abi_error = sim_kinematics_user_init(&uninitialized, &descriptor);
descriptor = identity_descriptor();
descriptor.capabilities &= ~SIM_KIN_USER_CAP_STATIC;
capability_error = sim_kinematics_user_init(&uninitialized, &descriptor);
descriptor = identity_descriptor();
strcpy(descriptor.name, "plugins/user.so");
path_name_error = sim_kinematics_user_init(&uninitialized, &descriptor);
descriptor = identity_descriptor();
descriptor.implementation = 99;
missing_implementation = sim_kinematics_user_init(&uninitialized, &descriptor);
dynamic_path_error = sim_kinematics_user_reject_dynamic("/tmp/userkins.so");
uninitialized_error = sim_kinematics_user_forward(&uninitialized, identity_joints, 5, &identity_pose, &info);
printf("{\"schema\":\"userkins-samples.v1\",\"abi_version\":%u,\"capabilities\":%u,\"identity\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"state_hash\":%llu,\"repeat_hash\":%llu},\"ugenser\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"iterations\":%u,\"residual\":%.12f,\"state_hash\":%llu,\"repeat_hash\":%llu},\"switch_to_user\":%d,\"active_mode\":%d,\"abi_error\":%d,\"capability_error\":%d,\"path_name_error\":%d,\"missing_implementation\":%d,\"dynamic_path_error\":%d,\"uninitialized_error\":%d}\n",
SIM_KIN_USER_ABI_VERSION, identity.descriptor.capabilities,
identity_pose.tran.x, identity_pose.tran.y, identity_pose.tran.z, identity_pose.a, identity_pose.c,
identity_roundtrip[0], identity_roundtrip[1], identity_roundtrip[2], identity_roundtrip[3], identity_roundtrip[4],
identity.forward_calls, identity.inverse_calls, (unsigned long long)identity_hash, (unsigned long long)identity_repeat_hash,
genser_pose.tran.x, genser_pose.tran.y, genser_pose.tran.z, genser_pose.a, genser_pose.b, genser_pose.c,
genser.forward_calls, genser.inverse_calls, info.iterations, info.residual,
(unsigned long long)genser_hash, (unsigned long long)genser_repeat_hash,
switch_to_user, sim_kinematics_switch_active(&switcher), abi_error, capability_error,
path_name_error, missing_implementation, dynamic_path_error, uninitialized_error);
return 0;
}