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

138
wasm-core/src/sim_kinematics_home_limit.c Normal file
View File

@@ -0,0 +1,138 @@
#include "sim_kinematics_home_limit.h"
#include <math.h>
#include <string.h>
static uint64_t home_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_home_limit_state *state) {
memset(state, 0, sizeof(*state));
state->last_error = SIM_KIN_HOME_OK;
state->limit_error = SIM_KIN_OK;
state->kinematics_error = SIM_KIN_OK;
}
int sim_kinematics_home_limit_init(sim_kinematics_home_limit *home_limit,
sim_kinematics *kinematics,
uint32_t joint_count) {
uint32_t configured_count;
if (home_limit == 0 || kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (!kinematics->configured) return SIM_KIN_HOME_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_HOME_ERR_CONFIGURATION;
memset(home_limit, 0, sizeof(*home_limit));
home_limit->kinematics = kinematics;
home_limit->state.joint_count = joint_count;
state_zero(&home_limit->state);
home_limit->state.joint_count = joint_count;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_reset(sim_kinematics_home_limit *home_limit) {
uint32_t joint_count;
if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
joint_count = home_limit->state.joint_count;
state_zero(&home_limit->state);
home_limit->state.joint_count = joint_count;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_home(sim_kinematics_home_limit *home_limit,
uint32_t home_sequence) {
sim_posemath_pose pose;
double joints[SIM_KIN_MAX_JOINTS] = {0};
int result;
if (home_limit == 0 || home_limit->kinematics == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (home_sequence > 0xffffu) return SIM_KIN_HOME_ERR_SEQUENCE;
result = sim_kinematics_home(home_limit->kinematics, joints,
home_limit->state.joint_count, &pose);
if (result != SIM_KIN_OK) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
home_limit->state.kinematics_error = result;
return home_limit->state.last_error;
}
memcpy(home_limit->state.joints, joints, sizeof(double) * home_limit->state.joint_count);
home_limit->state.pose = pose;
home_limit->state.home_sequence = home_sequence;
home_limit->state.homed = 1;
home_limit->state.tick += 1;
home_limit->state.last_error = SIM_KIN_HOME_OK;
home_limit->state.limit_error = SIM_KIN_OK;
home_limit->state.kinematics_error = SIM_KIN_OK;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_check(sim_kinematics_home_limit *home_limit,
const double *joints,
uint32_t joint_count) {
sim_posemath_pose pose;
uint32_t index;
int result;
if (home_limit == 0 || joints == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
if (!home_limit->state.homed) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_UNHOMED;
return home_limit->state.last_error;
}
if (joint_count != home_limit->state.joint_count) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_CONFIGURATION;
return home_limit->state.last_error;
}
for (index = 0; index < joint_count; ++index) {
if (!isfinite(joints[index])) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_NONFINITE;
return home_limit->state.last_error;
}
if (home_limit->kinematics->config.limit_enabled[index] &&
(joints[index] < home_limit->kinematics->config.joint_min[index] ||
joints[index] > home_limit->kinematics->config.joint_max[index])) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_LIMIT;
home_limit->state.limit_error = SIM_KIN_ERR_RANGE;
return home_limit->state.last_error;
}
}
result = sim_kinematics_forward(home_limit->kinematics, joints, joint_count, &pose);
if (result != SIM_KIN_OK) {
home_limit->state.last_error = SIM_KIN_HOME_ERR_FORWARD;
home_limit->state.kinematics_error = result;
return home_limit->state.last_error;
}
memcpy(home_limit->state.joints, joints, sizeof(double) * joint_count);
home_limit->state.pose = pose;
home_limit->state.tick += 1;
home_limit->state.last_error = SIM_KIN_HOME_OK;
home_limit->state.limit_error = SIM_KIN_OK;
home_limit->state.kinematics_error = SIM_KIN_OK;
return SIM_KIN_HOME_OK;
}
int sim_kinematics_home_limit_read(const sim_kinematics_home_limit *home_limit,
sim_kinematics_home_limit_state *state) {
if (home_limit == 0 || state == 0) return SIM_KIN_HOME_ERR_ARGUMENT;
*state = home_limit->state;
return SIM_KIN_HOME_OK;
}
uint64_t sim_kinematics_home_limit_state_hash(const sim_kinematics_home_limit *home_limit) {
sim_kinematics_home_limit_state state;
unsigned char bytes[sizeof(state)];
uint32_t index;
if (home_limit == 0) return 0;
state = home_limit->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, &state, sizeof(state));
return home_fnv1a64(bytes, sizeof(bytes));
}