Extend simulation runtime coverage and kinematics support

wasm-core/tests/sim_kinematics_motion_test.c

@@ -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;
+}