127 lines
6.9 KiB
C
127 lines
6.9 KiB
C
#include "sim_kinematics.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static sim_kinematics_config config_for(const char *coordinates) {
|
|
sim_kinematics_config config = {0};
|
|
config.joint_count = (uint32_t)strlen(coordinates);
|
|
strncpy(config.coordinates, coordinates, sizeof(config.coordinates) - 1);
|
|
return config;
|
|
}
|
|
|
|
static sim_kinematics_user_descriptor identity_descriptor(void) {
|
|
sim_kinematics_user_descriptor descriptor = {0};
|
|
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
|
|
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
|
|
SIM_KIN_USER_CAP_STATIC;
|
|
descriptor.implementation = SIM_KIN_USER_IDENTITY;
|
|
strcpy(descriptor.name, "userk.identity");
|
|
descriptor.config = config_for("XYZAC");
|
|
return descriptor;
|
|
}
|
|
|
|
static sim_kinematics_user_descriptor genser_descriptor(void) {
|
|
sim_kinematics_user_descriptor descriptor = {0};
|
|
descriptor.abi_version = SIM_KIN_USER_ABI_VERSION;
|
|
descriptor.capabilities = SIM_KIN_USER_CAP_FORWARD | SIM_KIN_USER_CAP_INVERSE |
|
|
SIM_KIN_USER_CAP_STATIC;
|
|
descriptor.implementation = SIM_KIN_USER_GENSER;
|
|
strcpy(descriptor.name, "ugenser.static");
|
|
descriptor.config = config_for("XYZABC");
|
|
descriptor.config.dh_joint_count = 6;
|
|
descriptor.config.max_iterations = 200;
|
|
descriptor.config.convergence_tolerance = 1e-7;
|
|
descriptor.config.dh_a[1] = 85.0;
|
|
descriptor.config.dh_a[2] = 380.0;
|
|
descriptor.config.dh_a[3] = 100.0;
|
|
descriptor.config.dh_alpha[1] = -1.5707963267948966;
|
|
descriptor.config.dh_alpha[3] = -1.5707963267948966;
|
|
descriptor.config.dh_alpha[4] = 1.5707963267948966;
|
|
descriptor.config.dh_alpha[5] = -1.5707963267948966;
|
|
descriptor.config.dh_d[0] = 350.0;
|
|
descriptor.config.dh_d[3] = 425.0;
|
|
descriptor.config.dh_d[5] = 235.0;
|
|
return descriptor;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_kinematics_user identity = {0};
|
|
sim_kinematics_user genser = {0};
|
|
sim_kinematics_user uninitialized = {0};
|
|
sim_kinematics_user_descriptor descriptor = identity_descriptor();
|
|
sim_kinematics_result_info info = {0};
|
|
sim_kinematics base = {0};
|
|
sim_kinematics_config base_config = config_for("XYZAC");
|
|
sim_kinematics_switch switcher = {0};
|
|
sim_posemath_pose identity_pose = {0};
|
|
sim_posemath_pose genser_pose = {0};
|
|
double identity_joints[5] = {10.0, 20.0, 30.0, 5.0, 15.0};
|
|
double identity_roundtrip[5] = {0};
|
|
double genser_joints[6] = {0.0, 10.0, -20.0, 30.0, 15.0, -10.0};
|
|
double genser_roundtrip[6] = {0};
|
|
uint64_t identity_hash;
|
|
uint64_t identity_repeat_hash;
|
|
uint64_t genser_hash;
|
|
uint64_t genser_repeat_hash;
|
|
int switch_to_user;
|
|
int abi_error;
|
|
int capability_error;
|
|
int path_name_error;
|
|
int missing_implementation;
|
|
int dynamic_path_error;
|
|
int uninitialized_error;
|
|
|
|
assert(sim_kinematics_user_init(&identity, &descriptor) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_forward(&identity, identity_joints, 5, &identity_pose, &info) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_inverse(&identity, &identity_pose, identity_roundtrip, 5, &info) == SIM_KIN_OK);
|
|
assert(memcmp(identity_joints, identity_roundtrip, sizeof(identity_joints)) == 0);
|
|
identity_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
|
|
identity_repeat_hash = sim_kinematics_user_state_hash(&identity, identity_roundtrip, 5, &identity_pose);
|
|
|
|
base_config.type = SIM_KIN_TRIVKINS;
|
|
assert(sim_kinematics_init(&base, &base_config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 0, &base, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_bind_switch(&identity, &switcher, 2, 0) == SIM_KIN_OK);
|
|
switch_to_user = sim_kinematics_switch_select(&switcher, 2, identity_roundtrip, 5, &identity_pose, &info);
|
|
assert(switch_to_user == SIM_KIN_OK && sim_kinematics_switch_active(&switcher) == 2);
|
|
|
|
descriptor = genser_descriptor();
|
|
assert(sim_kinematics_user_init(&genser, &descriptor) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_forward(&genser, genser_joints, 6, &genser_pose, &info) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_inverse(&genser, &genser_pose, genser_roundtrip, 6, &info) == SIM_KIN_OK);
|
|
assert(sim_kinematics_user_forward(&genser, genser_roundtrip, 6, &genser_pose, &info) == SIM_KIN_OK);
|
|
genser_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
|
|
genser_repeat_hash = sim_kinematics_user_state_hash(&genser, genser_roundtrip, 6, &genser_pose);
|
|
|
|
descriptor = identity_descriptor();
|
|
descriptor.abi_version += 1;
|
|
abi_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
|
descriptor = identity_descriptor();
|
|
descriptor.capabilities &= ~SIM_KIN_USER_CAP_STATIC;
|
|
capability_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
|
descriptor = identity_descriptor();
|
|
strcpy(descriptor.name, "plugins/user.so");
|
|
path_name_error = sim_kinematics_user_init(&uninitialized, &descriptor);
|
|
descriptor = identity_descriptor();
|
|
descriptor.implementation = 99;
|
|
missing_implementation = sim_kinematics_user_init(&uninitialized, &descriptor);
|
|
dynamic_path_error = sim_kinematics_user_reject_dynamic("/tmp/userkins.so");
|
|
uninitialized_error = sim_kinematics_user_forward(&uninitialized, identity_joints, 5, &identity_pose, &info);
|
|
|
|
printf("{\"schema\":\"userkins-samples.v1\",\"abi_version\":%u,\"capabilities\":%u,\"identity\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"joints\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"state_hash\":%llu,\"repeat_hash\":%llu},\"ugenser\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"forward_calls\":%u,\"inverse_calls\":%u,\"iterations\":%u,\"residual\":%.12f,\"state_hash\":%llu,\"repeat_hash\":%llu},\"switch_to_user\":%d,\"active_mode\":%d,\"abi_error\":%d,\"capability_error\":%d,\"path_name_error\":%d,\"missing_implementation\":%d,\"dynamic_path_error\":%d,\"uninitialized_error\":%d}\n",
|
|
SIM_KIN_USER_ABI_VERSION, identity.descriptor.capabilities,
|
|
identity_pose.tran.x, identity_pose.tran.y, identity_pose.tran.z, identity_pose.a, identity_pose.c,
|
|
identity_roundtrip[0], identity_roundtrip[1], identity_roundtrip[2], identity_roundtrip[3], identity_roundtrip[4],
|
|
identity.forward_calls, identity.inverse_calls, (unsigned long long)identity_hash, (unsigned long long)identity_repeat_hash,
|
|
genser_pose.tran.x, genser_pose.tran.y, genser_pose.tran.z, genser_pose.a, genser_pose.b, genser_pose.c,
|
|
genser.forward_calls, genser.inverse_calls, info.iterations, info.residual,
|
|
(unsigned long long)genser_hash, (unsigned long long)genser_repeat_hash,
|
|
switch_to_user, sim_kinematics_switch_active(&switcher), abi_error, capability_error,
|
|
path_name_error, missing_implementation, dynamic_path_error, uninitialized_error);
|
|
return 0;
|
|
}
|