108 lines
5.4 KiB
C
108 lines
5.4 KiB
C
#include "sim_kinematics_motion.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static sim_posemath_segment line(double x0, double x1, double velocity) {
|
|
sim_posemath_segment segment = {
|
|
{{x0, 0, 0}, 0, 0, 0, 0, 0, 0},
|
|
{{x1, 0, 0}, 0, 0, 0, 0, 0, 0},
|
|
{0, 0, 0}, {0, 0, 1}, velocity, 10.0, SIM_POSEMATH_LINEAR, 0
|
|
};
|
|
return segment;
|
|
}
|
|
|
|
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_kins = {0};
|
|
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
|
sim_kinematics_motion adapter = {0};
|
|
sim_kinematics_motion singular_adapter = {0};
|
|
sim_kinematics_motion_sample sample = {0};
|
|
sim_posemath_segment segment = line(0.0, 1.0, 1.0);
|
|
uint32_t id = 0;
|
|
uint32_t pause_tick;
|
|
uint64_t nominal_hash;
|
|
uint64_t nominal_repeat_hash;
|
|
uint64_t full_planner_before;
|
|
uint64_t full_planner_after;
|
|
uint64_t full_hash;
|
|
uint64_t singular_planner_before;
|
|
uint64_t singular_planner_after;
|
|
int pause_status;
|
|
int resume_status;
|
|
int full_status;
|
|
int singular_status;
|
|
int singular_kinematics_error;
|
|
int nonfinite_status;
|
|
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_motion_init(&adapter, &kinematics, 0.1, 10.0, 10.0) == SIM_KIN_MOTION_OK);
|
|
assert(sim_kinematics_motion_enqueue(&adapter, &segment, &id) == SIM_KIN_MOTION_OK && id == 1);
|
|
assert(sim_kinematics_motion_pause(&adapter, 1) == SIM_KIN_MOTION_OK);
|
|
pause_tick = adapter.planner.tick;
|
|
pause_status = sim_kinematics_motion_step(&adapter);
|
|
assert(pause_status == SIM_KIN_MOTION_ERR_STATE && adapter.planner.tick == pause_tick && adapter.count == 0);
|
|
assert(sim_kinematics_motion_pause(&adapter, 0) == SIM_KIN_MOTION_OK);
|
|
resume_status = sim_kinematics_motion_step(&adapter);
|
|
assert(resume_status == SIM_KIN_MOTION_OK);
|
|
assert(sim_kinematics_motion_peek(&adapter, &sample) == SIM_KIN_MOTION_OK);
|
|
assert(fabs(sample.pose.tran.x - 0.1) < 1e-12 && fabs(sample.joints[0] - 0.1) < 1e-12);
|
|
nominal_hash = sim_kinematics_motion_state_hash(&adapter);
|
|
nominal_repeat_hash = sim_kinematics_motion_state_hash(&adapter);
|
|
assert(sim_kinematics_motion_pop(&adapter, &sample) == SIM_KIN_MOTION_OK);
|
|
|
|
assert(sim_kinematics_motion_reset(&adapter) == SIM_KIN_MOTION_OK);
|
|
segment = line(0.0, 100.0, 1.0);
|
|
assert(sim_kinematics_motion_enqueue(&adapter, &segment, 0) == SIM_KIN_MOTION_OK);
|
|
for (uint32_t index = 0; index < SIM_KIN_MOTION_QUEUE_CAPACITY; ++index)
|
|
assert(sim_kinematics_motion_step(&adapter) == SIM_KIN_MOTION_OK);
|
|
full_planner_before = sim_tp_state_hash(&adapter.planner);
|
|
full_status = sim_kinematics_motion_step(&adapter);
|
|
full_planner_after = sim_tp_state_hash(&adapter.planner);
|
|
full_hash = sim_kinematics_motion_state_hash(&adapter);
|
|
assert(full_status == SIM_KIN_MOTION_ERR_CAPACITY);
|
|
assert(full_planner_before == full_planner_after && adapter.count == SIM_KIN_MOTION_QUEUE_CAPACITY);
|
|
|
|
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
|
assert(sim_kinematics_init(&singular_kins, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_motion_init(&singular_adapter, &singular_kins, 1.0, 10.0, 10.0) == SIM_KIN_MOTION_OK);
|
|
segment = line(0.0, 1.0, 1.0);
|
|
segment.end.b = 90.0;
|
|
assert(sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0) == SIM_KIN_MOTION_OK);
|
|
singular_planner_before = sim_tp_state_hash(&singular_adapter.planner);
|
|
singular_status = sim_kinematics_motion_step(&singular_adapter);
|
|
singular_planner_after = sim_tp_state_hash(&singular_adapter.planner);
|
|
singular_kinematics_error = singular_adapter.kinematics_error;
|
|
assert(singular_status == SIM_KIN_MOTION_ERR_INVERSE);
|
|
assert(singular_kinematics_error == SIM_KIN_ERR_SINGULAR);
|
|
assert(singular_planner_before == singular_planner_after && singular_adapter.count == 0);
|
|
|
|
segment = line(0.0, 1.0, 1.0);
|
|
segment.end.tran.x = NAN;
|
|
nonfinite_status = sim_kinematics_motion_enqueue(&singular_adapter, &segment, 0);
|
|
assert(nonfinite_status == SIM_KIN_MOTION_ERR_NONFINITE);
|
|
|
|
printf("{\"schema\":\"kinematics-motion-samples.v1\",\"nominal\":{\"pause_status\":%d,\"resume_status\":%d,\"tick\":%u,\"pose\":[%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f],\"state_hash\":%llu,\"repeat_hash\":%llu},\"capacity\":{\"status\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu,\"state_hash\":%llu},\"inverse_failure\":{\"status\":%d,\"kinematics_error\":%d,\"depth\":%u,\"planner_before\":%llu,\"planner_after\":%llu},\"nonfinite_status\":%d}\n",
|
|
pause_status, resume_status, sample.tick, sample.pose.tran.x,
|
|
sample.pose.tran.y, sample.pose.tran.z, sample.joints[0],
|
|
sample.joints[1], sample.joints[2],
|
|
(unsigned long long)nominal_hash, (unsigned long long)nominal_repeat_hash,
|
|
full_status, adapter.count, (unsigned long long)full_planner_before,
|
|
(unsigned long long)full_planner_after, (unsigned long long)full_hash,
|
|
singular_status, singular_kinematics_error, singular_adapter.count,
|
|
(unsigned long long)singular_planner_before,
|
|
(unsigned long long)singular_planner_after, nonfinite_status);
|
|
return 0;
|
|
}
|