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

36
wasm-core/src/sim_kinematics.c
View File

@@ -19,6 +19,14 @@ static double hash_normalize(double value) {
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
}
/* The five-leg Newton solve can differ by a few ulps between native and
* wasm32 math libraries. Keep the public pose/info boundary deterministic
* without changing the convergence test itself. */
static double pentapod_canonical(double value) {
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
}
static int finite_values(const double *values, size_t count) {
size_t index;
if (values == 0) return 0;
@@ -168,6 +176,7 @@ static int valid_config(const sim_kinematics_config *config) {
}
if (config->type == SIM_KIN_PENTAPOD) {
if (kinematics_count != SIM_KIN_PENTA_STRUTS || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B')) return SIM_KIN_ERR_CONFIGURATION;
if (!isfinite(config->penta_tool_offset)) return SIM_KIN_ERR_NONFINITE;
if (config->penta_iteration_limit > 100000u || config->penta_convergence_tolerance < 0.0 || !isfinite(config->penta_convergence_tolerance) || config->penta_max_error < 0.0 || !isfinite(config->penta_max_error) || !finite_values(config->penta_initial_pose, 5)) return SIM_KIN_ERR_CONFIGURATION;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (!finite_values(config->penta_base[index], 3) || !isfinite(config->penta_effector_radius[index]) || !isfinite(config->penta_effector_z[index]) || ((config->penta_effector_radius[index] < 0.0) || (config->penta_effector_radius[index] == 0.0 && config->penta_effector_z[index] != 0.0))) return SIM_KIN_ERR_CONFIGURATION;
}
@@ -269,7 +278,10 @@ int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config
static const double effector_z[SIM_KIN_PENTA_STRUTS] = {87.0, 114.5, 140.5, 232.5, 258.5};
int all_zero = 1;
size_t index;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0) all_zero = 0;
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0 ||
kinematics->config.penta_effector_radius[index] != 0.0 || kinematics->config.penta_effector_z[index] != 0.0) all_zero = 0;
}
if (all_zero) { memcpy(kinematics->config.penta_base, base, sizeof(base)); memcpy(kinematics->config.penta_effector_radius, radius, sizeof(radius)); memcpy(kinematics->config.penta_effector_z, effector_z, sizeof(effector_z)); }
if (kinematics->config.penta_iteration_limit == 0) kinematics->config.penta_iteration_limit = 120;
if (kinematics->config.penta_convergence_tolerance == 0.0) kinematics->config.penta_convergence_tolerance = 1e-9;
@@ -808,7 +820,10 @@ static int pentapod_lengths(const sim_kinematics *k, const double q[5], double l
PmRotationMatrix rotation, inverse;
if (pmRpyMatConvert(&rpy, &rotation) != 0 || pmMatInv(&rotation, &inverse) != 0) return SIM_KIN_ERR_SINGULAR;
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] - q[2]};
/* pentakins_read_hal_pins applies tool-offset to both ends of each
* strut; preserve that world-frame translation before rotating into
* the effector frame. */
PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] + k->config.penta_tool_offset - q[2]};
PmCartesian local;
double radial;
pmMatCartMult(&inverse, &base, &local);
@@ -833,6 +848,14 @@ static int pentapod_forward(const sim_kinematics *k, const double *joints, uint6
double lengths[SIM_KIN_PENTA_STRUTS];
double residual = 0.0;
uint32_t iteration;
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
/* LinuxCNC's pentakins rejects non-positive strut lengths before
* entering Newton iteration. */
if (joints[index] <= 0.0) {
if (info != 0) { info->iterations = 0; info->residual = 0.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_ERR_RANGE;
}
}
for (size_t index = 0; index < 5; ++index) q[index] = k->config.penta_initial_pose[index];
if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
for (iteration = 1; iteration <= k->config.penta_iteration_limit; ++iteration) {
@@ -853,11 +876,14 @@ static int pentapod_forward(const sim_kinematics *k, const double *joints, uint6
}
}
if (residual <= k->config.penta_convergence_tolerance) {
pose->tran.x = q[0]; pose->tran.y = q[1]; pose->tran.z = q[2]; pose->a = q[3] * RAD_TO_DEG; pose->b = q[4] * RAD_TO_DEG; pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
pose->tran.x = pentapod_canonical(q[0]); pose->tran.y = pentapod_canonical(q[1]); pose->tran.z = pentapod_canonical(q[2]); pose->a = pentapod_canonical(q[3] * RAD_TO_DEG); pose->b = pentapod_canonical(q[4] * RAD_TO_DEG); pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
if (info != 0) { info->iterations = iteration; info->residual = nearbyint(residual * 1000000000.0) / 1000000000.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_OK;
}
if (residual > k->config.penta_max_error) return SIM_KIN_ERR_RANGE;
if (residual > k->config.penta_max_error) {
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
return SIM_KIN_ERR_RANGE;
}
if (solve_linear5(jacobian, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
for (int column = 0; column < 5; ++column) q[column] -= delta[column];
}