217 lines
10 KiB
C
217 lines
10 KiB
C
#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_inverse_mode(const sim_kinematics_switch *switcher, int mode,
|
|
const sim_posemath_pose *pose, double *joints,
|
|
uint32_t joint_count,
|
|
sim_kinematics_result_info *info) {
|
|
if (switcher == 0 || pose == 0 || joints == 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;
|
|
return sim_kinematics_inverse_ex(switcher->modules[mode], pose, joint_count, 0, joints, info);
|
|
}
|
|
|
|
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, ¤t_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, ¤t_pose, info);
|
|
if (result == SIM_KIN_OK) *pose = current_pose;
|
|
return result;
|
|
}
|
|
|
|
int sim_kinematics_switch_select_physical(sim_kinematics_switch *switcher, int mode,
|
|
const double *joints, uint32_t joint_count,
|
|
sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info) {
|
|
int result;
|
|
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;
|
|
switcher->active_mode = mode;
|
|
switcher->pending_mode = mode;
|
|
switcher->switching = 0;
|
|
switcher->generation += 1;
|
|
switcher->switch_count += 1;
|
|
result = sim_kinematics_switch_forward(switcher, joints, joint_count, pose, info);
|
|
if (result != SIM_KIN_OK) return result;
|
|
switcher->last_pose[mode] = *pose;
|
|
switcher->have_last_pose[mode] = 1;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
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);
|
|
}
|