66 lines
3.3 KiB
C
66 lines
3.3 KiB
C
#include "sim_kinematics_feedback.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static sim_kinematics_config config_for(int type, const char *coordinates) {
|
|
sim_kinematics_config config = {0};
|
|
config.type = type;
|
|
config.joint_count = (uint32_t)strlen(coordinates);
|
|
strcpy(config.coordinates, coordinates);
|
|
return config;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_kinematics kinematics = {0};
|
|
sim_kinematics singular = {0};
|
|
sim_kinematics_feedback feedback = {0};
|
|
sim_kinematics_feedback singular_feedback = {0};
|
|
sim_kinematics_feedback_state state = {0};
|
|
sim_kinematics_result_info info = {0};
|
|
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
|
double joints[3] = {1.0, 2.0, 3.0};
|
|
double bad_joints[3] = {1.0, NAN, 3.0};
|
|
double singular_joints[5] = {0, 0, 0, 90, 0};
|
|
uint64_t hash;
|
|
uint64_t repeat_hash;
|
|
uint32_t valid_tick;
|
|
int update_status;
|
|
int bad_status;
|
|
int singular_status;
|
|
int stale_read_status;
|
|
int count_status;
|
|
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_feedback_init(&feedback, &kinematics, 3) == SIM_KIN_FEEDBACK_OK);
|
|
stale_read_status = sim_kinematics_feedback_read(&feedback, &state);
|
|
update_status = sim_kinematics_feedback_update(&feedback, joints, 3, 0, &info);
|
|
assert(update_status == SIM_KIN_FEEDBACK_OK);
|
|
assert(sim_kinematics_feedback_read(&feedback, &state) == SIM_KIN_FEEDBACK_OK);
|
|
valid_tick = state.tick;
|
|
hash = sim_kinematics_feedback_state_hash(&feedback);
|
|
repeat_hash = sim_kinematics_feedback_state_hash(&feedback);
|
|
assert(state.valid && state.tick == 1 && fabs(state.pose.tran.x - 1.0) < 1e-12);
|
|
bad_status = sim_kinematics_feedback_update(&feedback, bad_joints, 3, 0, &info);
|
|
assert(bad_status == SIM_KIN_FEEDBACK_ERR_NONFINITE && feedback.state.tick == valid_tick);
|
|
count_status = sim_kinematics_feedback_update(&feedback, joints, 2, 0, &info);
|
|
assert(count_status == SIM_KIN_FEEDBACK_ERR_CONFIGURATION);
|
|
|
|
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
|
config.mode = SIM_KIN_MODE_INVERSE_ONLY;
|
|
assert(sim_kinematics_init(&singular, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_feedback_init(&singular_feedback, &singular, 5) == SIM_KIN_FEEDBACK_OK);
|
|
singular_status = sim_kinematics_feedback_update(&singular_feedback, singular_joints, 5, 0, &info);
|
|
assert(singular_status == SIM_KIN_FEEDBACK_ERR_FORWARD && singular_feedback.state.valid == 0 && singular_feedback.state.kinematics_error == SIM_KIN_ERR_UNSUPPORTED);
|
|
|
|
printf("{\"schema\":\"kinematics-feedback-samples.v1\",\"nominal\":{\"status\":%d,\"tick\":%u,\"valid\":%u,\"pose\":[%.9f,%.9f,%.9f],\"flags\":[%llu,%llu],\"state_hash\":%llu,\"repeat_hash\":%llu},\"stale_read\":%d,\"nonfinite\":%d,\"count_error\":%d,\"singular\":{\"status\":%d,\"kinematics_error\":%d,\"valid\":%u}}\n",
|
|
update_status, state.tick, state.valid, state.pose.tran.x, state.pose.tran.y, state.pose.tran.z,
|
|
(unsigned long long)state.forward_flags, (unsigned long long)state.inverse_flags,
|
|
(unsigned long long)hash, (unsigned long long)repeat_hash, stale_read_status,
|
|
bad_status, count_status, singular_status, singular_feedback.state.kinematics_error,
|
|
singular_feedback.state.valid);
|
|
return 0;
|
|
}
|