110 lines
4.7 KiB
C
110 lines
4.7 KiB
C
#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);
|
|
}
|