86 lines
4.8 KiB
C
86 lines
4.8 KiB
C
#include "sim_kinematics.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);
|
|
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
|
|
return config;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_kinematics identity = {0};
|
|
sim_kinematics trt = {0};
|
|
sim_kinematics_config config;
|
|
sim_kinematics_switch switcher = {0};
|
|
sim_kinematics_result_info info = {0};
|
|
sim_posemath_pose initial = {0};
|
|
sim_posemath_pose switched = {0};
|
|
sim_posemath_pose returned = {0};
|
|
double joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
|
|
uint64_t initial_hash;
|
|
uint64_t switched_hash;
|
|
uint64_t returned_hash;
|
|
uint64_t returned_repeat_hash;
|
|
int forward_status;
|
|
int select_to_trt;
|
|
int select_to_identity;
|
|
int invalid_mode;
|
|
int missing_module;
|
|
int register_missing;
|
|
int begin_status;
|
|
int interrupted_begin;
|
|
int abort_status;
|
|
int interrupted_commit;
|
|
|
|
config = config_for(SIM_KIN_TRIVKINS, "XYZAC");
|
|
assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
|
|
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
|
assert(sim_kinematics_init(&trt, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 0, &identity, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 1, &trt, 0) == SIM_KIN_OK);
|
|
register_missing = sim_kinematics_switch_register(&switcher, 2, 0, 0);
|
|
forward_status = sim_kinematics_switch_forward(&switcher, joints, 5, &initial, &info);
|
|
assert(forward_status == SIM_KIN_OK);
|
|
initial_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &initial);
|
|
select_to_trt = sim_kinematics_switch_select(&switcher, 1, joints, 5, &switched, &info);
|
|
assert(select_to_trt == SIM_KIN_OK);
|
|
assert(fabs(switched.tran.x - initial.tran.x) < 1e-9);
|
|
assert(fabs(switched.tran.y - initial.tran.y) < 1e-9);
|
|
assert(fabs(switched.tran.z - initial.tran.z) < 1e-9);
|
|
switched_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &switched);
|
|
select_to_identity = sim_kinematics_switch_select(&switcher, 0, joints, 5, &returned, &info);
|
|
assert(select_to_identity == SIM_KIN_OK);
|
|
assert(fabs(returned.tran.x - initial.tran.x) < 1e-9);
|
|
assert(fabs(returned.tran.y - initial.tran.y) < 1e-9);
|
|
assert(fabs(returned.tran.z - initial.tran.z) < 1e-9);
|
|
returned_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
|
|
returned_repeat_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &returned);
|
|
invalid_mode = sim_kinematics_switch_select(&switcher, 3, joints, 5, &returned, &info);
|
|
missing_module = sim_kinematics_switch_select(&switcher, 2, joints, 5, &returned, &info);
|
|
begin_status = sim_kinematics_switch_begin(&switcher, 1);
|
|
interrupted_begin = sim_kinematics_switch_begin(&switcher, 0);
|
|
abort_status = sim_kinematics_switch_abort(&switcher);
|
|
assert(sim_kinematics_switch_begin(&switcher, 1) == SIM_KIN_OK);
|
|
abort_status = sim_kinematics_switch_abort(&switcher);
|
|
interrupted_commit = sim_kinematics_switch_commit(&switcher, joints, 5, &returned, &info);
|
|
assert(sim_kinematics_switch_active(&switcher) == 0);
|
|
|
|
printf("{\"schema\":\"switchkins-samples.v1\",\"registered\":[%u,%u,%u],\"initial_mode\":0,\"active_mode\":%d,\"forward_status\":%d,\"switch_to_trt\":%d,\"switch_to_identity\":%d,\"initial_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"switched_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"returned_pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"initial_hash\":%llu,\"switched_hash\":%llu,\"returned_hash\":%llu,\"returned_repeat_hash\":%llu,\"invalid_mode\":%d,\"missing_module\":%d,\"register_missing\":%d,\"begin_status\":%d,\"interrupted_begin\":%d,\"abort_status\":%d,\"interrupted_commit\":%d,\"generation\":%u,\"switch_count\":%u,\"abort_count\":%u}\n",
|
|
switcher.registered[0], switcher.registered[1], switcher.registered[2], switcher.active_mode,
|
|
forward_status, select_to_trt, select_to_identity,
|
|
initial.tran.x, initial.tran.y, initial.tran.z, initial.a, initial.b, initial.c,
|
|
switched.tran.x, switched.tran.y, switched.tran.z, switched.a, switched.b, switched.c,
|
|
returned.tran.x, returned.tran.y, returned.tran.z, returned.a, returned.b, returned.c,
|
|
(unsigned long long)initial_hash, (unsigned long long)switched_hash, (unsigned long long)returned_hash, (unsigned long long)returned_repeat_hash,
|
|
invalid_mode, missing_module, register_missing, begin_status, interrupted_begin, abort_status,
|
|
interrupted_commit, switcher.generation, switcher.switch_count, switcher.abort_count);
|
|
return 0;
|
|
}
|