Implement WebAssembly runtime and kinematics coverage
This commit is contained in:
@@ -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)
|
||||
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)
|
||||
target_include_directories(sim_core PUBLIC include)
|
||||
target_compile_definitions(sim_core PRIVATE SIM_API_BUILD=1)
|
||||
|
||||
@@ -22,7 +22,9 @@ target_include_directories(sim_posemath PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath
|
||||
)
|
||||
target_compile_definitions(sim_posemath PRIVATE ULAPI)
|
||||
target_compile_options(sim_posemath PRIVATE -ffp-contract=off -fno-fast-math)
|
||||
target_link_libraries(sim_core PUBLIC sim_posemath)
|
||||
target_compile_options(sim_core PRIVATE -ffp-contract=off -fno-fast-math)
|
||||
|
||||
if(NOT EMSCRIPTEN)
|
||||
add_executable(sim_api_test tests/sim_api_test.c)
|
||||
@@ -42,29 +44,59 @@ if(NOT EMSCRIPTEN)
|
||||
target_include_directories(sim_event_test PRIVATE include)
|
||||
add_test(NAME sim_event_test COMMAND sim_event_test)
|
||||
|
||||
add_executable(sim_hal_test tests/sim_hal_test.c)
|
||||
add_executable(sim_hal_test tests/sim_hal_test.c tests/sim_hal_test_main.c)
|
||||
target_link_libraries(sim_hal_test PRIVATE sim_core)
|
||||
target_include_directories(sim_hal_test PRIVATE include)
|
||||
target_compile_options(sim_hal_test PRIVATE -UNDEBUG)
|
||||
add_test(NAME sim_hal_test COMMAND sim_hal_test)
|
||||
|
||||
add_executable(sim_posemath_test tests/sim_posemath_test.c)
|
||||
target_link_libraries(sim_posemath_test PRIVATE sim_core)
|
||||
target_include_directories(sim_posemath_test PRIVATE include)
|
||||
add_test(NAME sim_posemath_test COMMAND sim_posemath_test)
|
||||
|
||||
add_executable(sim_interpreter_test tests/sim_interpreter_test.c)
|
||||
target_link_libraries(sim_interpreter_test PRIVATE sim_core)
|
||||
target_include_directories(sim_interpreter_test PRIVATE include)
|
||||
add_test(NAME sim_interpreter_test COMMAND sim_interpreter_test)
|
||||
|
||||
add_executable(sim_motion_queue_test tests/sim_motion_queue_test.c)
|
||||
target_link_libraries(sim_motion_queue_test PRIVATE sim_core)
|
||||
target_include_directories(sim_motion_queue_test PRIVATE include)
|
||||
add_test(NAME sim_motion_queue_test COMMAND sim_motion_queue_test)
|
||||
|
||||
add_executable(sim_kinematics_test tests/sim_kinematics_test.c tests/sim_kinematics_test_main.c)
|
||||
target_link_libraries(sim_kinematics_test PRIVATE sim_core)
|
||||
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)
|
||||
endif()
|
||||
|
||||
if(EMSCRIPTEN)
|
||||
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)
|
||||
add_executable(sim_hal_wasm_test tests/sim_hal_test.c tests/sim_hal_test_main.c)
|
||||
target_link_libraries(sim_hal_wasm_test PRIVATE sim_core)
|
||||
target_include_directories(sim_hal_wasm_test PRIVATE include)
|
||||
target_compile_options(sim_hal_wasm_test PRIVATE -UNDEBUG)
|
||||
target_link_options(sim_hal_wasm_test PRIVATE -sSINGLE_FILE=1)
|
||||
|
||||
add_executable(sim_kinematics_wasm_test tests/sim_kinematics_test.c tests/sim_kinematics_test_main.c)
|
||||
target_link_libraries(sim_kinematics_wasm_test PRIVATE sim_core)
|
||||
target_include_directories(sim_kinematics_wasm_test PRIVATE include)
|
||||
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)
|
||||
target_include_directories(sim_core_module PRIVATE
|
||||
include
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/rtapi
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../linuxcnc/src/libnml/posemath
|
||||
)
|
||||
target_compile_options(sim_core_module PRIVATE -ffp-contract=off -fno-fast-math)
|
||||
target_link_libraries(sim_core_module PRIVATE sim_posemath)
|
||||
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'] -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'] -sEXPORTED_RUNTIME_METHODS=['ccall','cwrap']"
|
||||
)
|
||||
endif()
|
||||
|
||||
@@ -49,6 +49,10 @@ int32_t sim_hal_get_pin(const char *name, sim_hal_value *out);
|
||||
int32_t sim_hal_connect(const char *source, const char *target);
|
||||
int32_t sim_hal_register_thread(const char *name, uint32_t period_ticks);
|
||||
int32_t sim_hal_register_function(const char *thread, const char *name);
|
||||
/* Register the static LinuxCNC corexy_by_hal component and its servo pins. */
|
||||
int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread);
|
||||
/* Execute one deterministic CoreXY command/feedback transform immediately. */
|
||||
int32_t sim_hal_corexy_transform(const char *component);
|
||||
int32_t sim_hal_set_function_failure(const char *name, uint32_t fail);
|
||||
int32_t sim_hal_step(uint32_t ticks);
|
||||
uint32_t sim_hal_tick(void);
|
||||
|
||||
74
wasm-core/include/sim_interpreter.h
Normal file
74
wasm-core/include/sim_interpreter.h
Normal file
@@ -0,0 +1,74 @@
|
||||
#ifndef LINUXCNC_WEB_WASM_SIM_INTERPRETER_H
|
||||
#define LINUXCNC_WEB_WASM_SIM_INTERPRETER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
enum sim_interpreter_result {
|
||||
SIM_INTERP_OK = 0,
|
||||
SIM_INTERP_ERR_ARGUMENT = -500,
|
||||
SIM_INTERP_ERR_NONFINITE = -501,
|
||||
SIM_INTERP_ERR_SYNTAX = -502,
|
||||
SIM_INTERP_ERR_RANGE = -503,
|
||||
SIM_INTERP_ERR_UNKNOWN_CODE = -504,
|
||||
SIM_INTERP_ERR_STATE = -505,
|
||||
SIM_INTERP_ERR_BUFFER = -506,
|
||||
};
|
||||
|
||||
enum sim_interpreter_units {
|
||||
SIM_INTERP_UNITS_INCH = 20,
|
||||
SIM_INTERP_UNITS_MM = 21,
|
||||
};
|
||||
|
||||
enum sim_interpreter_distance_mode {
|
||||
SIM_INTERP_ABSOLUTE = 90,
|
||||
SIM_INTERP_INCREMENTAL = 91,
|
||||
};
|
||||
|
||||
typedef struct sim_interpreter_state {
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
double a;
|
||||
double b;
|
||||
double c;
|
||||
double u;
|
||||
double v;
|
||||
double w;
|
||||
double feed;
|
||||
double speed;
|
||||
uint32_t line;
|
||||
int units;
|
||||
int distance_mode;
|
||||
int plane;
|
||||
int motion_mode;
|
||||
int spindle_mode;
|
||||
int program_end;
|
||||
int status;
|
||||
} sim_interpreter_state;
|
||||
|
||||
#define SIM_INTERPRETER_CANONICAL_CAPACITY 512u
|
||||
|
||||
typedef struct sim_interpreter {
|
||||
sim_interpreter_state state;
|
||||
uint64_t block_hash;
|
||||
uint32_t canonical_length;
|
||||
char canonical[SIM_INTERPRETER_CANONICAL_CAPACITY];
|
||||
} sim_interpreter;
|
||||
|
||||
int sim_interpreter_init(sim_interpreter *interpreter);
|
||||
int sim_interpreter_reset(sim_interpreter *interpreter);
|
||||
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length);
|
||||
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state);
|
||||
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity);
|
||||
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter);
|
||||
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
174
wasm-core/include/sim_kinematics.h
Normal file
174
wasm-core/include/sim_kinematics.h
Normal file
@@ -0,0 +1,174 @@
|
||||
#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_H
|
||||
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_H
|
||||
|
||||
#include "sim_posemath.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
enum sim_kinematics_result {
|
||||
SIM_KIN_OK = 0,
|
||||
SIM_KIN_ERR_ARGUMENT = -700,
|
||||
SIM_KIN_ERR_NONFINITE = -701,
|
||||
SIM_KIN_ERR_RANGE = -702,
|
||||
SIM_KIN_ERR_CONFIGURATION = -703,
|
||||
SIM_KIN_ERR_SINGULAR = -704,
|
||||
SIM_KIN_ERR_UNKNOWN_TYPE = -705,
|
||||
SIM_KIN_ERR_UNSUPPORTED = -706,
|
||||
};
|
||||
|
||||
enum sim_kinematics_type {
|
||||
SIM_KIN_TRIVKINS = 1,
|
||||
SIM_KIN_XYZAC_TRT = 2,
|
||||
SIM_KIN_XYZBC_TRT = 3,
|
||||
SIM_KIN_XYZBCW = 4,
|
||||
SIM_KIN_COREXY = 5,
|
||||
SIM_KIN_ROSE = 6,
|
||||
SIM_KIN_MAXKINS = 7,
|
||||
SIM_KIN_ROTATEKINS = 8,
|
||||
SIM_KIN_5AXISKINS = 9,
|
||||
SIM_KIN_SCARA = 10,
|
||||
SIM_KIN_PUMA = 11,
|
||||
SIM_KIN_SCORBOT = 12,
|
||||
SIM_KIN_GENSER = 13,
|
||||
SIM_KIN_LINEAR_DELTA = 14,
|
||||
SIM_KIN_ROTARY_DELTA = 15,
|
||||
SIM_KIN_TRIPOD = 16,
|
||||
SIM_KIN_GENHEX = 17,
|
||||
SIM_KIN_PENTAPOD = 18,
|
||||
};
|
||||
|
||||
/* LinuxCNC kinematics mode semantics. Zero means the module default. */
|
||||
enum sim_kinematics_mode {
|
||||
SIM_KIN_MODE_DEFAULT = 0,
|
||||
SIM_KIN_MODE_IDENTITY = 1,
|
||||
SIM_KIN_MODE_FORWARD_ONLY = 2,
|
||||
SIM_KIN_MODE_INVERSE_ONLY = 3,
|
||||
SIM_KIN_MODE_BOTH = 4,
|
||||
};
|
||||
|
||||
/* Delta mechanisms expose the two analytic roots through the flags fields.
|
||||
* The default (zero) is LinuxCNC's lower/platform root; bit zero selects the
|
||||
* alternate root for callers that need an explicit branch comparison. */
|
||||
#define SIM_KIN_DELTA_ALTERNATE_ROOT UINT64_C(1)
|
||||
/* Tripod forward flags select the below-base solution when set. */
|
||||
#define SIM_KIN_TRIPOD_BELOW_ROOT UINT64_C(1)
|
||||
/* Genhex forward flags select the alternate (below-platform) initial seed. */
|
||||
#define SIM_KIN_GENHEX_ALTERNATE_SEED UINT64_C(2)
|
||||
|
||||
#define SIM_KIN_MAX_JOINTS 9u
|
||||
#define SIM_KIN_GENHEX_STRUTS 6u
|
||||
#define SIM_KIN_PENTA_STRUTS 5u
|
||||
|
||||
typedef struct sim_kinematics_config {
|
||||
int type;
|
||||
int mode;
|
||||
uint32_t joint_count;
|
||||
/* Total configured joints and the prefix owned by kinematics. */
|
||||
uint32_t kinematics_joint_count;
|
||||
uint32_t extra_joint_count;
|
||||
char coordinates[16];
|
||||
double x_rot_point;
|
||||
double y_rot_point;
|
||||
double z_rot_point;
|
||||
double x_offset;
|
||||
double y_offset;
|
||||
double z_offset;
|
||||
double tool_offset;
|
||||
double pivot_length;
|
||||
double scara_d1;
|
||||
double scara_d2;
|
||||
double scara_d3;
|
||||
double scara_d4;
|
||||
double scara_d5;
|
||||
double scara_d6;
|
||||
double puma_a2;
|
||||
double puma_a3;
|
||||
double puma_d3;
|
||||
double puma_d4;
|
||||
double puma_d6;
|
||||
uint32_t dh_joint_count;
|
||||
double dh_a[SIM_KIN_MAX_JOINTS];
|
||||
double dh_alpha[SIM_KIN_MAX_JOINTS];
|
||||
double dh_d[SIM_KIN_MAX_JOINTS];
|
||||
double delta_radius;
|
||||
double delta_rod_length;
|
||||
double rotary_platform_radius;
|
||||
double rotary_thigh_length;
|
||||
double rotary_shin_length;
|
||||
double rotary_foot_radius;
|
||||
double tripod_bx;
|
||||
double tripod_cx;
|
||||
double tripod_cy;
|
||||
double genhex_base[SIM_KIN_GENHEX_STRUTS][3];
|
||||
double genhex_platform[SIM_KIN_GENHEX_STRUTS][3];
|
||||
double genhex_base_axis[SIM_KIN_GENHEX_STRUTS][3];
|
||||
double genhex_platform_axis[SIM_KIN_GENHEX_STRUTS][3];
|
||||
double genhex_screw_lead;
|
||||
double genhex_tool_offset;
|
||||
double genhex_spindle_offset;
|
||||
double genhex_max_error;
|
||||
double genhex_convergence_tolerance;
|
||||
uint32_t genhex_iteration_limit;
|
||||
double genhex_initial_pose[6];
|
||||
double penta_base[SIM_KIN_PENTA_STRUTS][3];
|
||||
double penta_effector_radius[SIM_KIN_PENTA_STRUTS];
|
||||
double penta_effector_z[SIM_KIN_PENTA_STRUTS];
|
||||
double penta_tool_offset;
|
||||
double penta_max_error;
|
||||
double penta_convergence_tolerance;
|
||||
uint32_t penta_iteration_limit;
|
||||
double penta_initial_pose[5];
|
||||
uint32_t max_iterations;
|
||||
double convergence_tolerance;
|
||||
uint8_t limit_enabled[SIM_KIN_MAX_JOINTS];
|
||||
double joint_min[SIM_KIN_MAX_JOINTS];
|
||||
double joint_max[SIM_KIN_MAX_JOINTS];
|
||||
} sim_kinematics_config;
|
||||
|
||||
typedef struct sim_kinematics {
|
||||
sim_kinematics_config config;
|
||||
int axis_joint[SIM_KIN_MAX_JOINTS];
|
||||
uint32_t configured;
|
||||
uint64_t forward_flags;
|
||||
uint64_t inverse_flags;
|
||||
sim_posemath_pose last_pose;
|
||||
double last_residual;
|
||||
uint32_t last_iterations;
|
||||
uint32_t branch_id;
|
||||
int32_t revolution_count;
|
||||
uint32_t previous_quadrant;
|
||||
} sim_kinematics;
|
||||
|
||||
typedef struct sim_kinematics_result_info {
|
||||
uint64_t forward_flags;
|
||||
uint64_t inverse_flags;
|
||||
double residual;
|
||||
uint32_t iterations;
|
||||
uint32_t branch_id;
|
||||
} sim_kinematics_result_info;
|
||||
|
||||
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);
|
||||
int sim_kinematics_forward_ex(const sim_kinematics *kinematics, const double *joints,
|
||||
uint32_t joint_count, uint64_t inverse_flags,
|
||||
sim_posemath_pose *pose, sim_kinematics_result_info *info);
|
||||
int sim_kinematics_inverse(sim_kinematics *kinematics, const sim_posemath_pose *pose,
|
||||
double *joints, uint32_t joint_count);
|
||||
int sim_kinematics_inverse_ex(sim_kinematics *kinematics, const sim_posemath_pose *pose,
|
||||
uint32_t joint_count, uint64_t forward_flags,
|
||||
double *joints, sim_kinematics_result_info *info);
|
||||
int sim_kinematics_home(const sim_kinematics *kinematics, double *joints, uint32_t joint_count,
|
||||
sim_posemath_pose *pose);
|
||||
int sim_kinematics_validate_pose(const sim_posemath_pose *pose);
|
||||
uint64_t sim_kinematics_state_hash(const sim_kinematics *kinematics,
|
||||
const double *joints, uint32_t joint_count,
|
||||
const sim_posemath_pose *pose);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
66
wasm-core/include/sim_motion_queue.h
Normal file
66
wasm-core/include/sim_motion_queue.h
Normal file
@@ -0,0 +1,66 @@
|
||||
#ifndef LINUXCNC_WEB_WASM_SIM_MOTION_QUEUE_H
|
||||
#define LINUXCNC_WEB_WASM_SIM_MOTION_QUEUE_H
|
||||
|
||||
#include "sim_posemath.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
enum sim_motion_result {
|
||||
SIM_MOTION_OK = 0,
|
||||
SIM_MOTION_ERR_ARGUMENT = -600,
|
||||
SIM_MOTION_ERR_NONFINITE = -601,
|
||||
SIM_MOTION_ERR_RANGE = -602,
|
||||
SIM_MOTION_ERR_GEOMETRY = -603,
|
||||
SIM_MOTION_ERR_MOTION_TYPE = -604,
|
||||
SIM_MOTION_ERR_STATE = -605,
|
||||
SIM_MOTION_ERR_CAPACITY = -606,
|
||||
SIM_MOTION_ERR_BUFFER = -607,
|
||||
};
|
||||
|
||||
#define SIM_MOTION_QUEUE_CAPACITY 64u
|
||||
|
||||
typedef struct sim_motion_command {
|
||||
sim_posemath_segment segment;
|
||||
uint32_t id;
|
||||
uint32_t tag;
|
||||
} sim_motion_command;
|
||||
|
||||
typedef struct sim_motion_state {
|
||||
sim_posemath_pose position;
|
||||
double velocity;
|
||||
double progress;
|
||||
uint32_t tick;
|
||||
uint32_t queue_depth;
|
||||
uint32_t active_id;
|
||||
uint32_t active_tag;
|
||||
int motion_type;
|
||||
int status;
|
||||
} sim_motion_state;
|
||||
|
||||
typedef struct sim_motion_queue {
|
||||
sim_motion_command commands[SIM_MOTION_QUEUE_CAPACITY];
|
||||
uint32_t head;
|
||||
uint32_t count;
|
||||
uint32_t next_id;
|
||||
uint32_t tick;
|
||||
double cycle_time;
|
||||
double velocity_limit;
|
||||
sim_motion_state state;
|
||||
} sim_motion_queue;
|
||||
|
||||
int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_limit);
|
||||
int sim_motion_reset(sim_motion_queue *queue);
|
||||
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out);
|
||||
int sim_motion_step(sim_motion_queue *queue);
|
||||
int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state);
|
||||
uint32_t sim_motion_queue_depth(const sim_motion_queue *queue);
|
||||
uint64_t sim_motion_state_hash(const sim_motion_queue *queue);
|
||||
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -5,12 +5,25 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
enum sim_posemath_result {
|
||||
SIM_POSEMATH_OK = 0,
|
||||
SIM_POSEMATH_ERR_ARGUMENT = -400,
|
||||
SIM_POSEMATH_ERR_NONFINITE = -401,
|
||||
SIM_POSEMATH_ERR_DEGENERATE = -402,
|
||||
SIM_POSEMATH_ERR_RANGE = -403,
|
||||
SIM_POSEMATH_ERR_MOTION_TYPE = -404,
|
||||
SIM_POSEMATH_ERR_STATE = -405,
|
||||
SIM_POSEMATH_ERR_CAPACITY = -406,
|
||||
};
|
||||
|
||||
/* Values match LinuxCNC's tc_motion_type_t. */
|
||||
enum sim_posemath_motion_type {
|
||||
SIM_POSEMATH_LINEAR = 1,
|
||||
SIM_POSEMATH_CIRCULAR = 2,
|
||||
SIM_POSEMATH_RIGID_TAP = 3,
|
||||
SIM_POSEMATH_SPHERICAL = 4,
|
||||
};
|
||||
|
||||
typedef struct sim_posemath_cartesian {
|
||||
@@ -19,11 +32,77 @@ typedef struct sim_posemath_cartesian {
|
||||
double z;
|
||||
} sim_posemath_cartesian;
|
||||
|
||||
typedef struct sim_posemath_pose {
|
||||
sim_posemath_cartesian tran;
|
||||
double a;
|
||||
double b;
|
||||
double c;
|
||||
double u;
|
||||
double v;
|
||||
double w;
|
||||
} sim_posemath_pose;
|
||||
|
||||
typedef struct sim_posemath_segment {
|
||||
sim_posemath_pose start;
|
||||
sim_posemath_pose end;
|
||||
sim_posemath_cartesian center;
|
||||
sim_posemath_cartesian normal;
|
||||
double velocity;
|
||||
double acceleration;
|
||||
int motion_type;
|
||||
int turn;
|
||||
} sim_posemath_segment;
|
||||
|
||||
typedef struct sim_tp_state {
|
||||
sim_posemath_pose position;
|
||||
double velocity;
|
||||
double progress;
|
||||
uint32_t tick;
|
||||
uint32_t queue_depth;
|
||||
uint32_t active_id;
|
||||
int motion_type;
|
||||
int status;
|
||||
} sim_tp_state;
|
||||
|
||||
#define SIM_TP_QUEUE_CAPACITY 32u
|
||||
|
||||
typedef struct sim_tp {
|
||||
sim_posemath_segment queue[SIM_TP_QUEUE_CAPACITY];
|
||||
uint32_t ids[SIM_TP_QUEUE_CAPACITY];
|
||||
uint32_t head;
|
||||
uint32_t count;
|
||||
uint32_t next_id;
|
||||
uint32_t active_id;
|
||||
uint32_t tick;
|
||||
double cycle_time;
|
||||
double velocity_limit;
|
||||
double acceleration_limit;
|
||||
double progress;
|
||||
double current_velocity;
|
||||
sim_posemath_pose position;
|
||||
int motion_type;
|
||||
int status;
|
||||
} sim_tp;
|
||||
|
||||
int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output);
|
||||
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output);
|
||||
int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
|
||||
double fraction, sim_posemath_cartesian *output);
|
||||
int sim_posemath_is_finite(const sim_posemath_cartesian *value);
|
||||
int sim_posemath_pose_is_finite(const sim_posemath_pose *value);
|
||||
int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
|
||||
double fraction, sim_posemath_pose *output);
|
||||
int sim_posemath_segment_validate(const sim_posemath_segment *segment);
|
||||
int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
|
||||
sim_posemath_pose *output);
|
||||
uint64_t sim_posemath_hash_state(const sim_tp_state *state);
|
||||
|
||||
int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit);
|
||||
int sim_tp_reset(sim_tp *tp);
|
||||
int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out);
|
||||
int sim_tp_step(sim_tp *tp);
|
||||
int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state);
|
||||
uint64_t sim_tp_state_hash(const sim_tp *tp);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@@ -21,8 +21,11 @@ typedef struct {
|
||||
uint32_t thread;
|
||||
uint32_t calls;
|
||||
uint32_t fail;
|
||||
uint32_t transform_kind;
|
||||
} function_entry;
|
||||
|
||||
enum { SIM_HAL_TRANSFORM_NONE = 0, SIM_HAL_TRANSFORM_COREXY = 1 };
|
||||
|
||||
typedef struct {
|
||||
uint32_t tick;
|
||||
pin_entry pins[SIM_HAL_PIN_CAPACITY];
|
||||
@@ -136,6 +139,73 @@ int32_t sim_hal_register_function(const char *thread, const char *name) {
|
||||
return SIM_HAL_ERR_FULL;
|
||||
}
|
||||
|
||||
static int32_t corexy_pin_names(const char *component, char names[8][SIM_HAL_NAME_CAPACITY]) {
|
||||
static const char *suffixes[8] = {
|
||||
".j0-motor-pos-cmd", ".j1-motor-pos-cmd", ".alpha-cmd", ".beta-cmd",
|
||||
".alpha-fb", ".beta-fb", ".j0-motor-pos-fb", ".j1-motor-pos-fb",
|
||||
};
|
||||
uint32_t index;
|
||||
if (!valid_name(component)) return SIM_HAL_ERR_ARGUMENT;
|
||||
for (index = 0; index < 8; ++index) {
|
||||
size_t prefix = strlen(component);
|
||||
size_t suffix = strlen(suffixes[index]);
|
||||
if (prefix + suffix >= SIM_HAL_NAME_CAPACITY) return SIM_HAL_ERR_RANGE;
|
||||
memcpy(names[index], component, prefix);
|
||||
memcpy(names[index] + prefix, suffixes[index], suffix + 1);
|
||||
}
|
||||
return SIM_HAL_OK;
|
||||
}
|
||||
|
||||
int32_t sim_hal_register_corexy_by_hal(const char *component, const char *thread) {
|
||||
char names[8][SIM_HAL_NAME_CAPACITY];
|
||||
uint32_t index;
|
||||
int32_t result;
|
||||
if (component == NULL || thread == NULL) return SIM_HAL_ERR_ARGUMENT;
|
||||
result = corexy_pin_names(component, names);
|
||||
if (result != SIM_HAL_OK) return result;
|
||||
for (index = 0; index < 8; ++index) {
|
||||
if (find_pin(names[index]) >= 0) return SIM_HAL_ERR_DUPLICATE;
|
||||
}
|
||||
if (find_function(component) >= 0) return SIM_HAL_ERR_DUPLICATE;
|
||||
for (index = 0; index < 8; ++index) {
|
||||
result = sim_hal_register_pin(names[index], SIM_HAL_FLOAT);
|
||||
if (result != SIM_HAL_OK) return result;
|
||||
}
|
||||
result = sim_hal_register_function(thread, component);
|
||||
if (result != SIM_HAL_OK) return result;
|
||||
state.functions[find_function(component)].transform_kind = SIM_HAL_TRANSFORM_COREXY;
|
||||
return SIM_HAL_OK;
|
||||
}
|
||||
|
||||
static int32_t float_pin(const char *name, float *value) {
|
||||
sim_hal_value pin = {0};
|
||||
int32_t result = sim_hal_get_pin(name, &pin);
|
||||
if (result != SIM_HAL_OK) return result;
|
||||
if (pin.type != SIM_HAL_FLOAT) return SIM_HAL_ERR_TYPE;
|
||||
*value = pin.value.real;
|
||||
return SIM_HAL_OK;
|
||||
}
|
||||
|
||||
static int32_t set_float_pin(const char *name, float value) {
|
||||
sim_hal_value pin = {SIM_HAL_FLOAT, {.real = value}};
|
||||
return sim_hal_set_pin(name, pin);
|
||||
}
|
||||
|
||||
int32_t sim_hal_corexy_transform(const char *component) {
|
||||
char names[8][SIM_HAL_NAME_CAPACITY];
|
||||
float j0_cmd, j1_cmd, alpha_fb, beta_fb;
|
||||
int32_t result = corexy_pin_names(component, names);
|
||||
if (result != SIM_HAL_OK) return result;
|
||||
result = float_pin(names[0], &j0_cmd); if (result != SIM_HAL_OK) return result;
|
||||
result = float_pin(names[1], &j1_cmd); if (result != SIM_HAL_OK) return result;
|
||||
result = float_pin(names[4], &alpha_fb); if (result != SIM_HAL_OK) return result;
|
||||
result = float_pin(names[5], &beta_fb); if (result != SIM_HAL_OK) return result;
|
||||
result = set_float_pin(names[2], 0.5f * (j0_cmd + j1_cmd)); if (result != SIM_HAL_OK) return result;
|
||||
result = set_float_pin(names[3], 0.5f * (j0_cmd - j1_cmd)); if (result != SIM_HAL_OK) return result;
|
||||
result = set_float_pin(names[6], alpha_fb + beta_fb); if (result != SIM_HAL_OK) return result;
|
||||
return set_float_pin(names[7], alpha_fb - beta_fb);
|
||||
}
|
||||
|
||||
int32_t sim_hal_set_function_failure(const char *name, uint32_t fail) {
|
||||
int index = find_function(name);
|
||||
if (index < 0) return SIM_HAL_ERR_NOT_FOUND;
|
||||
@@ -152,6 +222,10 @@ int32_t sim_hal_step(uint32_t ticks) {
|
||||
function_entry *function = &state.functions[function_index];
|
||||
if (!function->used || tick % state.threads[function->thread].period != 0U) continue;
|
||||
if (function->fail) return SIM_HAL_ERR_FUNCTION;
|
||||
if (function->transform_kind == SIM_HAL_TRANSFORM_COREXY) {
|
||||
int32_t result = sim_hal_corexy_transform(function->name);
|
||||
if (result != SIM_HAL_OK) return SIM_HAL_ERR_FUNCTION;
|
||||
}
|
||||
function->calls += 1U;
|
||||
}
|
||||
}
|
||||
|
||||
257
wasm-core/src/sim_interpreter.c
Normal file
257
wasm-core/src/sim_interpreter.c
Normal file
@@ -0,0 +1,257 @@
|
||||
#include "sim_interpreter.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
|
||||
uint64_t hash = UINT64_C(1469598103934665603);
|
||||
size_t index;
|
||||
for (index = 0; index < length; ++index) {
|
||||
hash ^= (uint64_t)bytes[index];
|
||||
hash *= UINT64_C(1099511628211);
|
||||
}
|
||||
return hash;
|
||||
}
|
||||
|
||||
static int finite_state(const sim_interpreter_state *state) {
|
||||
const double *values;
|
||||
size_t index;
|
||||
if (state == 0) return 0;
|
||||
values = &state->x;
|
||||
for (index = 0; index < 11; ++index) if (!isfinite(values[index])) return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int parse_number(const char **cursor, const char *end, double *output) {
|
||||
const char *start = *cursor;
|
||||
int sign = 1;
|
||||
int digits = 0;
|
||||
int exponent_sign = 1;
|
||||
int exponent = 0;
|
||||
long double value = 0.0L;
|
||||
long double scale = 1.0L;
|
||||
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
|
||||
if (**cursor == '-') sign = -1;
|
||||
++*cursor;
|
||||
}
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
value = value * 10.0L + (long double)(**cursor - '0');
|
||||
++*cursor;
|
||||
digits = 1;
|
||||
}
|
||||
if (*cursor < end && **cursor == '.') {
|
||||
++*cursor;
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
value = value * 10.0L + (long double)(**cursor - '0');
|
||||
scale *= 10.0L;
|
||||
++*cursor;
|
||||
digits = 1;
|
||||
}
|
||||
}
|
||||
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
||||
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
|
||||
++*cursor;
|
||||
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
|
||||
if (**cursor == '-') exponent_sign = -1;
|
||||
++*cursor;
|
||||
}
|
||||
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0');
|
||||
++*cursor;
|
||||
}
|
||||
}
|
||||
value = (long double)sign * value / scale;
|
||||
if (exponent) value *= powl(10.0L, (long double)(exponent_sign * exponent));
|
||||
*output = (double)value;
|
||||
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
||||
}
|
||||
|
||||
static int code_is(double value, int integer_code) {
|
||||
return isfinite(value) && fabs(value - (double)integer_code) <= 1e-12;
|
||||
}
|
||||
|
||||
static int axis_index(char letter) {
|
||||
switch (letter) {
|
||||
case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
|
||||
case 'A': return 3; case 'B': return 4; case 'C': return 5;
|
||||
case 'U': return 6; case 'V': return 7; case 'W': return 8;
|
||||
default: return -1;
|
||||
}
|
||||
}
|
||||
|
||||
static double *axis_pointer(sim_interpreter_state *state, int index) {
|
||||
return &state->x + index;
|
||||
}
|
||||
|
||||
static int apply_g(sim_interpreter_state *state, double code, int *groups) {
|
||||
if (code_is(code, 0)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 0; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 1)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 1; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 2) || code_is(code, 3)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 17) || code_is(code, 18) || code_is(code, 19)) { if (++groups[1] > 1) return SIM_INTERP_ERR_STATE; state->plane = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 20) || code_is(code, 21)) { if (++groups[2] > 1) return SIM_INTERP_ERR_STATE; state->units = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 90) || code_is(code, 91)) { if (++groups[3] > 1) return SIM_INTERP_ERR_STATE; state->distance_mode = (int)code; return SIM_INTERP_OK; }
|
||||
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
}
|
||||
|
||||
static int apply_m(sim_interpreter_state *state, double code, int *groups) {
|
||||
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) {
|
||||
if (++groups[4] > 1) return SIM_INTERP_ERR_STATE;
|
||||
state->spindle_mode = (int)code;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
if (code_is(code, 2) || code_is(code, 30)) {
|
||||
if (++groups[5] > 1) return SIM_INTERP_ERR_STATE;
|
||||
state->program_end = 1;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
}
|
||||
|
||||
static uint64_t state_hash(const sim_interpreter_state *state) {
|
||||
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7];
|
||||
size_t offset = 0;
|
||||
const double *values = &state->x;
|
||||
size_t index;
|
||||
for (index = 0; index < 11; ++index) { memcpy(bytes + offset, values + index, sizeof(double)); offset += sizeof(double); }
|
||||
memcpy(bytes + offset, &state->line, sizeof(uint32_t)); offset += sizeof(uint32_t);
|
||||
memcpy(bytes + offset, &state->units, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->distance_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->plane, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->motion_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->spindle_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->program_end, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->status, sizeof(int)); offset += sizeof(int);
|
||||
return fnv1a64(bytes, offset);
|
||||
}
|
||||
|
||||
static int make_canonical(sim_interpreter *interpreter) {
|
||||
int written;
|
||||
const sim_interpreter_state *s = &interpreter->state;
|
||||
written = snprintf(interpreter->canonical, sizeof(interpreter->canonical),
|
||||
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
|
||||
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, s->motion_mode, s->plane, s->program_end,
|
||||
s->speed, s->spindle_mode, s->status, s->units, s->u, s->v, s->w, s->x, s->y, s->z);
|
||||
if (written < 0 || (size_t)written >= sizeof(interpreter->canonical)) return SIM_INTERP_ERR_BUFFER;
|
||||
interpreter->canonical_length = (uint32_t)written;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
int sim_interpreter_reset(sim_interpreter *interpreter) {
|
||||
if (interpreter == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
memset(interpreter, 0, sizeof(*interpreter));
|
||||
interpreter->state.units = SIM_INTERP_UNITS_MM;
|
||||
interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE;
|
||||
interpreter->state.plane = 17;
|
||||
interpreter->state.motion_mode = 1;
|
||||
interpreter->state.spindle_mode = 5;
|
||||
interpreter->state.status = SIM_INTERP_OK;
|
||||
return make_canonical(interpreter);
|
||||
}
|
||||
|
||||
int sim_interpreter_init(sim_interpreter *interpreter) {
|
||||
return sim_interpreter_reset(interpreter);
|
||||
}
|
||||
|
||||
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length) {
|
||||
sim_interpreter_state next;
|
||||
const char *cursor;
|
||||
const char *end;
|
||||
int groups[6] = {0};
|
||||
int seen_axis[9] = {0};
|
||||
double axis_values[9] = {0};
|
||||
int seen_feed = 0;
|
||||
int seen_speed = 0;
|
||||
double feed_value = 0.0;
|
||||
double speed_value = 0.0;
|
||||
int seen_line = 0;
|
||||
int result = SIM_INTERP_OK;
|
||||
if (interpreter == 0 || block == 0 || length == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
if (!finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
|
||||
if (interpreter->state.program_end) return SIM_INTERP_ERR_STATE;
|
||||
next = interpreter->state;
|
||||
cursor = block;
|
||||
end = block + length;
|
||||
while (cursor < end) {
|
||||
char letter;
|
||||
double value;
|
||||
int index;
|
||||
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
|
||||
if (cursor >= end) break;
|
||||
if (*cursor == ';') break;
|
||||
if (*cursor == '(') {
|
||||
int closed = 0;
|
||||
++cursor;
|
||||
while (cursor < end) { if (*cursor++ == ')') { closed = 1; break; } }
|
||||
if (!closed) return SIM_INTERP_ERR_SYNTAX;
|
||||
continue;
|
||||
}
|
||||
letter = (char)toupper((unsigned char)*cursor++);
|
||||
if (letter == '%') continue;
|
||||
if (letter < 'A' || letter > 'Z') return SIM_INTERP_ERR_SYNTAX;
|
||||
result = parse_number(&cursor, end, &value);
|
||||
if (result != SIM_INTERP_OK) return result;
|
||||
switch (letter) {
|
||||
case 'G': result = apply_g(&next, value, groups); break;
|
||||
case 'M': result = apply_m(&next, value, groups); break;
|
||||
case 'N': if (seen_line++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0 || value > 4294967295.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else next.line = (uint32_t)value; break;
|
||||
case 'F': if (seen_feed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else feed_value = value; break;
|
||||
case 'S': if (seen_speed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else speed_value = value; break;
|
||||
default:
|
||||
index = axis_index(letter);
|
||||
if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE;
|
||||
else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE;
|
||||
if (result == SIM_INTERP_OK) axis_values[index] = value;
|
||||
break;
|
||||
}
|
||||
if (result != SIM_INTERP_OK) return result;
|
||||
}
|
||||
/* Modal words apply to the whole block, independent of their textual order. */
|
||||
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0);
|
||||
if (seen_speed) next.speed = speed_value;
|
||||
{
|
||||
size_t axis;
|
||||
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) {
|
||||
double value = axis_values[axis];
|
||||
double *destination = axis_pointer(&next, (int)axis);
|
||||
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) value *= 25.4;
|
||||
if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += value;
|
||||
else *destination = value;
|
||||
}
|
||||
}
|
||||
if (!finite_state(&next)) return SIM_INTERP_ERR_NONFINITE;
|
||||
next.line = interpreter->state.line + 1;
|
||||
next.status = SIM_INTERP_OK;
|
||||
interpreter->state = next;
|
||||
interpreter->block_hash = fnv1a64((const unsigned char *)block, length);
|
||||
result = make_canonical(interpreter);
|
||||
if (result != SIM_INTERP_OK) { interpreter->state.status = result; return result; }
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) {
|
||||
if (interpreter == 0 || state == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
*state = interpreter->state;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity) {
|
||||
if (interpreter == 0) return 0;
|
||||
if (output == 0 || capacity == 0) return interpreter->canonical_length + 1;
|
||||
if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1;
|
||||
memcpy(output, interpreter->canonical, interpreter->canonical_length + 1);
|
||||
return interpreter->canonical_length + 1;
|
||||
}
|
||||
|
||||
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) {
|
||||
if (interpreter == 0) return 0;
|
||||
return state_hash(&interpreter->state);
|
||||
}
|
||||
|
||||
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) {
|
||||
return interpreter == 0 ? 0 : interpreter->block_hash;
|
||||
}
|
||||
1181
wasm-core/src/sim_kinematics.c
Normal file
1181
wasm-core/src/sim_kinematics.c
Normal file
File diff suppressed because it is too large
Load Diff
154
wasm-core/src/sim_motion_queue.c
Normal file
154
wasm-core/src/sim_motion_queue.c
Normal file
@@ -0,0 +1,154 @@
|
||||
#include "sim_motion_queue.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
|
||||
uint64_t hash = UINT64_C(1469598103934665603);
|
||||
size_t index;
|
||||
for (index = 0; index < length; ++index) { hash ^= bytes[index]; hash *= UINT64_C(1099511628211); }
|
||||
return hash;
|
||||
}
|
||||
|
||||
static int valid_config(double cycle_time, double velocity_limit) {
|
||||
return isfinite(cycle_time) && isfinite(velocity_limit) && cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9;
|
||||
}
|
||||
|
||||
static double segment_length(const sim_posemath_segment *segment) {
|
||||
const double dx = segment->end.tran.x - segment->start.tran.x;
|
||||
const double dy = segment->end.tran.y - segment->start.tran.y;
|
||||
const double dz = segment->end.tran.z - segment->start.tran.z;
|
||||
double value = dx * dx + dy * dy + dz * dz;
|
||||
return isfinite(value) && value > 0.0 ? sqrt(value) : 0.0;
|
||||
}
|
||||
|
||||
static int poses_match(const sim_posemath_pose *left, const sim_posemath_pose *right) {
|
||||
const double *a = &left->tran.x;
|
||||
const double *b = &right->tran.x;
|
||||
size_t index;
|
||||
for (index = 0; index < 9; ++index) if (fabs(a[index] - b[index]) > 1e-12) return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void zero_state(sim_motion_state *state) {
|
||||
memset(state, 0, sizeof(*state));
|
||||
state->status = SIM_MOTION_OK;
|
||||
}
|
||||
|
||||
int sim_motion_reset(sim_motion_queue *queue) {
|
||||
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
|
||||
queue->head = 0; queue->count = 0; queue->next_id = 1; queue->tick = 0;
|
||||
zero_state(&queue->state);
|
||||
return SIM_MOTION_OK;
|
||||
}
|
||||
|
||||
int sim_motion_init(sim_motion_queue *queue, double cycle_time, double velocity_limit) {
|
||||
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
|
||||
if (!valid_config(cycle_time, velocity_limit)) return SIM_MOTION_ERR_RANGE;
|
||||
queue->cycle_time = cycle_time; queue->velocity_limit = velocity_limit;
|
||||
return sim_motion_reset(queue);
|
||||
}
|
||||
|
||||
int sim_motion_enqueue(sim_motion_queue *queue, const sim_motion_command *command, uint32_t *id_out) {
|
||||
uint32_t index;
|
||||
int result;
|
||||
if (queue == 0 || command == 0) return SIM_MOTION_ERR_ARGUMENT;
|
||||
result = sim_posemath_segment_validate(&command->segment);
|
||||
if (result == SIM_POSEMATH_ERR_MOTION_TYPE) return SIM_MOTION_ERR_MOTION_TYPE;
|
||||
if (result != SIM_POSEMATH_OK) return result == SIM_POSEMATH_ERR_NONFINITE ? SIM_MOTION_ERR_NONFINITE : SIM_MOTION_ERR_GEOMETRY;
|
||||
if (queue->count >= SIM_MOTION_QUEUE_CAPACITY) return SIM_MOTION_ERR_CAPACITY;
|
||||
if (command->segment.velocity > queue->velocity_limit) return SIM_MOTION_ERR_RANGE;
|
||||
if (queue->count > 0) {
|
||||
index = (queue->head + queue->count - 1) % SIM_MOTION_QUEUE_CAPACITY;
|
||||
if (!poses_match(&queue->commands[index].segment.end, &command->segment.start)) return SIM_MOTION_ERR_STATE;
|
||||
} else if (queue->state.tick > 0 && !poses_match(&queue->state.position, &command->segment.start)) {
|
||||
return SIM_MOTION_ERR_STATE;
|
||||
}
|
||||
index = (queue->head + queue->count) % SIM_MOTION_QUEUE_CAPACITY;
|
||||
queue->commands[index] = *command;
|
||||
queue->commands[index].id = queue->next_id++;
|
||||
if (id_out != 0) *id_out = queue->commands[index].id;
|
||||
queue->count++;
|
||||
return SIM_MOTION_OK;
|
||||
}
|
||||
|
||||
int sim_motion_step(sim_motion_queue *queue) {
|
||||
sim_motion_command *command;
|
||||
double length;
|
||||
double delta;
|
||||
int result;
|
||||
if (queue == 0) return SIM_MOTION_ERR_ARGUMENT;
|
||||
if (queue->count == 0) { queue->tick++; queue->state.tick = queue->tick; queue->state.status = SIM_MOTION_ERR_STATE; return SIM_MOTION_ERR_STATE; }
|
||||
command = &queue->commands[queue->head];
|
||||
length = segment_length(&command->segment);
|
||||
if (length <= 0.0) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
|
||||
queue->state.active_id = command->id; queue->state.active_tag = command->tag; queue->state.motion_type = command->segment.motion_type;
|
||||
delta = command->segment.velocity * queue->cycle_time / length;
|
||||
if (!isfinite(delta) || delta < 0.0) { queue->state.status = SIM_MOTION_ERR_NONFINITE; return queue->state.status; }
|
||||
queue->state.progress += delta;
|
||||
if (queue->state.progress >= 1.0) {
|
||||
queue->state.progress = 1.0;
|
||||
result = sim_posemath_segment_point(&command->segment, 1.0, &queue->state.position);
|
||||
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
|
||||
queue->head = (queue->head + 1) % SIM_MOTION_QUEUE_CAPACITY;
|
||||
queue->count--;
|
||||
queue->state.progress = 0.0;
|
||||
queue->state.active_id = 0;
|
||||
queue->state.active_tag = 0;
|
||||
queue->state.motion_type = 0;
|
||||
} else {
|
||||
result = sim_posemath_segment_point(&command->segment, queue->state.progress, &queue->state.position);
|
||||
if (result != SIM_POSEMATH_OK) { queue->state.status = SIM_MOTION_ERR_GEOMETRY; return queue->state.status; }
|
||||
}
|
||||
queue->state.velocity = command->segment.velocity;
|
||||
queue->tick++;
|
||||
queue->state.tick = queue->tick;
|
||||
queue->state.queue_depth = queue->count;
|
||||
queue->state.status = SIM_MOTION_OK;
|
||||
return SIM_MOTION_OK;
|
||||
}
|
||||
|
||||
int sim_motion_read_state(const sim_motion_queue *queue, sim_motion_state *state) {
|
||||
if (queue == 0 || state == 0) return SIM_MOTION_ERR_ARGUMENT;
|
||||
*state = queue->state;
|
||||
state->queue_depth = queue->count;
|
||||
return SIM_MOTION_OK;
|
||||
}
|
||||
|
||||
uint32_t sim_motion_queue_depth(const sim_motion_queue *queue) {
|
||||
return queue == 0 ? 0 : queue->count;
|
||||
}
|
||||
|
||||
uint64_t sim_motion_state_hash(const sim_motion_queue *queue) {
|
||||
sim_motion_state state;
|
||||
unsigned char bytes[sizeof(sim_motion_state)];
|
||||
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
|
||||
memset(bytes, 0, sizeof(bytes));
|
||||
memcpy(bytes, &state.position, sizeof(state.position));
|
||||
memcpy(bytes + sizeof(state.position), &state.velocity, sizeof(double));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double), &state.progress, sizeof(double));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2, &state.tick, sizeof(uint32_t));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t), &state.queue_depth, sizeof(uint32_t));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 2, &state.active_id, sizeof(uint32_t));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 3, &state.active_tag, sizeof(uint32_t));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4, &state.motion_type, sizeof(int));
|
||||
memcpy(bytes + sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int), &state.status, sizeof(int));
|
||||
return fnv1a64(bytes, sizeof(state.position) + sizeof(double) * 2 + sizeof(uint32_t) * 4 + sizeof(int) * 2);
|
||||
}
|
||||
|
||||
uint32_t sim_motion_canonical(const sim_motion_queue *queue, char *output, uint32_t capacity) {
|
||||
sim_motion_state state;
|
||||
char discard[1];
|
||||
char *destination = output == 0 ? discard : output;
|
||||
uint32_t destination_capacity = output == 0 ? 1 : capacity;
|
||||
int written;
|
||||
if (sim_motion_read_state(queue, &state) != SIM_MOTION_OK) return 0;
|
||||
written = snprintf(destination, destination_capacity,
|
||||
"{\"active_id\":%u,\"active_tag\":%u,\"motion_type\":%d,\"position\":{\"x\":%.17g,\"y\":%.17g,\"z\":%.17g},\"progress\":%.17g,\"queue_depth\":%u,\"status\":%d,\"tick\":%u,\"velocity\":%.17g}",
|
||||
state.active_id, state.active_tag, state.motion_type, state.position.tran.x, state.position.tran.y, state.position.tran.z,
|
||||
state.progress, state.queue_depth, state.status, state.tick, state.velocity);
|
||||
if (written < 0) return 0;
|
||||
if (output == 0 || capacity <= (uint32_t)written) return (uint32_t)written + 1;
|
||||
return (uint32_t)written + 1;
|
||||
}
|
||||
@@ -3,11 +3,25 @@
|
||||
#include "posemath.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
static int finite_cart(const sim_posemath_cartesian *value) {
|
||||
return value != 0 && isfinite(value->x) && isfinite(value->y) && isfinite(value->z);
|
||||
}
|
||||
|
||||
static int finite_pose(const sim_posemath_pose *value) {
|
||||
return value != 0 && finite_cart(&value->tran) && isfinite(value->a) &&
|
||||
isfinite(value->b) && isfinite(value->c) && isfinite(value->u) &&
|
||||
isfinite(value->v) && isfinite(value->w);
|
||||
}
|
||||
|
||||
static int finite_segment(const sim_posemath_segment *value) {
|
||||
return value != 0 && finite_pose(&value->start) && finite_pose(&value->end) &&
|
||||
finite_cart(&value->center) && finite_cart(&value->normal) &&
|
||||
isfinite(value->velocity) && isfinite(value->acceleration);
|
||||
}
|
||||
|
||||
static PmCartesian to_pm(const sim_posemath_cartesian *value) {
|
||||
PmCartesian result = {value->x, value->y, value->z};
|
||||
return result;
|
||||
@@ -27,7 +41,8 @@ int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_car
|
||||
PmCartesian source;
|
||||
PmCartesian result;
|
||||
double magnitude_squared = 0.0;
|
||||
if (!finite_cart(input) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (input == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!finite_cart(input)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
source = to_pm(input);
|
||||
if (pmCartMagSq(&source, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
@@ -39,7 +54,8 @@ int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_car
|
||||
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output) {
|
||||
PmCartesian lhs;
|
||||
PmCartesian rhs;
|
||||
if (!finite_cart(left) || !finite_cart(right) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (left == 0 || right == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!finite_cart(left) || !finite_cart(right)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
lhs = to_pm(left);
|
||||
rhs = to_pm(right);
|
||||
if (pmCartCartDot(&lhs, &rhs, output) != 0 || !isfinite(*output)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
@@ -52,11 +68,206 @@ int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posem
|
||||
PmCartesian finish;
|
||||
PmCartesian result;
|
||||
PmCartesian delta;
|
||||
if (!finite_cart(start) || !finite_cart(end) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!isfinite(fraction) || fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
|
||||
double magnitude_squared = 0.0;
|
||||
if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!finite_cart(start) || !finite_cart(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
|
||||
begin = to_pm(start);
|
||||
finish = to_pm(end);
|
||||
if (pmCartCartSub(&finish, &begin, &delta) != 0 || pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (pmCartCartSub(&finish, &begin, &delta) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (pmCartMagSq(&delta, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result)) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (!isfinite(result.x) || !isfinite(result.y) || !isfinite(result.z)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
from_pm(&result, output);
|
||||
return SIM_POSEMATH_OK;
|
||||
}
|
||||
|
||||
static int finite_pose_value(const sim_posemath_pose *value) {
|
||||
return finite_pose(value);
|
||||
}
|
||||
|
||||
int sim_posemath_pose_is_finite(const sim_posemath_pose *value) {
|
||||
return finite_pose_value(value);
|
||||
}
|
||||
|
||||
int sim_posemath_pose_line_point(const sim_posemath_pose *start, const sim_posemath_pose *end,
|
||||
double fraction, sim_posemath_pose *output) {
|
||||
if (start == 0 || end == 0 || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!finite_pose_value(start) || !finite_pose_value(end) || !isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
|
||||
output->tran.x = start->tran.x + (end->tran.x - start->tran.x) * fraction;
|
||||
output->tran.y = start->tran.y + (end->tran.y - start->tran.y) * fraction;
|
||||
output->tran.z = start->tran.z + (end->tran.z - start->tran.z) * fraction;
|
||||
output->a = start->a + (end->a - start->a) * fraction;
|
||||
output->b = start->b + (end->b - start->b) * fraction;
|
||||
output->c = start->c + (end->c - start->c) * fraction;
|
||||
output->u = start->u + (end->u - start->u) * fraction;
|
||||
output->v = start->v + (end->v - start->v) * fraction;
|
||||
output->w = start->w + (end->w - start->w) * fraction;
|
||||
return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
|
||||
}
|
||||
|
||||
int sim_posemath_segment_validate(const sim_posemath_segment *segment) {
|
||||
double length_sq;
|
||||
double normal_sq;
|
||||
if (segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!finite_segment(segment)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (segment->motion_type < SIM_POSEMATH_LINEAR || segment->motion_type > SIM_POSEMATH_SPHERICAL) return SIM_POSEMATH_ERR_MOTION_TYPE;
|
||||
if (segment->velocity < 0.0 || segment->acceleration < 0.0 || segment->velocity > 1e9 || segment->acceleration > 1e12) return SIM_POSEMATH_ERR_RANGE;
|
||||
length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
|
||||
(segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
|
||||
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
|
||||
if (!isfinite(length_sq) || length_sq <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (segment->motion_type == SIM_POSEMATH_CIRCULAR) {
|
||||
normal_sq = segment->normal.x * segment->normal.x + segment->normal.y * segment->normal.y + segment->normal.z * segment->normal.z;
|
||||
if (!isfinite(normal_sq) || normal_sq <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (segment->turn < -1000000 || segment->turn > 1000000) return SIM_POSEMATH_ERR_RANGE;
|
||||
}
|
||||
return SIM_POSEMATH_OK;
|
||||
}
|
||||
|
||||
int sim_posemath_segment_point(const sim_posemath_segment *segment, double fraction,
|
||||
sim_posemath_pose *output) {
|
||||
int result = sim_posemath_segment_validate(segment);
|
||||
if (result != SIM_POSEMATH_OK) return result;
|
||||
if (output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!isfinite(fraction)) return SIM_POSEMATH_ERR_NONFINITE;
|
||||
if (fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
|
||||
if (segment->motion_type != SIM_POSEMATH_CIRCULAR)
|
||||
return sim_posemath_pose_line_point(&segment->start, &segment->end, fraction, output);
|
||||
{
|
||||
PmCartesian start = to_pm(&segment->start.tran);
|
||||
PmCartesian end = to_pm(&segment->end.tran);
|
||||
PmCartesian center = to_pm(&segment->center);
|
||||
PmCartesian normal = to_pm(&segment->normal);
|
||||
PmCircle circle;
|
||||
PmCartesian point;
|
||||
if (pmCircleInit(&circle, &start, &end, ¢er, &normal, segment->turn) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
if (pmCirclePoint(&circle, circle.angle * fraction, &point) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
||||
output->tran.x = point.x; output->tran.y = point.y; output->tran.z = point.z;
|
||||
output->a = segment->start.a + (segment->end.a - segment->start.a) * fraction;
|
||||
output->b = segment->start.b + (segment->end.b - segment->start.b) * fraction;
|
||||
output->c = segment->start.c + (segment->end.c - segment->start.c) * fraction;
|
||||
output->u = segment->start.u + (segment->end.u - segment->start.u) * fraction;
|
||||
output->v = segment->start.v + (segment->end.v - segment->start.v) * fraction;
|
||||
output->w = segment->start.w + (segment->end.w - segment->start.w) * fraction;
|
||||
return finite_pose_value(output) ? SIM_POSEMATH_OK : SIM_POSEMATH_ERR_NONFINITE;
|
||||
}
|
||||
}
|
||||
|
||||
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
|
||||
uint64_t hash = UINT64_C(1469598103934665603);
|
||||
size_t index;
|
||||
for (index = 0; index < length; ++index) {
|
||||
hash ^= (uint64_t)bytes[index];
|
||||
hash *= UINT64_C(1099511628211);
|
||||
}
|
||||
return hash;
|
||||
}
|
||||
|
||||
uint64_t sim_posemath_hash_state(const sim_tp_state *state) {
|
||||
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) * 3 + sizeof(int) * 2];
|
||||
size_t offset = 0;
|
||||
const double *doubles;
|
||||
const uint32_t *uints;
|
||||
const int *ints;
|
||||
size_t index;
|
||||
if (state == 0) return 0;
|
||||
doubles = (const double *)&state->position;
|
||||
for (index = 0; index < 11; ++index) {
|
||||
memcpy(bytes + offset, doubles + index, sizeof(double));
|
||||
offset += sizeof(double);
|
||||
}
|
||||
uints = &state->tick;
|
||||
for (index = 0; index < 3; ++index) {
|
||||
memcpy(bytes + offset, uints + index, sizeof(uint32_t));
|
||||
offset += sizeof(uint32_t);
|
||||
}
|
||||
ints = &state->motion_type;
|
||||
for (index = 0; index < 2; ++index) {
|
||||
memcpy(bytes + offset, ints + index, sizeof(int));
|
||||
offset += sizeof(int);
|
||||
}
|
||||
return fnv1a64(bytes, offset);
|
||||
}
|
||||
|
||||
static int valid_tp_config(double cycle_time, double velocity_limit, double acceleration_limit) {
|
||||
return isfinite(cycle_time) && isfinite(velocity_limit) && isfinite(acceleration_limit) &&
|
||||
cycle_time > 0.0 && cycle_time <= 60.0 && velocity_limit >= 0.0 && velocity_limit <= 1e9 &&
|
||||
acceleration_limit >= 0.0 && acceleration_limit <= 1e12;
|
||||
}
|
||||
|
||||
int sim_tp_reset(sim_tp *tp) {
|
||||
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
tp->head = 0; tp->count = 0; tp->next_id = 1; tp->active_id = 0; tp->tick = 0;
|
||||
tp->progress = 0.0; tp->current_velocity = 0.0; tp->motion_type = 0; tp->status = SIM_POSEMATH_OK;
|
||||
tp->position.tran.x = 0.0; tp->position.tran.y = 0.0; tp->position.tran.z = 0.0;
|
||||
tp->position.a = tp->position.b = tp->position.c = tp->position.u = tp->position.v = tp->position.w = 0.0;
|
||||
return SIM_POSEMATH_OK;
|
||||
}
|
||||
|
||||
int sim_tp_init(sim_tp *tp, double cycle_time, double velocity_limit, double acceleration_limit) {
|
||||
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (!valid_tp_config(cycle_time, velocity_limit, acceleration_limit)) return SIM_POSEMATH_ERR_RANGE;
|
||||
tp->cycle_time = cycle_time; tp->velocity_limit = velocity_limit; tp->acceleration_limit = acceleration_limit;
|
||||
return sim_tp_reset(tp);
|
||||
}
|
||||
|
||||
int sim_tp_enqueue(sim_tp *tp, const sim_posemath_segment *segment, uint32_t *id_out) {
|
||||
uint32_t index;
|
||||
int result;
|
||||
if (tp == 0 || segment == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
result = sim_posemath_segment_validate(segment);
|
||||
if (result != SIM_POSEMATH_OK) return result;
|
||||
if (tp->count >= SIM_TP_QUEUE_CAPACITY) return SIM_POSEMATH_ERR_CAPACITY;
|
||||
if (segment->velocity > tp->velocity_limit || segment->acceleration > tp->acceleration_limit) return SIM_POSEMATH_ERR_RANGE;
|
||||
index = (tp->head + tp->count) % SIM_TP_QUEUE_CAPACITY;
|
||||
tp->queue[index] = *segment; tp->ids[index] = tp->next_id++;
|
||||
if (id_out != 0) *id_out = tp->ids[index];
|
||||
tp->count++;
|
||||
return SIM_POSEMATH_OK;
|
||||
}
|
||||
|
||||
int sim_tp_step(sim_tp *tp) {
|
||||
sim_posemath_segment *segment;
|
||||
double length_sq;
|
||||
double step;
|
||||
int result;
|
||||
if (tp == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
if (tp->count == 0) { tp->tick++; tp->status = SIM_POSEMATH_ERR_STATE; return SIM_POSEMATH_ERR_STATE; }
|
||||
segment = &tp->queue[tp->head];
|
||||
result = sim_posemath_segment_validate(segment);
|
||||
if (result != SIM_POSEMATH_OK) { tp->status = result; return result; }
|
||||
if (tp->active_id == 0) { tp->active_id = tp->ids[tp->head]; tp->motion_type = segment->motion_type; }
|
||||
length_sq = (segment->end.tran.x - segment->start.tran.x) * (segment->end.tran.x - segment->start.tran.x) +
|
||||
(segment->end.tran.y - segment->start.tran.y) * (segment->end.tran.y - segment->start.tran.y) +
|
||||
(segment->end.tran.z - segment->start.tran.z) * (segment->end.tran.z - segment->start.tran.z);
|
||||
step = segment->velocity * tp->cycle_time / sqrt(length_sq);
|
||||
if (!isfinite(step) || step < 0.0) { tp->status = SIM_POSEMATH_ERR_NONFINITE; return tp->status; }
|
||||
if (step < 1e-15) step = 1e-15;
|
||||
tp->progress += step;
|
||||
if (tp->progress >= 1.0) {
|
||||
tp->progress = 1.0;
|
||||
result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
|
||||
tp->head = (tp->head + 1) % SIM_TP_QUEUE_CAPACITY; tp->count--; tp->active_id = 0; tp->motion_type = 0; tp->progress = 0.0;
|
||||
} else {
|
||||
result = sim_posemath_segment_point(segment, tp->progress, &tp->position);
|
||||
}
|
||||
tp->current_velocity = segment->velocity; tp->tick++; tp->status = result;
|
||||
return result;
|
||||
}
|
||||
|
||||
int sim_tp_read_state(const sim_tp *tp, sim_tp_state *state) {
|
||||
if (tp == 0 || state == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
||||
state->position = tp->position; state->velocity = tp->current_velocity; state->progress = tp->progress;
|
||||
state->tick = tp->tick; state->queue_depth = tp->count; state->active_id = tp->active_id;
|
||||
state->motion_type = tp->motion_type; state->status = tp->status;
|
||||
return SIM_POSEMATH_OK;
|
||||
}
|
||||
|
||||
uint64_t sim_tp_state_hash(const sim_tp *tp) {
|
||||
sim_tp_state state;
|
||||
if (sim_tp_read_state(tp, &state) != SIM_POSEMATH_OK) return 0;
|
||||
return sim_posemath_hash_state(&state);
|
||||
}
|
||||
|
||||
@@ -1,8 +1,16 @@
|
||||
#include "sim_hal.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
int main(void) {
|
||||
static float get_float(const char *name) {
|
||||
sim_hal_value value = {0};
|
||||
assert(sim_hal_get_pin(name, &value) == SIM_HAL_OK);
|
||||
assert(value.type == SIM_HAL_FLOAT);
|
||||
return value.value.real;
|
||||
}
|
||||
|
||||
int sim_hal_test_run(void) {
|
||||
sim_hal_value bit = {SIM_HAL_BIT, {.bit = 1}};
|
||||
sim_hal_value wrong = {SIM_HAL_S32, {.s32 = 1}};
|
||||
sim_hal_value out = {0};
|
||||
@@ -25,5 +33,27 @@ int main(void) {
|
||||
assert(sim_hal_step(1) == SIM_HAL_ERR_FUNCTION);
|
||||
assert(sim_hal_tick() == 5);
|
||||
assert(sim_hal_register_thread("invalid", 0) == SIM_HAL_ERR_RANGE);
|
||||
|
||||
sim_hal_reset();
|
||||
assert(sim_hal_register_thread("servo-thread", 1) == SIM_HAL_OK);
|
||||
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_OK);
|
||||
assert(sim_hal_set_pin("corexy_by_hal.j0-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 6.0f}}) == SIM_HAL_OK);
|
||||
assert(sim_hal_set_pin("corexy_by_hal.j1-motor-pos-cmd", (sim_hal_value){SIM_HAL_FLOAT, {.real = 2.0f}}) == SIM_HAL_OK);
|
||||
assert(sim_hal_set_pin("corexy_by_hal.alpha-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 4.0f}}) == SIM_HAL_OK);
|
||||
assert(sim_hal_set_pin("corexy_by_hal.beta-fb", (sim_hal_value){SIM_HAL_FLOAT, {.real = 1.0f}}) == SIM_HAL_OK);
|
||||
assert(sim_hal_step(1) == SIM_HAL_OK);
|
||||
assert(get_float("corexy_by_hal.alpha-cmd") == 4.0f);
|
||||
assert(get_float("corexy_by_hal.beta-cmd") == 2.0f);
|
||||
assert(get_float("corexy_by_hal.j0-motor-pos-fb") == 5.0f);
|
||||
assert(get_float("corexy_by_hal.j1-motor-pos-fb") == 3.0f);
|
||||
assert(sim_hal_function_calls("corexy_by_hal") == 1);
|
||||
assert(sim_hal_corexy_transform("missing") == SIM_HAL_ERR_NOT_FOUND);
|
||||
assert(sim_hal_register_corexy_by_hal("corexy_by_hal", "servo-thread") == SIM_HAL_ERR_DUPLICATE);
|
||||
printf("{\"schema\":\"hal-corexy-sample.v1\",\"command\":{\"j0\":%.3f,\"j1\":%.3f,\"alpha\":%.3f,\"beta\":%.3f},\"feedback\":{\"alpha\":%.3f,\"beta\":%.3f,\"j0\":%.3f,\"j1\":%.3f},\"tick\":%u,\"function_calls\":%u}\n",
|
||||
(double)get_float("corexy_by_hal.j0-motor-pos-cmd"), (double)get_float("corexy_by_hal.j1-motor-pos-cmd"),
|
||||
(double)get_float("corexy_by_hal.alpha-cmd"), (double)get_float("corexy_by_hal.beta-cmd"),
|
||||
(double)get_float("corexy_by_hal.alpha-fb"), (double)get_float("corexy_by_hal.beta-fb"),
|
||||
(double)get_float("corexy_by_hal.j0-motor-pos-fb"), (double)get_float("corexy_by_hal.j1-motor-pos-fb"),
|
||||
sim_hal_tick(), sim_hal_function_calls("corexy_by_hal"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
5
wasm-core/tests/sim_hal_test_main.c
Normal file
5
wasm-core/tests/sim_hal_test_main.c
Normal file
@@ -0,0 +1,5 @@
|
||||
int sim_hal_test_run(void);
|
||||
|
||||
int main(void) {
|
||||
return sim_hal_test_run();
|
||||
}
|
||||
36
wasm-core/tests/sim_interpreter_test.c
Normal file
36
wasm-core/tests/sim_interpreter_test.c
Normal file
@@ -0,0 +1,36 @@
|
||||
#include "sim_interpreter.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
int main(void) {
|
||||
sim_interpreter interpreter = {0};
|
||||
sim_interpreter_state state = {0};
|
||||
sim_interpreter_state second = {0};
|
||||
char canonical[SIM_INTERPRETER_CANONICAL_CAPACITY] = {0};
|
||||
uint32_t required;
|
||||
|
||||
assert(sim_interpreter_init(&interpreter) == SIM_INTERP_OK);
|
||||
assert(sim_interpreter_execute(&interpreter, "G21 G90 G1 X10 Y2 F120", (uint32_t)(sizeof("G21 G90 G1 X10 Y2 F120") - 1)) == SIM_INTERP_OK);
|
||||
assert(sim_interpreter_read_state(&interpreter, &state) == SIM_INTERP_OK);
|
||||
assert(state.x == 10.0 && state.y == 2.0 && state.feed == 120.0);
|
||||
required = sim_interpreter_canonical(&interpreter, canonical, sizeof(canonical));
|
||||
assert(required > 1 && canonical[required - 1] == '\0');
|
||||
assert(sim_interpreter_state_hash(&interpreter) != 0);
|
||||
assert(sim_interpreter_execute(&interpreter, "G91 X1 (increment)", (uint32_t)(sizeof("G91 X1 (increment)") - 1)) == SIM_INTERP_OK);
|
||||
assert(sim_interpreter_read_state(&interpreter, &second) == SIM_INTERP_OK);
|
||||
assert(second.x == 11.0 && second.line == state.line + 1);
|
||||
assert(sim_interpreter_execute(&interpreter, "G20 X1", 6) == SIM_INTERP_OK);
|
||||
assert(fabs(interpreter.state.x - 36.4) < 1e-12);
|
||||
assert(sim_interpreter_execute(&interpreter, "X1 G90 G21", (uint32_t)(sizeof("X1 G90 G21") - 1)) == SIM_INTERP_OK);
|
||||
assert(interpreter.state.x == 1.0);
|
||||
assert(sim_interpreter_execute(&interpreter, "G1 G0 X1", 8) == SIM_INTERP_ERR_STATE);
|
||||
assert(sim_interpreter_execute(&interpreter, "G999", 4) == SIM_INTERP_ERR_UNKNOWN_CODE);
|
||||
assert(sim_interpreter_execute(&interpreter, "Xnan", 4) == SIM_INTERP_ERR_SYNTAX);
|
||||
assert(sim_interpreter_execute(&interpreter, "G1 X1 X2", 8) == SIM_INTERP_ERR_STATE);
|
||||
assert(sim_interpreter_execute(&interpreter, "M2", 2) == SIM_INTERP_OK);
|
||||
assert(interpreter.state.program_end == 1);
|
||||
assert(sim_interpreter_execute(&interpreter, "G1 X0", (uint32_t)(sizeof("G1 X0") - 1)) == SIM_INTERP_ERR_STATE);
|
||||
return 0;
|
||||
}
|
||||
585
wasm-core/tests/sim_kinematics_test.c
Normal file
585
wasm-core/tests/sim_kinematics_test.c
Normal file
@@ -0,0 +1,585 @@
|
||||
#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);
|
||||
config.pivot_length = 250.0;
|
||||
config.limit_enabled[0] = 1; config.joint_min[0] = -1000; config.joint_max[0] = 1000;
|
||||
return config;
|
||||
}
|
||||
|
||||
static void round_trip(sim_kinematics *kinematics, const double *input, uint32_t count) {
|
||||
double joints[SIM_KIN_MAX_JOINTS] = {0};
|
||||
sim_posemath_pose pose = {0};
|
||||
sim_posemath_pose repeat = {0};
|
||||
assert(sim_kinematics_forward(kinematics, input, count, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(kinematics, &pose, joints, count) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(kinematics, joints, count, &repeat) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - repeat.tran.x) < 1e-9);
|
||||
assert(fabs(pose.tran.y - repeat.tran.y) < 1e-9);
|
||||
assert(fabs(pose.tran.z - repeat.tran.z) < 1e-9);
|
||||
assert(sim_kinematics_state_hash(kinematics, joints, count, &repeat) == sim_kinematics_state_hash(kinematics, joints, count, &repeat));
|
||||
}
|
||||
|
||||
static double canonical(double value) {
|
||||
return fabs(value) < 5e-10 ? 0.0 : value;
|
||||
}
|
||||
|
||||
static void emit_coupled_samples(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics_config config;
|
||||
sim_posemath_pose pose = {0};
|
||||
double output[SIM_KIN_MAX_JOINTS] = {0};
|
||||
double corexy_pose[3];
|
||||
double corexy_joints[3];
|
||||
double rose_pose[3];
|
||||
double rose_joints[3];
|
||||
double max_pose[3];
|
||||
double max_joints[6];
|
||||
double rotate_pose[3];
|
||||
double rotate_joints[6];
|
||||
double fiveaxis_pose[3];
|
||||
double fiveaxis_joints[7];
|
||||
double xyzac_pose[3];
|
||||
double xyzac_joints[5];
|
||||
double xyzbc_pose[3];
|
||||
double xyzbc_joints[5];
|
||||
double scara_pose[3];
|
||||
double scara_joints[6];
|
||||
double puma_pose[3];
|
||||
double puma_joints[6];
|
||||
double scorbot_pose[3];
|
||||
double scorbot_joints[5];
|
||||
double genser_pose[3];
|
||||
double genser_joints[6];
|
||||
double ldelta_pose[3];
|
||||
double ldelta_joints[3];
|
||||
double rdelta_pose[3];
|
||||
double rdelta_joints[3];
|
||||
double tripod_pose[3];
|
||||
double tripod_joints[3];
|
||||
double genhex_pose[6];
|
||||
double genhex_joints[6];
|
||||
double penta_pose[5];
|
||||
double penta_joints[5];
|
||||
uint32_t rose_branch;
|
||||
uint64_t corexy_hash;
|
||||
uint64_t rose_hash;
|
||||
uint64_t max_hash;
|
||||
uint64_t rotate_hash;
|
||||
uint64_t fiveaxis_hash;
|
||||
uint64_t xyzac_hash;
|
||||
uint64_t xyzbc_hash;
|
||||
uint64_t scara_hash;
|
||||
uint64_t puma_hash;
|
||||
uint64_t scorbot_hash;
|
||||
uint64_t genser_hash;
|
||||
uint64_t ldelta_hash;
|
||||
uint64_t rdelta_hash;
|
||||
uint64_t tripod_hash;
|
||||
uint64_t genhex_hash;
|
||||
uint64_t penta_hash;
|
||||
uint32_t ldelta_branch;
|
||||
uint32_t ldelta_alt_branch;
|
||||
uint32_t rdelta_branch;
|
||||
uint32_t rdelta_alt_branch;
|
||||
int ldelta_no_solution;
|
||||
int ldelta_limit_error;
|
||||
int rdelta_singular;
|
||||
int ldelta_alt_status;
|
||||
int rdelta_alt_status;
|
||||
int ldelta_geometry_status;
|
||||
int rdelta_geometry_status;
|
||||
int tripod_no_solution;
|
||||
int tripod_singular;
|
||||
int tripod_limit_error;
|
||||
int tripod_geometry_status;
|
||||
uint32_t tripod_branch;
|
||||
uint32_t tripod_alt_branch;
|
||||
int tripod_alt_status;
|
||||
int genhex_nonpositive;
|
||||
int genhex_iter_limit_error;
|
||||
int genhex_max_error_status;
|
||||
int genhex_singular;
|
||||
int genhex_limit_error;
|
||||
int genhex_alt_status;
|
||||
uint32_t genhex_branch;
|
||||
uint32_t genhex_alt_branch;
|
||||
uint32_t genhex_iterations;
|
||||
double genhex_residual;
|
||||
int penta_nonpositive;
|
||||
int penta_iter_limit_error;
|
||||
int penta_max_error_status;
|
||||
int penta_singular;
|
||||
int penta_limit_error;
|
||||
int penta_alt_status;
|
||||
uint32_t penta_branch;
|
||||
uint32_t penta_alt_branch;
|
||||
uint32_t penta_iterations;
|
||||
double penta_residual;
|
||||
sim_posemath_pose tripod_alt_pose = {0};
|
||||
sim_posemath_pose genhex_alt_pose = {0};
|
||||
sim_posemath_pose penta_alt_pose = {0};
|
||||
uint32_t ldelta_iterations;
|
||||
uint32_t rdelta_iterations;
|
||||
sim_posemath_pose ldelta_alt_pose = {0};
|
||||
sim_posemath_pose rdelta_alt_pose = {0};
|
||||
sim_posemath_pose mapping_pose = {0};
|
||||
sim_kinematics_result_info delta_info = {0};
|
||||
sim_kinematics_result_info genser_info = {0};
|
||||
|
||||
config = config_for(SIM_KIN_COREXY, "XYZ");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){6, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
||||
corexy_pose[0] = canonical(pose.tran.x); corexy_pose[1] = canonical(pose.tran.y); corexy_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
memcpy(corexy_joints, output, sizeof(corexy_joints));
|
||||
corexy_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_ROSE, "XYZ");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){2, 7, 170}, 3, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){2, 7, 190}, 3, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
rose_pose[0] = canonical(pose.tran.x); rose_pose[1] = canonical(pose.tran.y); rose_pose[2] = canonical(pose.tran.z);
|
||||
memcpy(rose_joints, output, sizeof(rose_joints));
|
||||
rose_branch = kinematics.branch_id;
|
||||
rose_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_MAXKINS, "XYZABC");
|
||||
config.pivot_length = 0.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 30, 90}, 6, &pose) == SIM_KIN_OK);
|
||||
max_pose[0] = canonical(pose.tran.x); max_pose[1] = canonical(pose.tran.y); max_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &pose, 6, 0, output, &genser_info) == SIM_KIN_OK);
|
||||
memcpy(max_joints, output, sizeof(max_joints));
|
||||
max_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_ROTATEKINS, "XYZABC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6, &pose) == SIM_KIN_OK);
|
||||
rotate_pose[0] = canonical(pose.tran.x); rotate_pose[1] = canonical(pose.tran.y); rotate_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
||||
memcpy(rotate_joints, output, sizeof(rotate_joints));
|
||||
rotate_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_5AXISKINS, "XYZBCWY");
|
||||
config.pivot_length = 250.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7, &pose) == SIM_KIN_OK);
|
||||
fiveaxis_pose[0] = canonical(pose.tran.x); fiveaxis_pose[1] = canonical(pose.tran.y); fiveaxis_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 7) == SIM_KIN_OK);
|
||||
memcpy(fiveaxis_joints, output, sizeof(fiveaxis_joints));
|
||||
fiveaxis_hash = sim_kinematics_state_hash(&kinematics, output, 7, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
||||
config.x_rot_point = 20.0; config.y_offset = 3.0; config.tool_offset = 100.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){100, 50, 200, 30, 45}, 5, &pose) == SIM_KIN_OK);
|
||||
xyzac_pose[0] = canonical(pose.tran.x); xyzac_pose[1] = canonical(pose.tran.y); xyzac_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
||||
memcpy(xyzac_joints, output, sizeof(xyzac_joints));
|
||||
xyzac_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
||||
config.x_offset = 5.0; config.z_offset = 2.0; config.tool_offset = 50.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){100, 50, 200, 30, 45}, 5, &pose) == SIM_KIN_OK);
|
||||
xyzbc_pose[0] = canonical(pose.tran.x); xyzbc_pose[1] = canonical(pose.tran.y); xyzbc_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
||||
memcpy(xyzbc_joints, output, sizeof(xyzbc_joints));
|
||||
xyzbc_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
||||
config.scara_d1 = 490.0; config.scara_d2 = 340.0; config.scara_d3 = 50.0; config.scara_d4 = 250.0; config.scara_d5 = 50.0; config.scara_d6 = 50.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6, &pose) == SIM_KIN_OK);
|
||||
scara_pose[0] = canonical(pose.tran.x); scara_pose[1] = canonical(pose.tran.y); scara_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
||||
memcpy(scara_joints, output, sizeof(scara_joints)); scara_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_PUMA, "XYZABC");
|
||||
config.puma_a2 = 400.0; config.puma_a3 = 50.0; config.puma_d3 = 100.0; config.puma_d4 = 400.0; config.puma_d6 = 95.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, -30, 40, 25, -15}, 6, &pose) == SIM_KIN_OK);
|
||||
puma_pose[0] = canonical(pose.tran.x); puma_pose[1] = canonical(pose.tran.y); puma_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
||||
memcpy(puma_joints, output, sizeof(puma_joints)); puma_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_SCORBOT, "XYZAB");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){25, 35, -20, 10, -5}, 5, &pose) == SIM_KIN_OK);
|
||||
scorbot_pose[0] = canonical(pose.tran.x); scorbot_pose[1] = canonical(pose.tran.y); scorbot_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
||||
memcpy(scorbot_joints, output, sizeof(scorbot_joints)); scorbot_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_GENSER, "XYZABC");
|
||||
config.dh_joint_count = 6;
|
||||
config.max_iterations = 200;
|
||||
config.convergence_tolerance = 1e-7;
|
||||
config.dh_a[0] = 0.0; config.dh_a[1] = 85.0; config.dh_a[2] = 380.0; config.dh_a[3] = 100.0;
|
||||
config.dh_alpha[0] = 0.0; config.dh_alpha[1] = -1.5707963267948966; config.dh_alpha[2] = 0.0; config.dh_alpha[3] = -1.5707963267948966; config.dh_alpha[4] = 1.5707963267948966; config.dh_alpha[5] = -1.5707963267948966;
|
||||
config.dh_d[0] = 350.0; config.dh_d[1] = 0.0; config.dh_d[2] = 0.0; config.dh_d[3] = 425.0; config.dh_d[4] = 0.0; config.dh_d[5] = 235.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6, &pose) == SIM_KIN_OK);
|
||||
genser_pose[0] = canonical(pose.tran.x); genser_pose[1] = canonical(pose.tran.y); genser_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
||||
memcpy(genser_joints, output, sizeof(genser_joints)); genser_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_LINEAR_DELTA, "XYZ");
|
||||
config.joint_count = 4;
|
||||
config.kinematics_joint_count = 3;
|
||||
config.extra_joint_count = 1;
|
||||
config.delta_radius = 130.25; config.delta_rod_length = 269.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward_ex(&kinematics, (double[]){300, 300, 300}, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
||||
ldelta_branch = delta_info.branch_id; ldelta_iterations = delta_info.iterations;
|
||||
ldelta_pose[0] = canonical(pose.tran.x); ldelta_pose[1] = canonical(pose.tran.y); ldelta_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_forward_ex(&kinematics, (double[]){300, 300, 300}, 3, SIM_KIN_DELTA_ALTERNATE_ROOT, &ldelta_alt_pose, &delta_info) == SIM_KIN_OK);
|
||||
ldelta_alt_status = SIM_KIN_OK; ldelta_alt_branch = delta_info.branch_id;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
memcpy(ldelta_joints, output, sizeof(ldelta_joints)); ldelta_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
||||
ldelta_no_solution = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {1000.0, 0.0, 0.0}}, output, 3);
|
||||
config.limit_enabled[0] = 1; config.joint_min[0] = -10.0; config.joint_max[0] = 10.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
ldelta_limit_error = sim_kinematics_inverse(&kinematics, &pose, output, 3);
|
||||
config.limit_enabled[0] = 0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
config.delta_radius = 0.0; config.delta_rod_length = 269.0;
|
||||
ldelta_geometry_status = sim_kinematics_init(&kinematics, &config);
|
||||
config.delta_radius = 130.25; config.delta_rod_length = 269.0;
|
||||
|
||||
config = config_for(SIM_KIN_ROTARY_DELTA, "XYZ");
|
||||
config.rotary_platform_radius = 10.0; config.rotary_thigh_length = 10.0; config.rotary_shin_length = 14.0; config.rotary_foot_radius = 6.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward_ex(&kinematics, (double[]){10, 10, 10}, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
||||
rdelta_branch = delta_info.branch_id; rdelta_iterations = delta_info.iterations;
|
||||
rdelta_pose[0] = canonical(pose.tran.x); rdelta_pose[1] = canonical(pose.tran.y); rdelta_pose[2] = canonical(pose.tran.z);
|
||||
assert(sim_kinematics_forward_ex(&kinematics, (double[]){10, 10, 10}, 3, SIM_KIN_DELTA_ALTERNATE_ROOT, &rdelta_alt_pose, &delta_info) == SIM_KIN_OK);
|
||||
rdelta_alt_status = SIM_KIN_OK; rdelta_alt_branch = delta_info.branch_id;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
memcpy(rdelta_joints, output, sizeof(rdelta_joints)); rdelta_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
||||
rdelta_singular = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 0.0, 0.0}}, output, 3);
|
||||
config.rotary_platform_radius = 0.0;
|
||||
rdelta_geometry_status = sim_kinematics_init(&kinematics, &config);
|
||||
|
||||
/* Non-default coordinate order must use the same axis-to-joint mapping
|
||||
* in rotary Delta inverse as it does in forward. */
|
||||
config = config_for(SIM_KIN_ROTARY_DELTA, "YZX");
|
||||
config.rotary_platform_radius = 10.0; config.rotary_thigh_length = 10.0; config.rotary_shin_length = 14.0; config.rotary_foot_radius = 6.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30}, 3, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, output, 3, &mapping_pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - mapping_pose.tran.x) < 1e-9);
|
||||
assert(fabs(pose.tran.y - mapping_pose.tran.y) < 1e-9);
|
||||
assert(fabs(pose.tran.z - mapping_pose.tran.z) < 1e-9);
|
||||
|
||||
config = config_for(SIM_KIN_TRIPOD, "XYZ");
|
||||
config.tripod_bx = 10.0; config.tripod_cx = 5.0; config.tripod_cy = 7.071;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 4.0}}, 3, 0, output, &delta_info) == SIM_KIN_OK);
|
||||
memcpy(tripod_joints, output, sizeof(tripod_joints));
|
||||
assert(sim_kinematics_forward_ex(&kinematics, output, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
||||
tripod_branch = delta_info.branch_id;
|
||||
tripod_pose[0] = canonical(pose.tran.x); tripod_pose[1] = canonical(pose.tran.y); tripod_pose[2] = canonical(pose.tran.z);
|
||||
tripod_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
||||
assert(sim_kinematics_forward_ex(&kinematics, output, 3, SIM_KIN_TRIPOD_BELOW_ROOT, &tripod_alt_pose, &delta_info) == SIM_KIN_OK);
|
||||
tripod_alt_status = SIM_KIN_OK; tripod_alt_branch = delta_info.branch_id;
|
||||
tripod_no_solution = sim_kinematics_forward(&kinematics, (double[]){1.0, 1.0, 20.0}, 3, &pose);
|
||||
assert(sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 0.0}}, output, 3) == SIM_KIN_OK);
|
||||
tripod_singular = sim_kinematics_forward(&kinematics, output, 3, &pose);
|
||||
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);
|
||||
tripod_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 4.0}}, output, 3);
|
||||
config.limit_enabled[0] = 0;
|
||||
config.tripod_bx = 0.0;
|
||||
tripod_geometry_status = sim_kinematics_init(&kinematics, &config);
|
||||
config.tripod_bx = 10.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_home(&kinematics, output, 3, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.z - 1.0) < 1e-9);
|
||||
|
||||
config = config_for(SIM_KIN_GENHEX, "XYZABC");
|
||||
config.genhex_initial_pose[2] = 50.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {10.0, -5.0, 60.0}, .a = 5.0, .b = -3.0, .c = 10.0}, 6, 0, output, &genser_info) == SIM_KIN_OK);
|
||||
memcpy(genhex_joints, output, sizeof(genhex_joints));
|
||||
assert(sim_kinematics_forward_ex(&kinematics, output, 6, 0, &pose, &genser_info) == SIM_KIN_OK);
|
||||
genhex_pose[0] = canonical(pose.tran.x); genhex_pose[1] = canonical(pose.tran.y); genhex_pose[2] = canonical(pose.tran.z);
|
||||
genhex_pose[3] = canonical(pose.a); genhex_pose[4] = canonical(pose.b); genhex_pose[5] = canonical(pose.c);
|
||||
genhex_iterations = genser_info.iterations; genhex_residual = genser_info.residual; genhex_branch = genser_info.branch_id;
|
||||
genhex_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
||||
genhex_alt_status = sim_kinematics_forward_ex(&kinematics, output, 6, SIM_KIN_GENHEX_ALTERNATE_SEED, &genhex_alt_pose, &genser_info);
|
||||
genhex_alt_branch = genser_info.branch_id;
|
||||
genhex_nonpositive = sim_kinematics_forward(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0, 1.0}, 6, &pose);
|
||||
config.genhex_iteration_limit = 1;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
genhex_iter_limit_error = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
||||
config.genhex_iteration_limit = 120; config.genhex_max_error = 0.000001;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
genhex_max_error_status = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
||||
config.genhex_max_error = 500.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);
|
||||
genhex_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {10.0, -5.0, 60.0}, .a = 5.0, .b = -3.0, .c = 10.0}, output, 6);
|
||||
config.limit_enabled[0] = 0;
|
||||
config.genhex_base[0][0] = 1.0; config.genhex_base[0][1] = 0.0; config.genhex_base[0][2] = 0.0;
|
||||
config.genhex_platform[0][0] = 0.0; config.genhex_platform[0][1] = 1.0; config.genhex_platform[0][2] = 0.0;
|
||||
for (int duplicate = 1; duplicate < 6; ++duplicate) {
|
||||
memcpy(config.genhex_base[duplicate], config.genhex_base[0], sizeof(config.genhex_base[0]));
|
||||
memcpy(config.genhex_platform[duplicate], config.genhex_platform[0], sizeof(config.genhex_platform[0]));
|
||||
}
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
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);
|
||||
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_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);
|
||||
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_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);
|
||||
|
||||
printf("{\"schema\":\"kinematics-coupled-samples.v1\","
|
||||
"\"corexy\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"rose\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"branch\":%u,\"state_hash\":%llu},"
|
||||
"\"maxkins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"rotatekins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"fiveaxiskins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"xyzac_trt\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"xyzbc_trt\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"scara\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"puma\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"scorbot\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"genser\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
||||
"\"lineardelta\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"rotarydelta\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"tripod\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
"\"genhex\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
||||
"\"pentapod\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
||||
"\"delta_edges\":{\"lineardelta\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"no_solution\":%d,\"limit_error\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
||||
"\"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",
|
||||
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,
|
||||
rotate_pose[0], rotate_pose[1], rotate_pose[2], rotate_joints[0], rotate_joints[1], rotate_joints[2], rotate_joints[3], rotate_joints[4], rotate_joints[5], (unsigned long long)rotate_hash,
|
||||
fiveaxis_pose[0], fiveaxis_pose[1], fiveaxis_pose[2], fiveaxis_joints[0], fiveaxis_joints[1], fiveaxis_joints[2], fiveaxis_joints[3], fiveaxis_joints[4], fiveaxis_joints[5], fiveaxis_joints[6], (unsigned long long)fiveaxis_hash,
|
||||
xyzac_pose[0], xyzac_pose[1], xyzac_pose[2], xyzac_joints[0], xyzac_joints[1], xyzac_joints[2], xyzac_joints[3], xyzac_joints[4], (unsigned long long)xyzac_hash,
|
||||
xyzbc_pose[0], xyzbc_pose[1], xyzbc_pose[2], xyzbc_joints[0], xyzbc_joints[1], xyzbc_joints[2], xyzbc_joints[3], xyzbc_joints[4], (unsigned long long)xyzbc_hash,
|
||||
scara_pose[0], scara_pose[1], scara_pose[2], scara_joints[0], scara_joints[1], scara_joints[2], scara_joints[3], scara_joints[4], scara_joints[5], (unsigned long long)scara_hash,
|
||||
puma_pose[0], puma_pose[1], puma_pose[2], puma_joints[0], puma_joints[1], puma_joints[2], puma_joints[3], puma_joints[4], puma_joints[5], (unsigned long long)puma_hash,
|
||||
scorbot_pose[0], scorbot_pose[1], scorbot_pose[2], scorbot_joints[0], scorbot_joints[1], scorbot_joints[2], scorbot_joints[3], scorbot_joints[4], (unsigned long long)scorbot_hash,
|
||||
genser_pose[0], genser_pose[1], genser_pose[2], genser_joints[0], genser_joints[1], genser_joints[2], genser_joints[3], genser_joints[4], genser_joints[5], (unsigned long long)genser_hash, genser_info.iterations, genser_info.residual,
|
||||
ldelta_pose[0], ldelta_pose[1], ldelta_pose[2], ldelta_joints[0], ldelta_joints[1], ldelta_joints[2], (unsigned long long)ldelta_hash,
|
||||
rdelta_pose[0], rdelta_pose[1], rdelta_pose[2], rdelta_joints[0], rdelta_joints[1], rdelta_joints[2], (unsigned long long)rdelta_hash,
|
||||
tripod_pose[0], tripod_pose[1], tripod_pose[2], tripod_joints[0], tripod_joints[1], tripod_joints[2], (unsigned long long)tripod_hash,
|
||||
genhex_pose[0], genhex_pose[1], genhex_pose[2], genhex_pose[3], genhex_pose[4], genhex_pose[5], genhex_joints[0], genhex_joints[1], genhex_joints[2], genhex_joints[3], genhex_joints[4], genhex_joints[5], (unsigned long long)genhex_hash, genhex_iterations, genhex_residual,
|
||||
penta_pose[0], penta_pose[1], penta_pose[2], penta_pose[3], penta_pose[4], penta_joints[0], penta_joints[1], penta_joints[2], penta_joints[3], penta_joints[4], (unsigned long long)penta_hash, penta_iterations, penta_residual,
|
||||
ldelta_alt_status, ldelta_alt_branch, ldelta_alt_pose.tran.x, ldelta_alt_pose.tran.y, ldelta_alt_pose.tran.z, ldelta_no_solution, ldelta_limit_error, ldelta_geometry_status, ldelta_branch, ldelta_iterations,
|
||||
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);
|
||||
}
|
||||
|
||||
int sim_kinematics_test_run(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZABCUVW");
|
||||
double joints[9] = {10, 20, 30, 4, 5, 6, 7, 8, 9};
|
||||
sim_posemath_pose pose = {0};
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, joints, 9);
|
||||
|
||||
config = config_for(SIM_KIN_TRIVKINS, "xyyz");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3, 4}, 4, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - 1.0) < 1e-12);
|
||||
assert(fabs(pose.tran.y - 2.0) < 1e-12);
|
||||
assert(fabs(pose.tran.z - 4.0) < 1e-12);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 4) == SIM_KIN_OK);
|
||||
/* Duplicate Y maps to the first Y joint deterministically. */
|
||||
assert(fabs(joints[1] - joints[2]) < 1e-12);
|
||||
assert(sim_kinematics_state_hash(&kinematics, joints, 4, &pose) == sim_kinematics_state_hash(&kinematics, joints, 4, &pose));
|
||||
|
||||
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
||||
config.limit_enabled[1] = 1; config.joint_min[1] = -2.0; config.joint_max[1] = 2.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){0, 3, 0}, 3, &pose) == SIM_KIN_ERR_RANGE);
|
||||
assert(sim_kinematics_home(&kinematics, joints, 3, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x) < 1e-12 && fabs(pose.tran.y) < 1e-12 && fabs(pose.tran.z) < 1e-12);
|
||||
|
||||
/* Extra joints are configured but are not passed to kinematics. */
|
||||
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
||||
config.joint_count = 4;
|
||||
config.kinematics_joint_count = 3;
|
||||
config.extra_joint_count = 1;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3, 99}, 4, &pose) == SIM_KIN_ERR_CONFIGURATION);
|
||||
assert(sim_kinematics_home(&kinematics, joints, 3, &pose) == SIM_KIN_OK);
|
||||
|
||||
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
||||
config.mode = SIM_KIN_MODE_FORWARD_ONLY;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 3) == SIM_KIN_ERR_UNSUPPORTED);
|
||||
config.mode = SIM_KIN_MODE_INVERSE_ONLY;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3}, 3, &pose) == SIM_KIN_ERR_UNSUPPORTED);
|
||||
|
||||
config.mode = SIM_KIN_MODE_BOTH;
|
||||
config.max_iterations = 25;
|
||||
config.convergence_tolerance = 1e-10;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
{
|
||||
sim_kinematics_result_info info = {0};
|
||||
assert(sim_kinematics_forward_ex(&kinematics, (double[]){1, 2, 3}, 3, 7, &pose, &info) == SIM_KIN_OK);
|
||||
assert(info.inverse_flags == 7);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &pose, 3, 9, joints, &info) == SIM_KIN_OK);
|
||||
assert(info.forward_flags == 9);
|
||||
}
|
||||
|
||||
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
||||
config.x_rot_point = 20; config.y_offset = 3; config.tool_offset = 100;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
joints[0] = 100; joints[1] = 50; joints[2] = 200; joints[3] = 30; joints[4] = 45;
|
||||
round_trip(&kinematics, joints, 5);
|
||||
|
||||
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
||||
config.x_offset = 5; config.z_offset = 2; config.tool_offset = 50;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
joints[0] = 100; joints[1] = 50; joints[2] = 200; joints[3] = 30; joints[4] = 45;
|
||||
round_trip(&kinematics, joints, 5);
|
||||
|
||||
config = config_for(SIM_KIN_XYZBCW, "XYZBCW");
|
||||
config.pivot_length = 250;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
joints[0] = 10; joints[1] = 20; joints[2] = 30; joints[3] = 35; joints[4] = 40; joints[5] = -2;
|
||||
round_trip(&kinematics, joints, 6);
|
||||
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){NAN, 0, 0, 0, 0, 0}, 6, &pose) == SIM_KIN_ERR_NONFINITE);
|
||||
config.type = 99;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_UNKNOWN_TYPE);
|
||||
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
pose.a = 0; pose.b = 90; pose.c = 0;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_ERR_SINGULAR);
|
||||
|
||||
config = config_for(SIM_KIN_COREXY, "XYZ");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){6, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - 4.0) < 1e-12 && fabs(pose.tran.y - 2.0) < 1e-12);
|
||||
round_trip(&kinematics, (double[]){6, 2, 3}, 3);
|
||||
config = config_for(SIM_KIN_COREXY, "YZ");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
||||
|
||||
{
|
||||
sim_kinematics second = {0};
|
||||
sim_posemath_pose q2 = {{-1.0, 0.1, 7.0}, 0, 0, 0, 0, 0, 0};
|
||||
sim_posemath_pose q3 = {{-1.0, -0.1, 7.0}, 0, 0, 0, 0, 0, 0};
|
||||
uint64_t first_hash;
|
||||
config = config_for(SIM_KIN_ROSE, "XYZ");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_init(&second, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &q2, joints, 3) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &q3, joints, 3) == SIM_KIN_OK);
|
||||
assert(kinematics.revolution_count == 1 && joints[2] > 180.0 && joints[2] < 190.0);
|
||||
first_hash = sim_kinematics_state_hash(&kinematics, joints, 3, &q3);
|
||||
assert(sim_kinematics_inverse(&kinematics, &q3, joints, 3) == SIM_KIN_OK);
|
||||
assert(first_hash == sim_kinematics_state_hash(&kinematics, joints, 3, &q3));
|
||||
assert(sim_kinematics_inverse(&second, &q2, joints + 3, 3) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&second, &q3, joints + 3, 3) == SIM_KIN_OK);
|
||||
assert(first_hash == sim_kinematics_state_hash(&second, joints + 3, 3, &q3));
|
||||
assert(sim_kinematics_inverse(&kinematics, &q2, joints, 3) == SIM_KIN_OK);
|
||||
assert(kinematics.revolution_count == 0 && joints[2] > 170.0 && joints[2] < 180.0);
|
||||
config.joint_count = 4;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
||||
}
|
||||
|
||||
config = config_for(SIM_KIN_MAXKINS, "XYZABC");
|
||||
config.pivot_length = 0.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){3, 4, 5, 0, 30, 90}, 6);
|
||||
config.pivot_length = 0.666;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){0, 0, 5, 0, 30, 0}, 6, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - 0.333) < 1e-12);
|
||||
|
||||
config = config_for(SIM_KIN_ROTATEKINS, "XYZABC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - 4.0) < 1e-12 && fabs(pose.tran.y + 3.0) < 1e-12);
|
||||
|
||||
config = config_for(SIM_KIN_5AXISKINS, "XYZBCWY");
|
||||
config.pivot_length = 250.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.x - 10.0 - (250.0 - 2.0) * sin((180.0 - 35.0) * 0.017453292519943295769) * cos(40.0 * 0.017453292519943295769)) < 1e-9);
|
||||
config = config_for(SIM_KIN_5AXISKINS, "XYZBC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
||||
|
||||
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
||||
config.scara_d1 = 490.0; config.scara_d2 = 340.0; config.scara_d3 = 50.0; config.scara_d4 = 250.0; config.scara_d5 = 50.0; config.scara_d6 = 50.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.c - 5.0) < 1e-9);
|
||||
config = config_for(SIM_KIN_SCARA, "XYZC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
||||
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
||||
config.scara_d2 = 1.0; config.scara_d4 = 1.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
pose.tran.x = 10.0; pose.tran.y = 0.0; pose.tran.z = 0.0; pose.c = 0.0;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_RANGE);
|
||||
|
||||
config = config_for(SIM_KIN_PUMA, "XYZABC");
|
||||
config.puma_a2 = 400.0; config.puma_a3 = 50.0; config.puma_d3 = 100.0; config.puma_d4 = 400.0; config.puma_d6 = 95.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){10, 20, -30, 40, 25, -15}, 6);
|
||||
pose.tran.x = 0.0; pose.tran.y = 0.0; pose.tran.z = 0.0; pose.a = 0.0; pose.b = 0.0; pose.c = 0.0;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_RANGE || sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_SINGULAR);
|
||||
|
||||
config = config_for(SIM_KIN_SCORBOT, "XYZAB");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){25, 35, -20, 10, -5}, 5);
|
||||
pose.tran.x = 1000.0; pose.tran.y = 0.0; pose.tran.z = 0.0;
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_ERR_RANGE);
|
||||
|
||||
config = config_for(SIM_KIN_GENSER, "XYZABC");
|
||||
config.dh_joint_count = 6; config.max_iterations = 200; config.convergence_tolerance = 1e-7;
|
||||
config.dh_a[0] = 0.0; config.dh_a[1] = 85.0; config.dh_a[2] = 380.0; config.dh_a[3] = 100.0;
|
||||
config.dh_alpha[0] = 0.0; config.dh_alpha[1] = -1.5707963267948966; config.dh_alpha[2] = 0.0; config.dh_alpha[3] = -1.5707963267948966; config.dh_alpha[4] = 1.5707963267948966; config.dh_alpha[5] = -1.5707963267948966;
|
||||
config.dh_d[0] = 350.0; config.dh_d[1] = 0.0; config.dh_d[2] = 0.0; config.dh_d[3] = 425.0; config.dh_d[4] = 0.0; config.dh_d[5] = 235.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
round_trip(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6);
|
||||
config.max_iterations = 1;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6, &pose) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_SINGULAR);
|
||||
config.dh_alpha[0] = NAN;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
|
||||
|
||||
emit_coupled_samples();
|
||||
return 0;
|
||||
}
|
||||
5
wasm-core/tests/sim_kinematics_test_main.c
Normal file
5
wasm-core/tests/sim_kinematics_test_main.c
Normal file
@@ -0,0 +1,5 @@
|
||||
int sim_kinematics_test_run(void);
|
||||
|
||||
int main(void) {
|
||||
return sim_kinematics_test_run();
|
||||
}
|
||||
36
wasm-core/tests/sim_motion_queue_test.c
Normal file
36
wasm-core/tests/sim_motion_queue_test.c
Normal file
@@ -0,0 +1,36 @@
|
||||
#include "sim_motion_queue.h"
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
static sim_posemath_segment line(double x0, double x1) {
|
||||
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}, 1.0, 1.0, SIM_POSEMATH_LINEAR, 0
|
||||
};
|
||||
return segment;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_motion_queue queue = {0};
|
||||
sim_motion_command command = {0};
|
||||
sim_motion_state state = {0};
|
||||
char canonical[512] = {0};
|
||||
uint32_t id = 0;
|
||||
command.segment = line(0, 1);
|
||||
command.tag = 42;
|
||||
assert(sim_motion_init(&queue, 0.1, 10.0) == SIM_MOTION_OK);
|
||||
assert(sim_motion_enqueue(&queue, &command, &id) == SIM_MOTION_OK && id == 1);
|
||||
command.segment = line(1, 2);
|
||||
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_OK);
|
||||
assert(sim_motion_step(&queue) == SIM_MOTION_OK);
|
||||
assert(sim_motion_read_state(&queue, &state) == SIM_MOTION_OK);
|
||||
assert(state.tick == 1 && state.queue_depth == 2 && state.position.tran.x > 0.09);
|
||||
assert(sim_motion_state_hash(&queue) != 0);
|
||||
assert(sim_motion_canonical(&queue, canonical, sizeof(canonical)) > 1);
|
||||
command.segment = line(9, 10);
|
||||
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_STATE);
|
||||
command.segment.motion_type = 99;
|
||||
assert(sim_motion_enqueue(&queue, &command, 0) == SIM_MOTION_ERR_MOTION_TYPE);
|
||||
return 0;
|
||||
}
|
||||
@@ -16,8 +16,35 @@ int main(void) {
|
||||
assert(output.x == 6.5 && output.y == 7.0 && output.z == 5.0);
|
||||
assert(sim_posemath_line_point(&input, &end, 1.1, &output) == SIM_POSEMATH_ERR_RANGE);
|
||||
input.x = NAN;
|
||||
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_ARGUMENT);
|
||||
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_NONFINITE);
|
||||
input.x = 0.0; input.y = 0.0; input.z = 0.0;
|
||||
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);
|
||||
|
||||
{
|
||||
sim_posemath_pose start = {{0.0, 0.0, 0.0}, 0, 0, 0, 0, 0, 0};
|
||||
sim_posemath_pose finish = {{2.0, 0.0, 0.0}, 1, 2, 3, 4, 5, 6};
|
||||
sim_posemath_pose point = {0};
|
||||
sim_posemath_segment segment = {start, finish, {0, 0, 0}, {0, 0, 1}, 1.0, 2.0, SIM_POSEMATH_LINEAR, 0};
|
||||
sim_tp planner = {0};
|
||||
sim_tp_state first = {0};
|
||||
sim_tp_state second = {0};
|
||||
uint32_t id = 0;
|
||||
assert(sim_posemath_pose_line_point(&start, &finish, 0.5, &point) == SIM_POSEMATH_OK);
|
||||
assert(point.tran.x == 1.0 && point.a == 0.5 && point.w == 3.0);
|
||||
assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_OK);
|
||||
assert(sim_posemath_segment_point(&segment, 0.5, &point) == SIM_POSEMATH_OK);
|
||||
assert(sim_tp_init(&planner, 0.1, 10.0, 10.0) == SIM_POSEMATH_OK);
|
||||
assert(sim_tp_enqueue(&planner, &segment, &id) == SIM_POSEMATH_OK && id == 1);
|
||||
assert(sim_tp_step(&planner) == SIM_POSEMATH_OK);
|
||||
assert(sim_tp_read_state(&planner, &first) == SIM_POSEMATH_OK);
|
||||
assert(sim_tp_state_hash(&planner) == sim_posemath_hash_state(&first));
|
||||
assert(sim_tp_read_state(&planner, &second) == SIM_POSEMATH_OK);
|
||||
assert(sim_posemath_hash_state(&first) == sim_posemath_hash_state(&second));
|
||||
segment.motion_type = 99;
|
||||
assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_ERR_MOTION_TYPE);
|
||||
segment.motion_type = SIM_POSEMATH_LINEAR;
|
||||
segment.velocity = 11.0;
|
||||
assert(sim_tp_enqueue(&planner, &segment, 0) == SIM_POSEMATH_ERR_RANGE);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user