Extend simulation runtime coverage and kinematics support
This commit is contained in:
@@ -24,5 +24,12 @@ int main(void) {
|
||||
assert(sim_event_import(exported, export_size) == SIM_EVENT_OK);
|
||||
assert(sim_event_count() == 1);
|
||||
assert(sim_event_import(exported, export_size - 1) == SIM_EVENT_ERR_ARGUMENT);
|
||||
((sim_event_record *)exported)->seq = 2;
|
||||
assert(sim_event_import(exported, export_size) == SIM_EVENT_ERR_SEQUENCE);
|
||||
assert(sim_event_cursor() == 1);
|
||||
((sim_event_record *)exported)->seq = 1;
|
||||
((sim_event_record *)exported)->kind[SIM_EVENT_KIND_CAPACITY - 1U] = 'x';
|
||||
assert(sim_event_import(exported, export_size) == SIM_EVENT_ERR_ARGUMENT);
|
||||
assert(sim_event_cursor() == 1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
89
wasm-core/tests/sim_io_test.c
Normal file
89
wasm-core/tests/sim_io_test.c
Normal file
@@ -0,0 +1,89 @@
|
||||
#include "sim_io.h"
|
||||
|
||||
#include "sim_hal.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_posemath_pose pose(double x, double y, double z) {
|
||||
sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
|
||||
return value;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_io io = {0};
|
||||
sim_io_state state = {0};
|
||||
sim_hal_value pin = {0};
|
||||
char canonical[2048];
|
||||
uint64_t hash;
|
||||
uint64_t repeat_hash;
|
||||
int invalid_pin_status;
|
||||
int limit_status;
|
||||
int spindle_step_status;
|
||||
int tool_busy_status;
|
||||
int probe_status;
|
||||
int estop_command_status;
|
||||
int estop_recover_status;
|
||||
int recover_status;
|
||||
int fault_clear_status;
|
||||
|
||||
sim_hal_reset();
|
||||
assert(sim_io_init(&io, 2, 32, 0.1, 600.0, 5.0) == SIM_IO_OK);
|
||||
assert(sim_hal_pin_count() >= 58 && sim_hal_function_count() == 1);
|
||||
assert(sim_hal_get_pin("motion.probe-input", &pin) == SIM_HAL_OK && pin.type == SIM_HAL_BIT);
|
||||
invalid_pin_status = sim_hal_set_pin("spindle.0.speed-out", (sim_hal_value){SIM_HAL_BIT, {.bit = 1}});
|
||||
assert(invalid_pin_status == SIM_HAL_ERR_TYPE);
|
||||
limit_status = sim_io_spindle_command(&io, 0, 100001.0, 1);
|
||||
assert(limit_status == SIM_IO_ERR_LIMIT);
|
||||
assert(sim_io_spindle_command(&io, 0, 1200.0, 1) == SIM_IO_OK);
|
||||
spindle_step_status = sim_io_step(&io, 10);
|
||||
assert(spindle_step_status == SIM_IO_OK && io.state.spindles[0].speed_rpm == 600.0);
|
||||
assert(sim_io_step(&io, 10) == SIM_IO_OK && io.state.spindles[0].at_speed == 1);
|
||||
|
||||
assert(sim_io_tool_prepare(&io, 7, 3) == SIM_IO_OK);
|
||||
assert(sim_io_tool_set_prepared(&io, 1) == SIM_IO_OK);
|
||||
tool_busy_status = sim_io_tool_change(&io);
|
||||
assert(tool_busy_status == SIM_IO_ERR_BUSY);
|
||||
assert(sim_io_spindle_command(&io, 0, 0.0, 0) == SIM_IO_OK);
|
||||
assert(sim_io_step(&io, 20) == SIM_IO_OK);
|
||||
assert(sim_io_tool_change(&io) == SIM_IO_OK);
|
||||
assert(sim_io_tool_set_changed(&io, 1) == SIM_IO_OK && io.state.current_tool == 7);
|
||||
|
||||
assert(sim_io_probe_arm(&io, 0) == SIM_IO_OK);
|
||||
probe_status = sim_io_probe_update(&io, &(sim_posemath_pose){{1.0, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 1);
|
||||
assert(probe_status == SIM_IO_OK && io.state.probe.state == SIM_IO_PROBE_TRIPPED);
|
||||
assert(sim_io_probe_update(&io, &(sim_posemath_pose){{NAN, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 0) == SIM_IO_ERR_NONFINITE);
|
||||
|
||||
assert(sim_io_set_estop(&io, 1) == SIM_IO_OK);
|
||||
estop_command_status = sim_io_spindle_command(&io, 0, 100.0, 1);
|
||||
estop_recover_status = sim_io_recover(&io);
|
||||
assert(estop_command_status == SIM_IO_ERR_ESTOP && estop_recover_status == SIM_IO_ERR_ESTOP);
|
||||
assert(sim_io_set_estop(&io, 0) == SIM_IO_OK);
|
||||
recover_status = sim_io_recover(&io);
|
||||
assert(recover_status == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 1);
|
||||
|
||||
assert(sim_io_tool_set_fault(&io, 1, 42) == SIM_IO_OK);
|
||||
fault_clear_status = sim_io_tool_clear_fault(&io);
|
||||
assert(fault_clear_status == SIM_IO_ERR_FAULT);
|
||||
assert(sim_io_tool_set_fault(&io, 0, 0) == SIM_IO_OK);
|
||||
assert(sim_io_tool_clear_fault(&io) == SIM_IO_OK);
|
||||
hash = sim_io_state_hash(&io);
|
||||
repeat_hash = sim_io_state_hash(&io);
|
||||
assert(hash == repeat_hash);
|
||||
assert(sim_io_canonical(&io, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
|
||||
assert(sim_io_canonical(&io, 0, 0) == strlen(canonical) + 1);
|
||||
assert(sim_hal_get_pin("iocontrol.0.tool-number", &pin) == SIM_HAL_OK && pin.value.s32 == 7);
|
||||
assert(sim_hal_get_pin("spindle.0.brake", &pin) == SIM_HAL_OK && pin.value.bit == 1);
|
||||
printf("{\"schema\":\"io-samples.v1\",\"pin\":{\"invalid_type\":%d,\"count\":%u,\"functions\":%u},\"spindle\":{\"limit\":%d,\"step\":%d,\"speed\":%.3f,\"at_speed\":%u,\"tool_busy\":%d},\"tool\":{\"state\":%u,\"current\":%d,\"fault_clear\":%d},\"probe\":{\"status\":%d,\"state\":%u,\"position\":[%.3f,%.3f,%.3f]},\"estop\":{\"command\":%d,\"recover_before_clear\":%d,\"recover\":%d},\"state_hash\":%llu,\"repeat_hash\":%llu,\"canonical\":%s}\n",
|
||||
invalid_pin_status, sim_hal_pin_count(), sim_hal_function_count(), limit_status,
|
||||
spindle_step_status, io.state.spindles[0].speed_rpm, io.state.spindles[0].at_speed,
|
||||
tool_busy_status, io.state.tool_state, io.state.current_tool, fault_clear_status,
|
||||
probe_status, io.state.probe.state, io.state.probe.position.tran.x,
|
||||
io.state.probe.position.tran.y, io.state.probe.position.tran.z,
|
||||
estop_command_status, estop_recover_status, recover_status,
|
||||
(unsigned long long)hash, (unsigned long long)repeat_hash, canonical);
|
||||
(void)pose;
|
||||
return 0;
|
||||
}
|
||||
65
wasm-core/tests/sim_kinematics_feedback_test.c
Normal file
65
wasm-core/tests/sim_kinematics_feedback_test.c
Normal file
@@ -0,0 +1,65 @@
|
||||
#include "sim_kinematics_feedback.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_kinematics_config config_for(int type, const char *coordinates) {
|
||||
sim_kinematics_config config = {0};
|
||||
config.type = type;
|
||||
config.joint_count = (uint32_t)strlen(coordinates);
|
||||
strcpy(config.coordinates, coordinates);
|
||||
return config;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics singular = {0};
|
||||
sim_kinematics_feedback feedback = {0};
|
||||
sim_kinematics_feedback singular_feedback = {0};
|
||||
sim_kinematics_feedback_state state = {0};
|
||||
sim_kinematics_result_info info = {0};
|
||||
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
||||
double joints[3] = {1.0, 2.0, 3.0};
|
||||
double bad_joints[3] = {1.0, NAN, 3.0};
|
||||
double singular_joints[5] = {0, 0, 0, 90, 0};
|
||||
uint64_t hash;
|
||||
uint64_t repeat_hash;
|
||||
uint32_t valid_tick;
|
||||
int update_status;
|
||||
int bad_status;
|
||||
int singular_status;
|
||||
int stale_read_status;
|
||||
int count_status;
|
||||
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_feedback_init(&feedback, &kinematics, 3) == SIM_KIN_FEEDBACK_OK);
|
||||
stale_read_status = sim_kinematics_feedback_read(&feedback, &state);
|
||||
update_status = sim_kinematics_feedback_update(&feedback, joints, 3, 0, &info);
|
||||
assert(update_status == SIM_KIN_FEEDBACK_OK);
|
||||
assert(sim_kinematics_feedback_read(&feedback, &state) == SIM_KIN_FEEDBACK_OK);
|
||||
valid_tick = state.tick;
|
||||
hash = sim_kinematics_feedback_state_hash(&feedback);
|
||||
repeat_hash = sim_kinematics_feedback_state_hash(&feedback);
|
||||
assert(state.valid && state.tick == 1 && fabs(state.pose.tran.x - 1.0) < 1e-12);
|
||||
bad_status = sim_kinematics_feedback_update(&feedback, bad_joints, 3, 0, &info);
|
||||
assert(bad_status == SIM_KIN_FEEDBACK_ERR_NONFINITE && feedback.state.tick == valid_tick);
|
||||
count_status = sim_kinematics_feedback_update(&feedback, joints, 2, 0, &info);
|
||||
assert(count_status == SIM_KIN_FEEDBACK_ERR_CONFIGURATION);
|
||||
|
||||
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
||||
config.mode = SIM_KIN_MODE_INVERSE_ONLY;
|
||||
assert(sim_kinematics_init(&singular, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_feedback_init(&singular_feedback, &singular, 5) == SIM_KIN_FEEDBACK_OK);
|
||||
singular_status = sim_kinematics_feedback_update(&singular_feedback, singular_joints, 5, 0, &info);
|
||||
assert(singular_status == SIM_KIN_FEEDBACK_ERR_FORWARD && singular_feedback.state.valid == 0 && singular_feedback.state.kinematics_error == SIM_KIN_ERR_UNSUPPORTED);
|
||||
|
||||
printf("{\"schema\":\"kinematics-feedback-samples.v1\",\"nominal\":{\"status\":%d,\"tick\":%u,\"valid\":%u,\"pose\":[%.9f,%.9f,%.9f],\"flags\":[%llu,%llu],\"state_hash\":%llu,\"repeat_hash\":%llu},\"stale_read\":%d,\"nonfinite\":%d,\"count_error\":%d,\"singular\":{\"status\":%d,\"kinematics_error\":%d,\"valid\":%u}}\n",
|
||||
update_status, state.tick, state.valid, state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
|
||||
(unsigned long long)state.forward_flags, (unsigned long long)state.inverse_flags,
|
||||
(unsigned long long)hash, (unsigned long long)repeat_hash, stale_read_status,
|
||||
bad_status, count_status, singular_status, singular_feedback.state.kinematics_error,
|
||||
singular_feedback.state.valid);
|
||||
return 0;
|
||||
}
|
||||
57
wasm-core/tests/sim_kinematics_home_limit_test.c
Normal file
57
wasm-core/tests/sim_kinematics_home_limit_test.c
Normal file
@@ -0,0 +1,57 @@
|
||||
#include "sim_kinematics_home_limit.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_kinematics_config config_for(void) {
|
||||
sim_kinematics_config config = {0};
|
||||
config.type = SIM_KIN_TRIVKINS;
|
||||
config.joint_count = 3;
|
||||
strcpy(config.coordinates, "XYZ");
|
||||
config.limit_enabled[0] = 1; config.joint_min[0] = -5.0; config.joint_max[0] = 5.0;
|
||||
config.limit_enabled[1] = 1; config.joint_min[1] = -5.0; config.joint_max[1] = 5.0;
|
||||
config.limit_enabled[2] = 1; config.joint_min[2] = -5.0; config.joint_max[2] = 5.0;
|
||||
return config;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics_home_limit home_limit = {0};
|
||||
sim_kinematics_home_limit_state state = {0};
|
||||
sim_kinematics_config config = config_for();
|
||||
double nominal[3] = {1.0, 2.0, 3.0};
|
||||
double outside[3] = {6.0, 2.0, 3.0};
|
||||
double nonfinite[3] = {NAN, 2.0, 3.0};
|
||||
uint64_t hash;
|
||||
uint64_t repeat_hash;
|
||||
int before_home;
|
||||
int home_status;
|
||||
int limit_status;
|
||||
int nonfinite_status;
|
||||
int sequence_status;
|
||||
int count_status;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_home_limit_init(&home_limit, &kinematics, 3) == SIM_KIN_HOME_OK);
|
||||
before_home = sim_kinematics_home_limit_check(&home_limit, nominal, 3);
|
||||
sequence_status = sim_kinematics_home_limit_home(&home_limit, 0x10000u);
|
||||
home_status = sim_kinematics_home_limit_home(&home_limit, 7);
|
||||
assert(home_status == SIM_KIN_HOME_OK && home_limit.state.homed == 1 && home_limit.state.home_sequence == 7);
|
||||
limit_status = sim_kinematics_home_limit_check(&home_limit, outside, 3);
|
||||
assert(limit_status == SIM_KIN_HOME_ERR_LIMIT && home_limit.state.joints[0] == 0.0);
|
||||
nonfinite_status = sim_kinematics_home_limit_check(&home_limit, nonfinite, 3);
|
||||
count_status = sim_kinematics_home_limit_check(&home_limit, nominal, 2);
|
||||
assert(sim_kinematics_home_limit_check(&home_limit, nominal, 3) == SIM_KIN_HOME_OK);
|
||||
assert(sim_kinematics_home_limit_read(&home_limit, &state) == SIM_KIN_HOME_OK);
|
||||
hash = sim_kinematics_home_limit_state_hash(&home_limit);
|
||||
repeat_hash = sim_kinematics_home_limit_state_hash(&home_limit);
|
||||
assert(hash == repeat_hash && state.tick == 2);
|
||||
printf("{\"schema\":\"kinematics-home-limit-samples.v1\",\"before_home\":%d,\"sequence_error\":%d,\"home_status\":%d,\"limit_status\":%d,\"nonfinite_status\":%d,\"count_status\":%d,\"state\":{\"homed\":%u,\"sequence\":%u,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"hash\":%llu,\"repeat_hash\":%llu}}\n",
|
||||
before_home, sequence_status, home_status, limit_status, nonfinite_status,
|
||||
count_status, state.homed, state.home_sequence, state.tick,
|
||||
state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
|
||||
state.joints[0], state.joints[1], state.joints[2],
|
||||
(unsigned long long)hash, (unsigned long long)repeat_hash);
|
||||
return 0;
|
||||
}
|
||||
107
wasm-core/tests/sim_kinematics_motion_test.c
Normal file
107
wasm-core/tests/sim_kinematics_motion_test.c
Normal file
@@ -0,0 +1,107 @@
|
||||
#include "sim_kinematics_motion.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_posemath_segment line(double x0, double x1, double velocity) {
|
||||
sim_posemath_segment segment = {
|
||||
{{x0, 0, 0}, 0, 0, 0, 0, 0, 0},
|
||||
{{x1, 0, 0}, 0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0}, {0, 0, 1}, velocity, 10.0, SIM_POSEMATH_LINEAR, 0
|
||||
};
|
||||
return segment;
|
||||
}
|
||||
|
||||
static sim_kinematics_config config_for(int type, const char *coordinates) {
|
||||
sim_kinematics_config config = {0};
|
||||
config.type = type;
|
||||
config.joint_count = (uint32_t)strlen(coordinates);
|
||||
strcpy(config.coordinates, coordinates);
|
||||
return config;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics singular_kins = {0};
|
||||
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
||||
sim_kinematics_motion adapter = {0};
|
||||
sim_kinematics_motion singular_adapter = {0};
|
||||
sim_kinematics_motion_sample sample = {0};
|
||||
sim_posemath_segment segment = line(0.0, 1.0, 1.0);
|
||||
uint32_t id = 0;
|
||||
uint32_t pause_tick;
|
||||
uint64_t nominal_hash;
|
||||
uint64_t nominal_repeat_hash;
|
||||
uint64_t full_planner_before;
|
||||
uint64_t full_planner_after;
|
||||
uint64_t full_hash;
|
||||
uint64_t singular_planner_before;
|
||||
uint64_t singular_planner_after;
|
||||
int pause_status;
|
||||
int resume_status;
|
||||
int full_status;
|
||||
int singular_status;
|
||||
int singular_kinematics_error;
|
||||
int nonfinite_status;
|
||||
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_motion_init(&adapter, &kinematics, 0.1, 10.0, 10.0) == SIM_KIN_MOTION_OK);
|
||||
assert(sim_kinematics_motion_enqueue(&adapter, &segment, &id) == SIM_KIN_MOTION_OK && id == 1);
|
||||
assert(sim_kinematics_motion_pause(&adapter, 1) == SIM_KIN_MOTION_OK);
|
||||
pause_tick = adapter.planner.tick;
|
||||
pause_status = sim_kinematics_motion_step(&adapter);
|
||||
assert(pause_status == SIM_KIN_MOTION_ERR_STATE && adapter.planner.tick == pause_tick && adapter.count == 0);
|
||||
assert(sim_kinematics_motion_pause(&adapter, 0) == SIM_KIN_MOTION_OK);
|
||||
resume_status = sim_kinematics_motion_step(&adapter);
|
||||
assert(resume_status == SIM_KIN_MOTION_OK);
|
||||
assert(sim_kinematics_motion_peek(&adapter, &sample) == SIM_KIN_MOTION_OK);
|
||||
assert(fabs(sample.pose.tran.x - 0.1) < 1e-12 && fabs(sample.joints[0] - 0.1) < 1e-12);
|
||||
nominal_hash = sim_kinematics_motion_state_hash(&adapter);
|
||||
nominal_repeat_hash = sim_kinematics_motion_state_hash(&adapter);
|
||||
assert(sim_kinematics_motion_pop(&adapter, &sample) == SIM_KIN_MOTION_OK);
|
||||
|
||||
assert(sim_kinematics_motion_reset(&adapter) == SIM_KIN_MOTION_OK);
|
||||
segment = line(0.0, 100.0, 1.0);
|
||||
assert(sim_kinematics_motion_enqueue(&adapter, &segment, 0) == SIM_KIN_MOTION_OK);
|
||||
for (uint32_t index = 0; index < SIM_KIN_MOTION_QUEUE_CAPACITY; ++index)
|
||||
assert(sim_kinematics_motion_step(&adapter) == SIM_KIN_MOTION_OK);
|
||||
full_planner_before = sim_tp_state_hash(&adapter.planner);
|
||||
full_status = sim_kinematics_motion_step(&adapter);
|
||||
full_planner_after = sim_tp_state_hash(&adapter.planner);
|
||||
full_hash = sim_kinematics_motion_state_hash(&adapter);
|
||||
assert(full_status == SIM_KIN_MOTION_ERR_CAPACITY);
|
||||
assert(full_planner_before == full_planner_after && adapter.count == SIM_KIN_MOTION_QUEUE_CAPACITY);
|
||||
|
||||
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
||||
assert(sim_kinematics_init(&singular_kins, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_motion_init(&singular_adapter, &singular_kins, 1.0, 10.0, 10.0) == SIM_KIN_MOTION_OK);
|
||||
segment = line(0.0, 1.0, 1.0);
|
||||
segment.end.b = 90.0;
|
||||
assert(sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0) == SIM_KIN_MOTION_OK);
|
||||
singular_planner_before = sim_tp_state_hash(&singular_adapter.planner);
|
||||
singular_status = sim_kinematics_motion_step(&singular_adapter);
|
||||
singular_planner_after = sim_tp_state_hash(&singular_adapter.planner);
|
||||
singular_kinematics_error = singular_adapter.kinematics_error;
|
||||
assert(singular_status == SIM_KIN_MOTION_ERR_INVERSE);
|
||||
assert(singular_kinematics_error == SIM_KIN_ERR_SINGULAR);
|
||||
assert(singular_planner_before == singular_planner_after && singular_adapter.count == 0);
|
||||
|
||||
segment = line(0.0, 1.0, 1.0);
|
||||
segment.end.tran.x = NAN;
|
||||
nonfinite_status = sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0);
|
||||
assert(nonfinite_status == SIM_KIN_MOTION_ERR_NONFINITE);
|
||||
|
||||
printf("{\"schema\":\"kinematics-motion-samples.v1\",\"nominal\":{\"pause_status\":%d,\"resume_status\":%d,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"state_hash\":%llu,\"repeat_hash\":%llu},\"capacity\":{\"status\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu,\"state_hash\":%llu},\"inverse_failure\":{\"status\":%d,\"kinematics_error\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu},\"nonfinite_status\":%d}\n",
|
||||
pause_status, resume_status, sample.tick, sample.pose.tran.x,
|
||||
sample.pose.tran.y, sample.pose.tran.z, sample.joints[0],
|
||||
sample.joints[1], sample.joints[2],
|
||||
(unsigned long long)nominal_hash, (unsigned long long)nominal_repeat_hash,
|
||||
full_status, adapter.count, (unsigned long long)full_planner_before,
|
||||
(unsigned long long)full_planner_after, (unsigned long long)full_hash,
|
||||
singular_status, singular_kinematics_error, singular_adapter.count,
|
||||
(unsigned long long)singular_planner_before,
|
||||
(unsigned long long)singular_planner_after, nonfinite_status);
|
||||
return 0;
|
||||
}
|
||||
63
wasm-core/tests/sim_kinematics_registry_test.c
Normal file
63
wasm-core/tests/sim_kinematics_registry_test.c
Normal file
@@ -0,0 +1,63 @@
|
||||
#include "sim_kinematics.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics_capability capability = {0};
|
||||
sim_kinematics_config config = {0};
|
||||
uint32_t count = sim_kinematics_registry_count();
|
||||
uint32_t verified = 0;
|
||||
uint64_t capability_union = 0;
|
||||
uint64_t parameter_union = 0;
|
||||
int lookup_status;
|
||||
int iterative_require;
|
||||
int capability_error;
|
||||
int parameter_error;
|
||||
int type_mismatch;
|
||||
int coordinate_error;
|
||||
int joint_error;
|
||||
int unknown_name;
|
||||
int path_name;
|
||||
int range_error;
|
||||
|
||||
assert(count == 18);
|
||||
printf("{\"schema\":\"kinematics-registry-samples.v1\",\"entries\":[");
|
||||
for (uint32_t index = 0; index < count; ++index) {
|
||||
assert(sim_kinematics_registry_get(index, &capability) == SIM_KIN_OK);
|
||||
memset(&config, 0, sizeof(config));
|
||||
config.type = capability.type;
|
||||
config.joint_count = capability.min_joints;
|
||||
strcpy(config.coordinates, capability.required_coordinates[0] == '\0' ? "X" : capability.required_coordinates);
|
||||
if (sim_kinematics_registry_validate_config(capability.name, &config, 0) == SIM_KIN_OK) verified += 1;
|
||||
capability_union |= capability.capabilities;
|
||||
parameter_union |= capability.parameter_mask;
|
||||
printf("%s{\"name\":\"%s\",\"type\":%d,\"required\":\"%s\",\"min\":%u,\"max\":%u,\"capabilities\":%u,\"parameters\":%llu}",
|
||||
index == 0 ? "" : ",", capability.name, capability.type,
|
||||
capability.required_coordinates, capability.min_joints,
|
||||
capability.max_joints, capability.capabilities,
|
||||
(unsigned long long)capability.parameter_mask);
|
||||
}
|
||||
lookup_status = sim_kinematics_registry_find("xyzac-trt-kins", &capability);
|
||||
iterative_require = sim_kinematics_registry_require("genserkins", SIM_KIN_CAP_ITERATIVE, SIM_KIN_PARAM_DH, &capability);
|
||||
capability_error = sim_kinematics_registry_require("trivkins", SIM_KIN_CAP_ITERATIVE, 0, 0);
|
||||
parameter_error = sim_kinematics_registry_require("trivkins", 0, SIM_KIN_PARAM_TOOL, 0);
|
||||
memset(&config, 0, sizeof(config));
|
||||
config.type = SIM_KIN_TRIVKINS; config.joint_count = 5; strcpy(config.coordinates, "XYZAC");
|
||||
type_mismatch = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
|
||||
config.type = SIM_KIN_XYZAC_TRT; strcpy(config.coordinates, "XYZAQ");
|
||||
coordinate_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
|
||||
strcpy(config.coordinates, "XYZAC"); config.joint_count = 4;
|
||||
joint_error = sim_kinematics_registry_validate_config("xyzac-trt-kins", &config, 0);
|
||||
unknown_name = sim_kinematics_registry_find("missing-kins", &capability);
|
||||
path_name = sim_kinematics_registry_find("plugins/user.so", &capability);
|
||||
range_error = sim_kinematics_registry_get(count, &capability);
|
||||
printf("],\"count\":%u,\"verified\":%u,\"capability_union\":%llu,\"parameter_union\":%llu,\"registry_hash\":%llu,\"lookup_status\":%d,\"iterative_require\":%d,\"capability_error\":%d,\"parameter_error\":%d,\"type_mismatch\":%d,\"coordinate_error\":%d,\"joint_error\":%d,\"unknown_name\":%d,\"path_name\":%d,\"range_error\":%d}\n",
|
||||
count, verified, (unsigned long long)capability_union,
|
||||
(unsigned long long)parameter_union,
|
||||
(unsigned long long)sim_kinematics_registry_state_hash(), lookup_status,
|
||||
iterative_require, capability_error, parameter_error, type_mismatch,
|
||||
coordinate_error, joint_error, unknown_name, path_name, range_error);
|
||||
return 0;
|
||||
}
|
||||
@@ -32,6 +32,11 @@ static double canonical(double value) {
|
||||
return fabs(value) < 5e-10 ? 0.0 : value;
|
||||
}
|
||||
|
||||
static double canonical_pentapod(double value) {
|
||||
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
|
||||
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
|
||||
}
|
||||
|
||||
static void emit_coupled_samples(void) {
|
||||
sim_kinematics kinematics = {0};
|
||||
sim_kinematics_config config;
|
||||
@@ -123,6 +128,11 @@ static void emit_coupled_samples(void) {
|
||||
uint32_t penta_branch;
|
||||
uint32_t penta_alt_branch;
|
||||
uint32_t penta_iterations;
|
||||
uint32_t penta_alt_iterations;
|
||||
uint32_t penta_nonpositive_iterations;
|
||||
uint32_t penta_iter_limit_iterations;
|
||||
uint32_t penta_max_error_iterations;
|
||||
uint32_t penta_singular_iterations;
|
||||
double penta_residual;
|
||||
sim_posemath_pose tripod_alt_pose = {0};
|
||||
sim_posemath_pose genhex_alt_pose = {0};
|
||||
@@ -341,21 +351,21 @@ static void emit_coupled_samples(void) {
|
||||
genhex_singular = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
||||
|
||||
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
|
||||
config.penta_initial_pose[0] = 0.0; config.penta_initial_pose[1] = 500.0; config.penta_initial_pose[2] = 300.0; config.penta_initial_pose[3] = 2.0 * 0.017453292519943295769; config.penta_initial_pose[4] = -3.0 * 0.017453292519943295769;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, 5, 0, output, &genser_info) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 0.01, .b = -0.01}, 5, 0, output, &genser_info) == SIM_KIN_OK);
|
||||
memcpy(penta_joints, output, sizeof(penta_joints));
|
||||
assert(sim_kinematics_forward_ex(&kinematics, output, 5, 0, &pose, &genser_info) == SIM_KIN_OK);
|
||||
penta_pose[0] = canonical(pose.tran.x); penta_pose[1] = canonical(pose.tran.y); penta_pose[2] = canonical(pose.tran.z); penta_pose[3] = canonical(pose.a); penta_pose[4] = canonical(pose.b);
|
||||
penta_iterations = genser_info.iterations; penta_residual = genser_info.residual; penta_branch = genser_info.branch_id;
|
||||
penta_pose[0] = canonical_pentapod(pose.tran.x); penta_pose[1] = canonical_pentapod(pose.tran.y); penta_pose[2] = canonical_pentapod(pose.tran.z); penta_pose[3] = canonical_pentapod(pose.a); penta_pose[4] = canonical_pentapod(pose.b);
|
||||
pose.tran.x = penta_pose[0]; pose.tran.y = penta_pose[1]; pose.tran.z = penta_pose[2]; pose.a = penta_pose[3]; pose.b = penta_pose[4];
|
||||
penta_iterations = genser_info.iterations; penta_residual = nearbyint(genser_info.residual * 1000000000.0) / 1000000000.0; penta_branch = genser_info.branch_id;
|
||||
penta_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
||||
penta_alt_status = sim_kinematics_forward_ex(&kinematics, output, 5, SIM_KIN_GENHEX_ALTERNATE_SEED, &penta_alt_pose, &genser_info); penta_alt_branch = genser_info.branch_id;
|
||||
penta_nonpositive = sim_kinematics_forward(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0}, 5, &pose);
|
||||
penta_alt_status = sim_kinematics_forward_ex(&kinematics, output, 5, SIM_KIN_GENHEX_ALTERNATE_SEED, &penta_alt_pose, &genser_info); penta_alt_branch = genser_info.branch_id; penta_alt_iterations = genser_info.iterations;
|
||||
penta_nonpositive = sim_kinematics_forward_ex(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0}, 5, 0, &pose, &genser_info); penta_nonpositive_iterations = genser_info.iterations;
|
||||
config.penta_initial_pose[3] = 0.0; config.penta_initial_pose[4] = 0.0;
|
||||
config.penta_iteration_limit = 1; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_iter_limit_error = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
|
||||
config.penta_iteration_limit = 120; config.penta_max_error = 0.000001; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_max_error_status = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
|
||||
config.penta_iteration_limit = 1; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_iter_limit_error = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_iter_limit_iterations = genser_info.iterations;
|
||||
config.penta_iteration_limit = 120; config.penta_max_error = 0.000001; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_max_error_status = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_max_error_iterations = genser_info.iterations;
|
||||
config.penta_max_error = 1000.0; config.limit_enabled[0] = 1; config.joint_min[0] = 0.0; config.joint_max[0] = 1.0; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, output, 5);
|
||||
config.limit_enabled[0] = 0; config.penta_base[0][0] = 1.0; config.penta_base[0][1] = 1.0; config.penta_base[0][2] = 1.0; for (int duplicate = 1; duplicate < 5; ++duplicate) memcpy(config.penta_base[duplicate], config.penta_base[0], sizeof(config.penta_base[0])); assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_singular = sim_kinematics_forward(&kinematics, penta_joints, 5, &pose);
|
||||
config.limit_enabled[0] = 0; config.penta_base[0][0] = 1.0; config.penta_base[0][1] = 1.0; config.penta_base[0][2] = 1.0; for (int duplicate = 1; duplicate < 5; ++duplicate) memcpy(config.penta_base[duplicate], config.penta_base[0], sizeof(config.penta_base[0])); assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_singular = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_singular_iterations = genser_info.iterations;
|
||||
|
||||
printf("{\"schema\":\"kinematics-coupled-samples.v1\","
|
||||
"\"corexy\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
||||
@@ -378,7 +388,7 @@ static void emit_coupled_samples(void) {
|
||||
"\"rotarydelta\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"singular\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
||||
"\"tripod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"no_solution\":%d,\"singular\":%d,\"limit_error\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
||||
"\"genhex\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"nonpositive\":%d,\"iteration_limit\":%d,\"max_error\":%d,\"singular\":%d,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f},"
|
||||
"\"pentapod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"nonpositive\":%d,\"iteration_limit\":%d,\"max_error\":%d,\"singular\":%d,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f}}}\n",
|
||||
"\"pentapod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"alternate_iterations\":%u,\"nonpositive\":%d,\"nonpositive_iterations\":%u,\"iteration_limit\":%d,\"iteration_limit_iterations\":%u,\"max_error\":%d,\"max_error_iterations\":%u,\"singular\":%d,\"singular_iterations\":%u,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f}}}\n",
|
||||
corexy_pose[0], corexy_pose[1], corexy_pose[2], corexy_joints[0], corexy_joints[1], corexy_joints[2], (unsigned long long)corexy_hash,
|
||||
rose_pose[0], rose_pose[1], rose_pose[2], rose_joints[0], rose_joints[1], rose_joints[2], rose_branch, (unsigned long long)rose_hash,
|
||||
max_pose[0], max_pose[1], max_pose[2], max_joints[0], max_joints[1], max_joints[2], max_joints[3], max_joints[4], max_joints[5], (unsigned long long)max_hash,
|
||||
@@ -399,7 +409,7 @@ static void emit_coupled_samples(void) {
|
||||
rdelta_alt_status, rdelta_alt_branch, rdelta_alt_pose.tran.x, rdelta_alt_pose.tran.y, rdelta_alt_pose.tran.z, rdelta_singular, rdelta_geometry_status, rdelta_branch, rdelta_iterations,
|
||||
tripod_alt_status, tripod_alt_branch, tripod_alt_pose.tran.x, tripod_alt_pose.tran.y, tripod_alt_pose.tran.z, tripod_no_solution, tripod_singular, tripod_limit_error, tripod_geometry_status, tripod_branch, delta_info.iterations,
|
||||
genhex_alt_status, genhex_alt_branch, genhex_alt_pose.tran.x, genhex_alt_pose.tran.y, genhex_alt_pose.tran.z, genhex_nonpositive, genhex_iter_limit_error, genhex_max_error_status, genhex_singular, genhex_limit_error, genhex_branch, genhex_iterations, genhex_residual,
|
||||
penta_alt_status, penta_alt_branch, penta_alt_pose.tran.x, penta_alt_pose.tran.y, penta_alt_pose.tran.z, penta_nonpositive, penta_iter_limit_error, penta_max_error_status, penta_singular, penta_limit_error, penta_branch, penta_iterations, penta_residual);
|
||||
penta_alt_status, penta_alt_branch, penta_alt_pose.tran.x, penta_alt_pose.tran.y, penta_alt_pose.tran.z, penta_alt_iterations, penta_nonpositive, penta_nonpositive_iterations, penta_iter_limit_error, penta_iter_limit_iterations, penta_max_error_status, penta_max_error_iterations, penta_singular, penta_singular_iterations, penta_limit_error, penta_branch, penta_iterations, penta_residual);
|
||||
}
|
||||
|
||||
int sim_kinematics_test_run(void) {
|
||||
@@ -580,6 +590,26 @@ int sim_kinematics_test_run(void) {
|
||||
config.dh_alpha[0] = NAN;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
|
||||
|
||||
/* Pentapod owns XYZAB only: yaw is not mapped, and LinuxCNC applies the
|
||||
* tool offset to both base and effector HAL geometry. */
|
||||
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
|
||||
config.penta_initial_pose[3] = 2.0 * 0.017453292519943295769;
|
||||
config.penta_initial_pose[4] = -3.0 * 0.017453292519943295769;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
pose = (sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0, .c = 17.0};
|
||||
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.c) < 1e-12);
|
||||
config.penta_tool_offset = 25.0;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, joints, 5) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
|
||||
assert(fabs(pose.tran.z - 300.0) < 1e-9);
|
||||
config.penta_tool_offset = NAN;
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
|
||||
config = config_for(SIM_KIN_PENTAPOD, "XYZABC");
|
||||
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
||||
|
||||
emit_coupled_samples();
|
||||
return 0;
|
||||
}
|
||||
|
||||
85
wasm-core/tests/sim_switchkins_test.c
Normal file
85
wasm-core/tests/sim_switchkins_test.c
Normal file
@@ -0,0 +1,85 @@
|
||||
#include "sim_kinematics.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_kinematics_config config_for(int type, const char *coordinates) {
|
||||
sim_kinematics_config config = {0};
|
||||
config.type = type;
|
||||
config.joint_count = (uint32_t)strlen(coordinates);
|
||||
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
|
||||
return config;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics identity = {0};
|
||||
sim_kinematics trt = {0};
|
||||
sim_kinematics_config config;
|
||||
sim_kinematics_switch switcher = {0};
|
||||
sim_kinematics_result_info info = {0};
|
||||
sim_posemath_pose initial = {0};
|
||||
sim_posemath_pose switched = {0};
|
||||
sim_posemath_pose returned = {0};
|
||||
double joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
|
||||
uint64_t initial_hash;
|
||||
uint64_t switched_hash;
|
||||
uint64_t returned_hash;
|
||||
uint64_t returned_repeat_hash;
|
||||
int forward_status;
|
||||
int select_to_trt;
|
||||
int select_to_identity;
|
||||
int invalid_mode;
|
||||
int missing_module;
|
||||
int register_missing;
|
||||
int begin_status;
|
||||
int interrupted_begin;
|
||||
int abort_status;
|
||||
int interrupted_commit;
|
||||
|
||||
config = config_for(SIM_KIN_TRIVKINS, "XYZAC");
|
||||
assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
|
||||
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
||||
assert(sim_kinematics_init(&trt, &config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_switch_register(&switcher, 0, &identity, 0) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_switch_register(&switcher, 1, &trt, 0) == SIM_KIN_OK);
|
||||
register_missing = sim_kinematics_switch_register(&switcher, 2, 0, 0);
|
||||
forward_status = sim_kinematics_switch_forward(&switcher, joints, 5, &initial, &info);
|
||||
assert(forward_status == SIM_KIN_OK);
|
||||
initial_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &initial);
|
||||
select_to_trt = sim_kinematics_switch_select(&switcher, 1, joints, 5, &switched, &info);
|
||||
assert(select_to_trt == SIM_KIN_OK);
|
||||
assert(fabs(switched.tran.x - initial.tran.x) < 1e-9);
|
||||
assert(fabs(switched.tran.y - initial.tran.y) < 1e-9);
|
||||
assert(fabs(switched.tran.z - initial.tran.z) < 1e-9);
|
||||
switched_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &switched);
|
||||
select_to_identity = sim_kinematics_switch_select(&switcher, 0, joints, 5, &returned, &info);
|
||||
assert(select_to_identity == SIM_KIN_OK);
|
||||
assert(fabs(returned.tran.x - initial.tran.x) < 1e-9);
|
||||
assert(fabs(returned.tran.y - initial.tran.y) < 1e-9);
|
||||
assert(fabs(returned.tran.z - initial.tran.z) < 1e-9);
|
||||
returned_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
|
||||
returned_repeat_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
|
||||
invalid_mode = sim_kinematics_switch_select(&switcher, 3, joints, 5, &returned, &info);
|
||||
missing_module = sim_kinematics_switch_select(&switcher, 2, joints, 5, &returned, &info);
|
||||
begin_status = sim_kinematics_switch_begin(&switcher, 1);
|
||||
interrupted_begin = sim_kinematics_switch_begin(&switcher, 0);
|
||||
abort_status = sim_kinematics_switch_abort(&switcher);
|
||||
assert(sim_kinematics_switch_begin(&switcher, 1) == SIM_KIN_OK);
|
||||
abort_status = sim_kinematics_switch_abort(&switcher);
|
||||
interrupted_commit = sim_kinematics_switch_commit(&switcher, joints, 5, &returned, &info);
|
||||
assert(sim_kinematics_switch_active(&switcher) == 0);
|
||||
|
||||
printf("{\"schema\":\"switchkins-samples.v1\",\"registered\":[%u,%u,%u],\"initial_mode\":0,\"active_mode\":%d,\"forward_status\":%d,\"switch_to_trt\":%d,\"switch_to_identity\":%d,\"initial_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"switched_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"returned_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"initial_hash\":%llu,\"switched_hash\":%llu,\"returned_hash\":%llu,\"returned_repeat_hash\":%llu,\"invalid_mode\":%d,\"missing_module\":%d,\"register_missing\":%d,\"begin_status\":%d,\"interrupted_begin\":%d,\"abort_status\":%d,\"interrupted_commit\":%d,\"generation\":%u,\"switch_count\":%u,\"abort_count\":%u}\n",
|
||||
switcher.registered[0], switcher.registered[1], switcher.registered[2], switcher.active_mode,
|
||||
forward_status, select_to_trt, select_to_identity,
|
||||
initial.tran.x, initial.tran.y, initial.tran.z, initial.a, initial.b, initial.c,
|
||||
switched.tran.x, switched.tran.y, switched.tran.z, switched.a, switched.b, switched.c,
|
||||
returned.tran.x, returned.tran.y, returned.tran.z, returned.a, returned.b, returned.c,
|
||||
(unsigned long long)initial_hash, (unsigned long long)switched_hash, (unsigned long long)returned_hash, (unsigned long long)returned_repeat_hash,
|
||||
invalid_mode, missing_module, register_missing, begin_status, interrupted_begin, abort_status,
|
||||
interrupted_commit, switcher.generation, switcher.switch_count, switcher.abort_count);
|
||||
return 0;
|
||||
}
|
||||
126
wasm-core/tests/sim_userkins_test.c
Normal file
126
wasm-core/tests/sim_userkins_test.c
Normal file
@@ -0,0 +1,126 @@
|
||||
#include "sim_kinematics.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static sim_kinematics_config config_for(const char *coordinates) {
|
||||
sim_kinematics_config config = {0};
|
||||
config.joint_count = (uint32_t)strlen(coordinates);
|
||||
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
|
||||
return config;
|
||||
}
|
||||
|
||||
static sim_kinematics_user_descriptor identity_descriptor(void) {
|
||||
sim_kinematics_user_descriptor descriptor = {0};
|
||||
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
|
||||
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
|
||||
SIM_KIN_USER_CAP_STATIC;
|
||||
descriptor.implementation = SIM_KIN_USER_IDENTITY;
|
||||
strcpy(descriptor.name, "userk.identity");
|
||||
descriptor.config = config_for("XYZAC");
|
||||
return descriptor;
|
||||
}
|
||||
|
||||
static sim_kinematics_user_descriptor genser_descriptor(void) {
|
||||
sim_kinematics_user_descriptor descriptor = {0};
|
||||
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
|
||||
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
|
||||
SIM_KIN_USER_CAP_STATIC;
|
||||
descriptor.implementation = SIM_KIN_USER_GENSER;
|
||||
strcpy(descriptor.name, "ugenser.static");
|
||||
descriptor.config = config_for("XYZABC");
|
||||
descriptor.config.dh_joint_count = 6;
|
||||
descriptor.config.max_iterations = 200;
|
||||
descriptor.config.convergence_tolerance = 1e-7;
|
||||
descriptor.config.dh_a[1] = 85.0;
|
||||
descriptor.config.dh_a[2] = 380.0;
|
||||
descriptor.config.dh_a[3] = 100.0;
|
||||
descriptor.config.dh_alpha[1] = -1.5707963267948966;
|
||||
descriptor.config.dh_alpha[3] = -1.5707963267948966;
|
||||
descriptor.config.dh_alpha[4] = 1.5707963267948966;
|
||||
descriptor.config.dh_alpha[5] = -1.5707963267948966;
|
||||
descriptor.config.dh_d[0] = 350.0;
|
||||
descriptor.config.dh_d[3] = 425.0;
|
||||
descriptor.config.dh_d[5] = 235.0;
|
||||
return descriptor;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
sim_kinematics_user identity = {0};
|
||||
sim_kinematics_user genser = {0};
|
||||
sim_kinematics_user uninitialized = {0};
|
||||
sim_kinematics_user_descriptor descriptor = identity_descriptor();
|
||||
sim_kinematics_result_info info = {0};
|
||||
sim_kinematics base = {0};
|
||||
sim_kinematics_config base_config = config_for("XYZAC");
|
||||
sim_kinematics_switch switcher = {0};
|
||||
sim_posemath_pose identity_pose = {0};
|
||||
sim_posemath_pose genser_pose = {0};
|
||||
double identity_joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
|
||||
double identity_roundtrip[5] = {0};
|
||||
double genser_joints[6] = {0.0, 10.0, -20.0, 30.0, 15.0, -10.0};
|
||||
double genser_roundtrip[6] = {0};
|
||||
uint64_t identity_hash;
|
||||
uint64_t identity_repeat_hash;
|
||||
uint64_t genser_hash;
|
||||
uint64_t genser_repeat_hash;
|
||||
int switch_to_user;
|
||||
int abi_error;
|
||||
int capability_error;
|
||||
int path_name_error;
|
||||
int missing_implementation;
|
||||
int dynamic_path_error;
|
||||
int uninitialized_error;
|
||||
|
||||
assert(sim_kinematics_user_init(&identity, &descriptor) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_forward(&identity, identity_joints, 5, &identity_pose, &info) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_inverse(&identity, &identity_pose, identity_roundtrip, 5, &info) == SIM_KIN_OK);
|
||||
assert(memcmp(identity_joints, identity_roundtrip, sizeof(identity_joints)) == 0);
|
||||
identity_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
|
||||
identity_repeat_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
|
||||
|
||||
base_config.type = SIM_KIN_TRIVKINS;
|
||||
assert(sim_kinematics_init(&base, &base_config) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_switch_register(&switcher, 0, &base, 0) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_bind_switch(&identity, &switcher, 2, 0) == SIM_KIN_OK);
|
||||
switch_to_user = sim_kinematics_switch_select(&switcher, 2, identity_roundtrip, 5, &identity_pose, &info);
|
||||
assert(switch_to_user == SIM_KIN_OK && sim_kinematics_switch_active(&switcher) == 2);
|
||||
|
||||
descriptor = genser_descriptor();
|
||||
assert(sim_kinematics_user_init(&genser, &descriptor) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_forward(&genser, genser_joints, 6, &genser_pose, &info) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_inverse(&genser, &genser_pose, genser_roundtrip, 6, &info) == SIM_KIN_OK);
|
||||
assert(sim_kinematics_user_forward(&genser, genser_roundtrip, 6, &genser_pose, &info) == SIM_KIN_OK);
|
||||
genser_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
|
||||
genser_repeat_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
|
||||
|
||||
descriptor = identity_descriptor();
|
||||
descriptor.abi_version += 1;
|
||||
abi_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
||||
descriptor = identity_descriptor();
|
||||
descriptor.capabilities &= ~SIM_KIN_USER_CAP_STATIC;
|
||||
capability_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
||||
descriptor = identity_descriptor();
|
||||
strcpy(descriptor.name, "plugins/user.so");
|
||||
path_name_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
||||
descriptor = identity_descriptor();
|
||||
descriptor.implementation = 99;
|
||||
missing_implementation = sim_kinematics_user_init(&uninitialized, &descriptor);
|
||||
dynamic_path_error = sim_kinematics_user_reject_dynamic("/tmp/userkins.so");
|
||||
uninitialized_error = sim_kinematics_user_forward(&uninitialized, identity_joints, 5, &identity_pose, &info);
|
||||
|
||||
printf("{\"schema\":\"userkins-samples.v1\",\"abi_version\":%u,\"capabilities\":%u,\"identity\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"state_hash\":%llu,\"repeat_hash\":%llu},\"ugenser\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"iterations\":%u,\"residual\":%.12f,\"state_hash\":%llu,\"repeat_hash\":%llu},\"switch_to_user\":%d,\"active_mode\":%d,\"abi_error\":%d,\"capability_error\":%d,\"path_name_error\":%d,\"missing_implementation\":%d,\"dynamic_path_error\":%d,\"uninitialized_error\":%d}\n",
|
||||
SIM_KIN_USER_ABI_VERSION, identity.descriptor.capabilities,
|
||||
identity_pose.tran.x, identity_pose.tran.y, identity_pose.tran.z, identity_pose.a, identity_pose.c,
|
||||
identity_roundtrip[0], identity_roundtrip[1], identity_roundtrip[2], identity_roundtrip[3], identity_roundtrip[4],
|
||||
identity.forward_calls, identity.inverse_calls, (unsigned long long)identity_hash, (unsigned long long)identity_repeat_hash,
|
||||
genser_pose.tran.x, genser_pose.tran.y, genser_pose.tran.z, genser_pose.a, genser_pose.b, genser_pose.c,
|
||||
genser.forward_calls, genser.inverse_calls, info.iterations, info.residual,
|
||||
(unsigned long long)genser_hash, (unsigned long long)genser_repeat_hash,
|
||||
switch_to_user, sim_kinematics_switch_active(&switcher), abi_error, capability_error,
|
||||
path_name_error, missing_implementation, dynamic_path_error, uninitialized_error);
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user