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

Extend simulation runtime coverage and kinematics support

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

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

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