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

185
wasm-core/src/sim_switchkins.c Normal file
View File

@@ -0,0 +1,185 @@
#include "sim_kinematics.h"
#include <math.h>
#include <string.h>
static uint64_t switch_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_mode(int mode) {
return mode >= 0 && mode < (int)SIM_KIN_SWITCH_MAX_TYPES;
}
static void switch_info(sim_kinematics_result_info *info) {
if (info != 0) memset(info, 0, sizeof(*info));
}
int sim_kinematics_switch_init(sim_kinematics_switch *switcher, int default_mode,
uint8_t fwd_iterates_mask) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(default_mode)) return SIM_KIN_ERR_INVALID_MODE;
memset(switcher, 0, sizeof(*switcher));
switcher->active_mode = default_mode;
switcher->pending_mode = default_mode;
switcher->fwd_iterates_mask = fwd_iterates_mask & 0x07u;
return SIM_KIN_OK;
}
int sim_kinematics_switch_register(sim_kinematics_switch *switcher, int mode,
sim_kinematics *module, uint8_t fwd_iterates) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (module == 0 || !module->configured) return SIM_KIN_ERR_MISSING_MODULE;
switcher->modules[mode] = module;
switcher->registered[mode] = 1;
if (fwd_iterates) switcher->fwd_iterates_mask |= (uint8_t)(1u << mode);
else switcher->fwd_iterates_mask &= (uint8_t)~(1u << mode);
return SIM_KIN_OK;
}
int sim_kinematics_switch_begin(sim_kinematics_switch *switcher, int mode) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
switcher->pending_mode = mode;
switcher->switching = (uint8_t)(mode != switcher->active_mode);
return SIM_KIN_OK;
}
int sim_kinematics_switch_abort(sim_kinematics_switch *switcher) {
if (switcher == 0) return SIM_KIN_ERR_ARGUMENT;
if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
switcher->pending_mode = switcher->active_mode;
switcher->switching = 0;
switcher->abort_count += 1;
return SIM_KIN_OK;
}
int sim_kinematics_switch_commit(sim_kinematics_switch *switcher, double *joints,
uint32_t joint_count, sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_kinematics *target;
double remapped[SIM_KIN_MAX_JOINTS] = {0};
sim_kinematics_result_info local_info;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
target = switcher->modules[switcher->pending_mode];
if (target == 0 || !switcher->registered[switcher->pending_mode]) {
switcher->switching = 0;
switcher->pending_mode = switcher->active_mode;
return SIM_KIN_ERR_MISSING_MODULE;
}
if (joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_ERR_CONFIGURATION;
result = sim_kinematics_inverse_ex(target, pose, joint_count, 0, remapped,
&local_info);
if (result != SIM_KIN_OK) {
switcher->switching = 0;
switcher->pending_mode = switcher->active_mode;
if (info != 0) *info = local_info;
return result;
}
memcpy(joints, remapped, sizeof(double) * joint_count);
switcher->active_mode = switcher->pending_mode;
switcher->pending_mode = switcher->active_mode;
switcher->switching = 0;
switcher->generation += 1;
switcher->switch_count += 1;
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
if (info != 0) *info = local_info;
return SIM_KIN_OK;
}
int sim_kinematics_switch_forward(sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_kinematics *module;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
module = switcher->modules[switcher->active_mode];
if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
result = sim_kinematics_forward_ex(module, joints, joint_count, 0, pose, info);
if (result == SIM_KIN_OK) {
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
}
return result;
}
int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
const sim_posemath_pose *pose, double *joints,
uint32_t joint_count,
sim_kinematics_result_info *info) {
sim_kinematics *module;
int result;
switch_info(info);
if (switcher == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!valid_mode(switcher->active_mode)) return SIM_KIN_ERR_INVALID_MODE;
module = switcher->modules[switcher->active_mode];
if (module == 0 || !switcher->registered[switcher->active_mode]) return SIM_KIN_ERR_MISSING_MODULE;
result = sim_kinematics_inverse_ex(module, pose, joint_count, 0, joints, info);
if (result == SIM_KIN_OK) {
switcher->last_pose[switcher->active_mode] = *pose;
switcher->have_last_pose[switcher->active_mode] = 1;
}
return result;
}
int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
double *joints, uint32_t joint_count,
sim_posemath_pose *pose,
sim_kinematics_result_info *info) {
sim_posemath_pose current_pose;
int result;
switch_info(info);
if (switcher == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
if (!valid_mode(mode)) return SIM_KIN_ERR_INVALID_MODE;
if (switcher->switching) return SIM_KIN_ERR_SWITCH_INTERRUPTED;
if (!switcher->registered[mode] || switcher->modules[mode] == 0) return SIM_KIN_ERR_MISSING_MODULE;
if (mode == switcher->active_mode) return sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
result = sim_kinematics_switch_forward(switcher, joints, joint_count, &current_pose, info);
if (result != SIM_KIN_OK) return result;
result = sim_kinematics_switch_begin(switcher, mode);
if (result != SIM_KIN_OK) return result;
result = sim_kinematics_switch_commit(switcher, joints, joint_count, &current_pose, info);
if (result == SIM_KIN_OK) *pose = current_pose;
return result;
}
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher) {
return switcher == 0 ? SIM_KIN_ERR_ARGUMENT : switcher->active_mode;
}
uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
const double *joints, uint32_t joint_count,
const sim_posemath_pose *pose) {
unsigned char bytes[256];
size_t offset = 0;
uint64_t module_hash;
if (switcher == 0 || joints == 0 || pose == 0 || joint_count > SIM_KIN_MAX_JOINTS) return 0;
if (!valid_mode(switcher->active_mode) || switcher->modules[switcher->active_mode] == 0) return 0;
module_hash = sim_kinematics_state_hash(switcher->modules[switcher->active_mode], joints, joint_count, pose);
memcpy(bytes + offset, &switcher->active_mode, sizeof(switcher->active_mode)); offset += sizeof(switcher->active_mode);
memcpy(bytes + offset, &switcher->pending_mode, sizeof(switcher->pending_mode)); offset += sizeof(switcher->pending_mode);
memcpy(bytes + offset, &switcher->switching, sizeof(switcher->switching)); offset += sizeof(switcher->switching);
memcpy(bytes + offset, &switcher->generation, sizeof(switcher->generation)); offset += sizeof(switcher->generation);
memcpy(bytes + offset, &switcher->switch_count, sizeof(switcher->switch_count)); offset += sizeof(switcher->switch_count);
memcpy(bytes + offset, &switcher->abort_count, sizeof(switcher->abort_count)); offset += sizeof(switcher->abort_count);
memcpy(bytes + offset, &module_hash, sizeof(module_hash)); offset += sizeof(module_hash);
return switch_fnv1a64(bytes, offset);
}