125 lines
6.3 KiB
C
125 lines
6.3 KiB
C
#include "sim_kinematics.h"
|
|
|
|
#include <string.h>
|
|
|
|
#define COMMON (SIM_KIN_CAP_FORWARD | SIM_KIN_CAP_INVERSE | SIM_KIN_CAP_SWITCHABLE)
|
|
#define BASE_PARAMS (SIM_KIN_PARAM_MAPPING | SIM_KIN_PARAM_LIMITS)
|
|
|
|
static const sim_kinematics_capability registry[] = {
|
|
{1, SIM_KIN_TRIVKINS, "trivkins", "", 1, 9, COMMON | SIM_KIN_CAP_IDENTITY | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS},
|
|
{1, SIM_KIN_XYZAC_TRT, "xyzac-trt-kins", "XYZAC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
|
|
{1, SIM_KIN_XYZBC_TRT, "xyzbc-trt-kins", "XYZBC", 5, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_OFFSETS | SIM_KIN_PARAM_TOOL},
|
|
{1, SIM_KIN_XYZBCW, "xyzbcw", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
|
|
{1, SIM_KIN_COREXY, "corexykins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
|
|
{1, SIM_KIN_ROSE, "rosekins", "XYZ", 3, 3, COMMON, BASE_PARAMS},
|
|
{1, SIM_KIN_MAXKINS, "maxkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
|
|
{1, SIM_KIN_ROTATEKINS, "rotatekins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS},
|
|
{1, SIM_KIN_5AXISKINS, "5axiskins", "XYZBCW", 6, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS | SIM_KIN_CAP_DUPLICATE_COORDINATES, BASE_PARAMS | SIM_KIN_PARAM_PIVOT},
|
|
{1, SIM_KIN_SCARA, "scarakins", "XYZCAB", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_PUMA, "pumakins", "XYZABC", 6, 6, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_SCORBOT, "scorbot-kins", "XYZAB", 5, 5, COMMON, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_GENSER, "genserkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_DH | SIM_KIN_PARAM_SOLVER},
|
|
{1, SIM_KIN_LINEAR_DELTA, "lineardeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_ROTARY_DELTA, "rotarydeltakins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_TRIPOD, "tripodkins", "XYZ", 3, 9, COMMON | SIM_KIN_CAP_EXTRA_JOINTS, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY},
|
|
{1, SIM_KIN_GENHEX, "genhexkins", "XYZABC", 6, 6, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
|
|
{1, SIM_KIN_PENTAPOD, "pentakins", "XYZAB", 5, 5, COMMON | SIM_KIN_CAP_ITERATIVE, BASE_PARAMS | SIM_KIN_PARAM_GEOMETRY | SIM_KIN_PARAM_TOOL | SIM_KIN_PARAM_SOLVER},
|
|
};
|
|
|
|
static uint64_t registry_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 int path_like(const char *name) {
|
|
size_t index;
|
|
if (name == 0 || name[0] == '\0') return 0;
|
|
for (index = 0; name[index] != '\0'; ++index)
|
|
if (name[index] == '/' || name[index] == '\\') return 1;
|
|
return 0;
|
|
}
|
|
|
|
static int has_coordinate_casefold(const char *coordinates, char wanted) {
|
|
size_t index;
|
|
for (index = 0; coordinates[index] != '\0'; ++index) {
|
|
char value = coordinates[index];
|
|
if (value >= 'a' && value <= 'z') value = (char)(value - 'a' + 'A');
|
|
if (value == wanted) return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
uint32_t sim_kinematics_registry_count(void) {
|
|
return (uint32_t)(sizeof(registry) / sizeof(registry[0]));
|
|
}
|
|
|
|
int sim_kinematics_registry_get(uint32_t index,
|
|
sim_kinematics_capability *capability) {
|
|
if (capability == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (index >= sim_kinematics_registry_count()) return SIM_KIN_ERR_RANGE;
|
|
*capability = registry[index];
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
int sim_kinematics_registry_find(const char *name,
|
|
sim_kinematics_capability *capability) {
|
|
uint32_t index;
|
|
if (name == 0 || capability == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (path_like(name)) return SIM_KIN_ERR_DYNAMIC_MODULE;
|
|
for (index = 0; index < sim_kinematics_registry_count(); ++index) {
|
|
if (strcmp(name, registry[index].name) == 0) {
|
|
*capability = registry[index];
|
|
return SIM_KIN_OK;
|
|
}
|
|
}
|
|
return SIM_KIN_ERR_UNKNOWN_TYPE;
|
|
}
|
|
|
|
int sim_kinematics_registry_require(const char *name, uint32_t capabilities,
|
|
uint64_t parameter_mask,
|
|
sim_kinematics_capability *capability) {
|
|
sim_kinematics_capability found;
|
|
int result = sim_kinematics_registry_find(name, &found);
|
|
if (result != SIM_KIN_OK) return result;
|
|
if ((found.capabilities & capabilities) != capabilities ||
|
|
(found.parameter_mask & parameter_mask) != parameter_mask) return SIM_KIN_ERR_CAPABILITY;
|
|
if (capability != 0) *capability = found;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
int sim_kinematics_registry_validate_config(const char *name,
|
|
const sim_kinematics_config *config,
|
|
sim_kinematics_capability *capability) {
|
|
sim_kinematics_capability found;
|
|
sim_kinematics instance;
|
|
const char *coordinates;
|
|
uint32_t joint_count;
|
|
size_t index;
|
|
int result;
|
|
if (config == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
result = sim_kinematics_registry_find(name, &found);
|
|
if (result != SIM_KIN_OK) return result;
|
|
if (config->type != found.type) return SIM_KIN_ERR_CONFIGURATION;
|
|
joint_count = config->kinematics_joint_count == 0 ? config->joint_count : config->kinematics_joint_count;
|
|
if (joint_count < found.min_joints || joint_count > found.max_joints) return SIM_KIN_ERR_CONFIGURATION;
|
|
coordinates = config->coordinates[0] == '\0' ? "XYZABCUVW" : config->coordinates;
|
|
for (index = 0; found.required_coordinates[index] != '\0'; ++index)
|
|
if (!has_coordinate_casefold(coordinates, found.required_coordinates[index])) return SIM_KIN_ERR_CONFIGURATION;
|
|
result = sim_kinematics_init(&instance, config);
|
|
if (result != SIM_KIN_OK) return result;
|
|
if (capability != 0) *capability = found;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
uint64_t sim_kinematics_registry_state_hash(void) {
|
|
return registry_fnv1a64((const unsigned char *)registry, sizeof(registry));
|
|
}
|
|
|
|
#undef BASE_PARAMS
|
|
#undef COMMON
|