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

109
wasm-core/src/sim_kinematics_feedback.c Normal file
View File

@@ -0,0 +1,109 @@
#include "sim_kinematics_feedback.h"
#include <math.h>
#include <string.h>
static uint64_t feedback_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 void state_zero(sim_kinematics_feedback_state *state) {
memset(state, 0, sizeof(*state));
state->last_error = SIM_KIN_FEEDBACK_OK;
state->kinematics_error = SIM_KIN_OK;
}
int sim_kinematics_feedback_init(sim_kinematics_feedback *feedback,
sim_kinematics *kinematics,
uint32_t joint_count) {
uint32_t configured_count;
if (feedback == 0 || kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (!kinematics->configured) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
configured_count = kinematics->config.kinematics_joint_count == 0 ?
kinematics->config.joint_count : kinematics->config.kinematics_joint_count;
if (joint_count == 0 || joint_count != configured_count || joint_count > SIM_KIN_MAX_JOINTS)
return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
memset(feedback, 0, sizeof(*feedback));
feedback->kinematics = kinematics;
feedback->state.joint_count = joint_count;
state_zero(&feedback->state);
feedback->state.joint_count = joint_count;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_reset(sim_kinematics_feedback *feedback) {
uint32_t joint_count;
if (feedback == 0 || feedback->kinematics == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
joint_count = feedback->state.joint_count;
state_zero(&feedback->state);
feedback->state.joint_count = joint_count;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_update(sim_kinematics_feedback *feedback,
const double *joints, uint32_t joint_count,
uint64_t inverse_flags,
sim_kinematics_result_info *info) {
sim_posemath_pose pose;
sim_kinematics_result_info local_info;
uint32_t index;
int result;
if (feedback == 0 || feedback->kinematics == 0 || joints == 0)
return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (joint_count != feedback->state.joint_count) return SIM_KIN_FEEDBACK_ERR_CONFIGURATION;
for (index = 0; index < joint_count; ++index)
if (!isfinite(joints[index])) {
feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_NONFINITE;
feedback->state.kinematics_error = SIM_KIN_ERR_NONFINITE;
return feedback->state.last_error;
}
memset(&pose, 0, sizeof(pose));
result = sim_kinematics_forward_ex(feedback->kinematics, joints, joint_count,
inverse_flags, &pose, &local_info);
if (result != SIM_KIN_OK) {
feedback->state.last_error = SIM_KIN_FEEDBACK_ERR_FORWARD;
feedback->state.kinematics_error = result;
if (info != 0) *info = local_info;
return feedback->state.last_error;
}
feedback->state.pose = pose;
memcpy(feedback->state.joints, joints, sizeof(double) * joint_count);
feedback->state.forward_flags = local_info.forward_flags;
feedback->state.inverse_flags = local_info.inverse_flags;
feedback->state.tick += 1;
feedback->state.valid = 1;
feedback->state.last_error = SIM_KIN_FEEDBACK_OK;
feedback->state.kinematics_error = SIM_KIN_OK;
if (info != 0) *info = local_info;
return SIM_KIN_FEEDBACK_OK;
}
int sim_kinematics_feedback_read(const sim_kinematics_feedback *feedback,
sim_kinematics_feedback_state *state) {
if (feedback == 0 || state == 0) return SIM_KIN_FEEDBACK_ERR_ARGUMENT;
if (!feedback->state.valid) return SIM_KIN_FEEDBACK_ERR_STATE;
*state = feedback->state;
return SIM_KIN_FEEDBACK_OK;
}
uint64_t sim_kinematics_feedback_state_hash(const sim_kinematics_feedback *feedback) {
unsigned char bytes[sizeof(sim_kinematics_feedback_state)];
sim_kinematics_feedback_state state;
size_t offset = 0;
uint32_t index;
if (feedback == 0) return 0;
state = feedback->state;
for (index = 0; index < 9; ++index) {
double *pose_value = &state.pose.tran.x;
pose_value[index] = isfinite(pose_value[index]) ? nearbyint(pose_value[index] * 1000000000.0) / 1000000000.0 : pose_value[index];
if (index < state.joint_count) state.joints[index] = isfinite(state.joints[index]) ? nearbyint(state.joints[index] * 1000000000.0) / 1000000000.0 : state.joints[index];
}
memcpy(bytes + offset, &state, sizeof(state)); offset += sizeof(state);
return feedback_fnv1a64(bytes, offset);
}