1208 lines
75 KiB
C
1208 lines
75 KiB
C
#include "sim_kinematics.h"
|
|
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
#include "posemath.h"
|
|
|
|
#define DEG_TO_RAD 0.017453292519943295769236907684886
|
|
#define RAD_TO_DEG 57.295779513082320876798154814105
|
|
|
|
static uint64_t 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 double hash_normalize(double value) {
|
|
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
|
|
}
|
|
|
|
/* The five-leg Newton solve can differ by a few ulps between native and
|
|
* wasm32 math libraries. Keep the public pose/info boundary deterministic
|
|
* without changing the convergence test itself. */
|
|
static double pentapod_canonical(double value) {
|
|
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
|
|
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
|
|
}
|
|
|
|
static int finite_values(const double *values, size_t count) {
|
|
size_t index;
|
|
if (values == 0) return 0;
|
|
for (index = 0; index < count; ++index) if (!isfinite(values[index])) return 0;
|
|
return 1;
|
|
}
|
|
|
|
int sim_kinematics_validate_pose(const sim_posemath_pose *pose) {
|
|
return pose != 0 && finite_values(&pose->tran.x, 9) ? SIM_KIN_OK : SIM_KIN_ERR_NONFINITE;
|
|
}
|
|
|
|
static int axis_index(char letter) {
|
|
switch (letter) {
|
|
case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
|
|
case 'A': return 3; case 'B': return 4; case 'C': return 5;
|
|
case 'U': return 6; case 'V': return 7; case 'W': return 8;
|
|
default: return -1;
|
|
}
|
|
}
|
|
|
|
static int has_coordinate(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;
|
|
}
|
|
|
|
static int effective_mode(const sim_kinematics_config *config) {
|
|
if (config->mode != SIM_KIN_MODE_DEFAULT) return config->mode;
|
|
return config->type == SIM_KIN_TRIVKINS ? SIM_KIN_MODE_IDENTITY : SIM_KIN_MODE_BOTH;
|
|
}
|
|
|
|
static uint32_t kinematics_joint_count(const sim_kinematics_config *config) {
|
|
return config->kinematics_joint_count == 0 ? config->joint_count : config->kinematics_joint_count;
|
|
}
|
|
|
|
static int known_type(int type) {
|
|
switch (type) {
|
|
case SIM_KIN_TRIVKINS:
|
|
case SIM_KIN_XYZAC_TRT:
|
|
case SIM_KIN_XYZBC_TRT:
|
|
case SIM_KIN_XYZBCW:
|
|
case SIM_KIN_COREXY:
|
|
case SIM_KIN_ROSE:
|
|
case SIM_KIN_MAXKINS:
|
|
case SIM_KIN_ROTATEKINS:
|
|
case SIM_KIN_5AXISKINS:
|
|
case SIM_KIN_SCARA:
|
|
case SIM_KIN_PUMA:
|
|
case SIM_KIN_SCORBOT:
|
|
case SIM_KIN_GENSER:
|
|
case SIM_KIN_LINEAR_DELTA:
|
|
case SIM_KIN_ROTARY_DELTA:
|
|
case SIM_KIN_TRIPOD:
|
|
case SIM_KIN_GENHEX:
|
|
case SIM_KIN_PENTAPOD:
|
|
return 1;
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
static void initialize_info(sim_kinematics_result_info *info, uint64_t forward_flags, uint64_t inverse_flags) {
|
|
if (info == 0) return;
|
|
info->forward_flags = forward_flags;
|
|
info->inverse_flags = inverse_flags;
|
|
info->residual = 0.0;
|
|
info->iterations = 0;
|
|
info->branch_id = 0;
|
|
}
|
|
|
|
static int config_axis(const sim_kinematics *kinematics, int axis) {
|
|
return kinematics->axis_joint[axis];
|
|
}
|
|
|
|
static int valid_config(const sim_kinematics_config *config) {
|
|
const char *coordinates;
|
|
size_t length;
|
|
size_t index;
|
|
uint32_t kinematics_count;
|
|
if (config == 0 || config->joint_count < 1 || config->joint_count > SIM_KIN_MAX_JOINTS) return SIM_KIN_ERR_CONFIGURATION;
|
|
kinematics_count = kinematics_joint_count(config);
|
|
if (kinematics_count < 1 || kinematics_count > config->joint_count) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->extra_joint_count != 0 && config->extra_joint_count != config->joint_count - kinematics_count) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (!known_type(config->type)) return SIM_KIN_ERR_UNKNOWN_TYPE;
|
|
if (config->mode < SIM_KIN_MODE_DEFAULT || config->mode > SIM_KIN_MODE_BOTH) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (!finite_values(&config->x_rot_point, 19) || !finite_values(&config->delta_radius, 9)) return SIM_KIN_ERR_NONFINITE;
|
|
if (config->max_iterations > 100000u) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->convergence_tolerance < 0.0 || !isfinite(config->convergence_tolerance)) return SIM_KIN_ERR_CONFIGURATION;
|
|
coordinates = config->coordinates[0] == '\0' ? "XYZABCUVW" : config->coordinates;
|
|
length = strlen(coordinates);
|
|
if (length == 0 || length > SIM_KIN_MAX_JOINTS || length > kinematics_count) return SIM_KIN_ERR_CONFIGURATION;
|
|
for (index = 0; index < length; ++index) {
|
|
int axis = axis_index((char)(coordinates[index] >= 'a' && coordinates[index] <= 'z' ? coordinates[index] - 'a' + 'A' : coordinates[index]));
|
|
if (axis < 0) return SIM_KIN_ERR_CONFIGURATION;
|
|
(void)axis; /* Duplicate axis letters are valid for gantry/extra-joint mappings. */
|
|
}
|
|
if (config->type == SIM_KIN_XYZAC_TRT && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_XYZBC_TRT && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if ((config->type == SIM_KIN_XYZBCW || config->type == SIM_KIN_5AXISKINS) && (!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C') || !has_coordinate(coordinates, 'W'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if ((config->type == SIM_KIN_COREXY || config->type == SIM_KIN_ROSE ||
|
|
config->type == SIM_KIN_MAXKINS || config->type == SIM_KIN_ROTATEKINS) &&
|
|
(!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') ||
|
|
!has_coordinate(coordinates, 'Z'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_ROSE && kinematics_count != 3) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_SCARA && (kinematics_count != 6 || !has_coordinate(coordinates, 'C') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_PUMA && (kinematics_count != 6 || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_SCORBOT && (kinematics_count != 5 || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B'))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_GENSER && (kinematics_count != 6 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C') || (config->dh_joint_count != 0 && config->dh_joint_count != 6))) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->type == SIM_KIN_GENSER && (!finite_values(config->dh_a, 6) || !finite_values(config->dh_alpha, 6) || !finite_values(config->dh_d, 6))) return SIM_KIN_ERR_NONFINITE;
|
|
if (config->type == SIM_KIN_LINEAR_DELTA) {
|
|
if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
|
|
/* A completely zero geometry means "use the LinuxCNC defaults" and
|
|
* is filled in by init. Partially specified or non-positive geometry
|
|
* is always rejected instead of allowing a divide-by-zero later. */
|
|
if (!((config->delta_radius == 0.0) && (config->delta_rod_length == 0.0)) &&
|
|
(config->delta_radius <= 0.0 || config->delta_rod_length <= 0.0)) return SIM_KIN_ERR_CONFIGURATION;
|
|
}
|
|
if (config->type == SIM_KIN_ROTARY_DELTA) {
|
|
if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (!((config->rotary_platform_radius == 0.0) && (config->rotary_thigh_length == 0.0) &&
|
|
(config->rotary_shin_length == 0.0) && (config->rotary_foot_radius == 0.0)) &&
|
|
(config->rotary_platform_radius <= 0.0 || config->rotary_thigh_length <= 0.0 ||
|
|
config->rotary_shin_length <= 0.0 || config->rotary_foot_radius <= 0.0)) return SIM_KIN_ERR_CONFIGURATION;
|
|
}
|
|
if (config->type == SIM_KIN_TRIPOD) {
|
|
if (kinematics_count < 3 || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z')) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (!((config->tripod_bx == 0.0) && (config->tripod_cx == 0.0) && (config->tripod_cy == 0.0)) &&
|
|
(fabs(config->tripod_bx) < 1e-12 || fabs(config->tripod_cy) < 1e-12)) return SIM_KIN_ERR_CONFIGURATION;
|
|
}
|
|
if (config->type == SIM_KIN_GENHEX) {
|
|
if (kinematics_count != SIM_KIN_GENHEX_STRUTS ||
|
|
!has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') ||
|
|
!has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') ||
|
|
!has_coordinate(coordinates, 'B') || !has_coordinate(coordinates, 'C')) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->genhex_iteration_limit > 100000u) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (config->genhex_convergence_tolerance < 0.0 || !isfinite(config->genhex_convergence_tolerance) ||
|
|
config->genhex_max_error < 0.0 || !isfinite(config->genhex_max_error) ||
|
|
!finite_values(config->genhex_initial_pose, 6)) return SIM_KIN_ERR_CONFIGURATION;
|
|
for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
|
|
if (!finite_values(config->genhex_base[index], 3) || !finite_values(config->genhex_platform[index], 3) ||
|
|
!finite_values(config->genhex_base_axis[index], 3) || !finite_values(config->genhex_platform_axis[index], 3)) return SIM_KIN_ERR_NONFINITE;
|
|
}
|
|
}
|
|
if (config->type == SIM_KIN_PENTAPOD) {
|
|
if (kinematics_count != SIM_KIN_PENTA_STRUTS || !has_coordinate(coordinates, 'X') || !has_coordinate(coordinates, 'Y') || !has_coordinate(coordinates, 'Z') || !has_coordinate(coordinates, 'A') || !has_coordinate(coordinates, 'B')) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (!isfinite(config->penta_tool_offset)) return SIM_KIN_ERR_NONFINITE;
|
|
if (config->penta_iteration_limit > 100000u || config->penta_convergence_tolerance < 0.0 || !isfinite(config->penta_convergence_tolerance) || config->penta_max_error < 0.0 || !isfinite(config->penta_max_error) || !finite_values(config->penta_initial_pose, 5)) return SIM_KIN_ERR_CONFIGURATION;
|
|
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) if (!finite_values(config->penta_base[index], 3) || !isfinite(config->penta_effector_radius[index]) || !isfinite(config->penta_effector_z[index]) || ((config->penta_effector_radius[index] < 0.0) || (config->penta_effector_radius[index] == 0.0 && config->penta_effector_z[index] != 0.0))) return SIM_KIN_ERR_CONFIGURATION;
|
|
}
|
|
for (index = 0; index < kinematics_count; ++index) {
|
|
if (config->limit_enabled[index] && (!isfinite(config->joint_min[index]) || !isfinite(config->joint_max[index]) || config->joint_min[index] > config->joint_max[index])) return SIM_KIN_ERR_CONFIGURATION;
|
|
}
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int in_limits(const sim_kinematics *kinematics, const double *joints) {
|
|
uint32_t index;
|
|
for (index = 0; index < kinematics_joint_count(&kinematics->config); ++index)
|
|
if (kinematics->config.limit_enabled[index] && (joints[index] < kinematics->config.joint_min[index] || joints[index] > kinematics->config.joint_max[index])) return SIM_KIN_ERR_RANGE;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static void map_joints(sim_kinematics *kinematics) {
|
|
size_t index;
|
|
for (index = 0; index < SIM_KIN_MAX_JOINTS; ++index) kinematics->axis_joint[index] = -1;
|
|
for (index = 0; index < kinematics_joint_count(&kinematics->config); ++index) {
|
|
int axis = axis_index((char)(kinematics->config.coordinates[index] >= 'a' && kinematics->config.coordinates[index] <= 'z' ? kinematics->config.coordinates[index] - 'a' + 'A' : kinematics->config.coordinates[index]));
|
|
if (axis >= 0 && kinematics->axis_joint[axis] < 0) kinematics->axis_joint[axis] = (int)index;
|
|
}
|
|
}
|
|
|
|
int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config *config) {
|
|
int result;
|
|
int geometry_zero;
|
|
if (kinematics == 0 || config == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
result = valid_config(config);
|
|
if (result != SIM_KIN_OK) return result;
|
|
memset(kinematics, 0, sizeof(*kinematics));
|
|
kinematics->config = *config;
|
|
if (kinematics->config.coordinates[0] == '\0') strcpy(kinematics->config.coordinates, "XYZABCUVW");
|
|
if (kinematics->config.kinematics_joint_count == 0) kinematics->config.kinematics_joint_count = kinematics->config.joint_count;
|
|
if (kinematics->config.extra_joint_count == 0) kinematics->config.extra_joint_count = kinematics->config.joint_count - kinematics->config.kinematics_joint_count;
|
|
geometry_zero = kinematics->config.scara_d1 == 0.0 && kinematics->config.scara_d2 == 0.0 &&
|
|
kinematics->config.scara_d3 == 0.0 && kinematics->config.scara_d4 == 0.0 &&
|
|
kinematics->config.scara_d5 == 0.0 && kinematics->config.scara_d6 == 0.0;
|
|
if (kinematics->config.type == SIM_KIN_SCARA && geometry_zero) {
|
|
kinematics->config.scara_d1 = 490.0; kinematics->config.scara_d2 = 340.0;
|
|
kinematics->config.scara_d3 = 50.0; kinematics->config.scara_d4 = 250.0;
|
|
kinematics->config.scara_d5 = 50.0; kinematics->config.scara_d6 = 50.0;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_PUMA && kinematics->config.puma_a2 == 0.0 &&
|
|
kinematics->config.puma_a3 == 0.0 && kinematics->config.puma_d3 == 0.0 &&
|
|
kinematics->config.puma_d4 == 0.0 && kinematics->config.puma_d6 == 0.0) {
|
|
kinematics->config.puma_a2 = 300.0; kinematics->config.puma_a3 = 50.0;
|
|
kinematics->config.puma_d3 = 70.0; kinematics->config.puma_d4 = 400.0;
|
|
kinematics->config.puma_d6 = 70.0;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_GENSER && kinematics->config.dh_joint_count == 0) kinematics->config.dh_joint_count = 6;
|
|
if (kinematics->config.type == SIM_KIN_LINEAR_DELTA && kinematics->config.delta_radius == 0.0 && kinematics->config.delta_rod_length == 0.0) {
|
|
kinematics->config.delta_radius = 130.25; kinematics->config.delta_rod_length = 269.0;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_ROTARY_DELTA && kinematics->config.rotary_platform_radius == 0.0 && kinematics->config.rotary_thigh_length == 0.0 && kinematics->config.rotary_shin_length == 0.0 && kinematics->config.rotary_foot_radius == 0.0) {
|
|
kinematics->config.rotary_platform_radius = 10.0; kinematics->config.rotary_thigh_length = 10.0; kinematics->config.rotary_shin_length = 14.0; kinematics->config.rotary_foot_radius = 6.0;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_TRIPOD && kinematics->config.tripod_bx == 0.0 && kinematics->config.tripod_cx == 0.0 && kinematics->config.tripod_cy == 0.0) {
|
|
kinematics->config.tripod_bx = 10.0; kinematics->config.tripod_cx = 5.0; kinematics->config.tripod_cy = 7.071;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_GENHEX) {
|
|
size_t index;
|
|
int all_zero = 1;
|
|
static const double base[SIM_KIN_GENHEX_STRUTS][3] = {
|
|
{-22.950, 13.250, 0.0}, {22.950, 13.250, 0.0}, {22.950, 13.250, 0.0},
|
|
{0.0, -26.500, 0.0}, {0.0, -26.500, 0.0}, {-22.950, 13.250, 0.0}
|
|
};
|
|
static const double platform[SIM_KIN_GENHEX_STRUTS][3] = {
|
|
{-1.000, 11.500, 0.0}, {1.000, 11.500, 0.0}, {10.459, -4.884, 0.0},
|
|
{9.459, -6.616, 0.0}, {-9.459, -6.616, 0.0}, {-10.459, -4.884, 0.0}
|
|
};
|
|
static const double base_axis[SIM_KIN_GENHEX_STRUTS][3] = {
|
|
{0.707107, 0.0, 0.707107}, {0.0, -0.707107, 0.707107}, {-0.707107, 0.0, 0.707107},
|
|
{-0.707107, 0.0, 0.707107}, {0.0, 0.707107, 0.707107}, {0.707107, 0.0, 0.707107}
|
|
};
|
|
static const double platform_axis[SIM_KIN_GENHEX_STRUTS][3] = {
|
|
{-1.0, 0.0, 0.0}, {0.866025, 0.5, 0.0}, {0.866025, 0.5, 0.0},
|
|
{0.866025, -0.5, 0.0}, {0.866025, -0.5, 0.0}, {-1.0, 0.0, 0.0}
|
|
};
|
|
for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
|
|
size_t axis;
|
|
for (axis = 0; axis < 3; ++axis) if (kinematics->config.genhex_base[index][axis] != 0.0 || kinematics->config.genhex_platform[index][axis] != 0.0) all_zero = 0;
|
|
}
|
|
if (all_zero) {
|
|
memcpy(kinematics->config.genhex_base, base, sizeof(base));
|
|
memcpy(kinematics->config.genhex_platform, platform, sizeof(platform));
|
|
memcpy(kinematics->config.genhex_base_axis, base_axis, sizeof(base_axis));
|
|
memcpy(kinematics->config.genhex_platform_axis, platform_axis, sizeof(platform_axis));
|
|
}
|
|
if (kinematics->config.genhex_iteration_limit == 0) kinematics->config.genhex_iteration_limit = 120;
|
|
if (kinematics->config.genhex_convergence_tolerance == 0.0) kinematics->config.genhex_convergence_tolerance = 1e-9;
|
|
if (kinematics->config.genhex_max_error == 0.0) kinematics->config.genhex_max_error = 500.0;
|
|
if (kinematics->config.genhex_initial_pose[2] == 0.0) kinematics->config.genhex_initial_pose[2] = 50.0;
|
|
}
|
|
if (kinematics->config.type == SIM_KIN_PENTAPOD) {
|
|
static const double base[SIM_KIN_PENTA_STRUTS][3] = {{-418.0, 500.0, -140.0}, {418.0, 500.0, -140.0}, {0.0, 500.0, -310.0}, {418.0, 500.0, 510.0}, {-418.0, 500.0, 510.0}};
|
|
static const double radius[SIM_KIN_PENTA_STRUTS] = {80.30, 80.30, 80.30, 80.30, 80.30};
|
|
static const double effector_z[SIM_KIN_PENTA_STRUTS] = {87.0, 114.5, 140.5, 232.5, 258.5};
|
|
int all_zero = 1;
|
|
size_t index;
|
|
for (index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
|
|
if (kinematics->config.penta_base[index][0] != 0.0 || kinematics->config.penta_base[index][1] != 0.0 || kinematics->config.penta_base[index][2] != 0.0 ||
|
|
kinematics->config.penta_effector_radius[index] != 0.0 || kinematics->config.penta_effector_z[index] != 0.0) all_zero = 0;
|
|
}
|
|
if (all_zero) { memcpy(kinematics->config.penta_base, base, sizeof(base)); memcpy(kinematics->config.penta_effector_radius, radius, sizeof(radius)); memcpy(kinematics->config.penta_effector_z, effector_z, sizeof(effector_z)); }
|
|
if (kinematics->config.penta_iteration_limit == 0) kinematics->config.penta_iteration_limit = 120;
|
|
if (kinematics->config.penta_convergence_tolerance == 0.0) kinematics->config.penta_convergence_tolerance = 1e-9;
|
|
if (kinematics->config.penta_max_error == 0.0) kinematics->config.penta_max_error = 1000.0;
|
|
if (kinematics->config.penta_initial_pose[1] == 0.0) kinematics->config.penta_initial_pose[1] = 500.0;
|
|
if (kinematics->config.penta_initial_pose[2] == 0.0) kinematics->config.penta_initial_pose[2] = 300.0;
|
|
}
|
|
map_joints(kinematics);
|
|
kinematics->configured = 1;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static void zero_pose(sim_posemath_pose *pose) { memset(pose, 0, sizeof(*pose)); }
|
|
|
|
static double joint(const sim_kinematics *k, const double *joints, int axis) {
|
|
int index = config_axis(k, axis);
|
|
return index < 0 ? 0.0 : joints[index];
|
|
}
|
|
|
|
static void set_optional_axes(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
|
|
pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4); pose->c = joint(k, joints, 5);
|
|
pose->u = joint(k, joints, 6); pose->v = joint(k, joints, 7); pose->w = joint(k, joints, 8);
|
|
}
|
|
|
|
static void set_joint(double *joints, const sim_kinematics *k, int axis, double value);
|
|
|
|
static int scara_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
|
|
double a0 = joint(k, joints, 0) * DEG_TO_RAD;
|
|
double a1 = joint(k, joints, 1) * DEG_TO_RAD + a0;
|
|
double a3 = joint(k, joints, 5) * DEG_TO_RAD + a1;
|
|
pose->tran.x = k->config.scara_d2 * cos(a0) + k->config.scara_d4 * cos(a1) + k->config.scara_d6 * cos(a3);
|
|
pose->tran.y = k->config.scara_d2 * sin(a0) + k->config.scara_d4 * sin(a1) + k->config.scara_d6 * sin(a3);
|
|
pose->tran.z = k->config.scara_d1 + k->config.scara_d3 - joint(k, joints, 2) - k->config.scara_d5;
|
|
pose->c = a3 * RAD_TO_DEG;
|
|
pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4);
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int scara_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint64_t flags, uint32_t *branch) {
|
|
double a3 = pose->c * DEG_TO_RAD;
|
|
double xt = pose->tran.x - k->config.scara_d6 * cos(a3);
|
|
double yt = pose->tran.y - k->config.scara_d6 * sin(a3);
|
|
double rsq = xt * xt + yt * yt;
|
|
double cc = (rsq - k->config.scara_d2 * k->config.scara_d2 - k->config.scara_d4 * k->config.scara_d4) /
|
|
(2.0 * k->config.scara_d2 * k->config.scara_d4);
|
|
double q1;
|
|
double q0;
|
|
if (cc < -1.0 - 1e-12 || cc > 1.0 + 1e-12) return SIM_KIN_ERR_RANGE;
|
|
if (cc < -1.0) cc = -1.0;
|
|
if (cc > 1.0) cc = 1.0;
|
|
q1 = acos(cc);
|
|
if (flags & UINT64_C(1)) q1 = -q1;
|
|
q0 = atan2(yt, xt) - atan2(k->config.scara_d4 * sin(q1), k->config.scara_d2 + k->config.scara_d4 * cos(q1));
|
|
set_joint(joints, k, 0, q0 * RAD_TO_DEG);
|
|
set_joint(joints, k, 1, q1 * RAD_TO_DEG);
|
|
set_joint(joints, k, 2, k->config.scara_d1 + k->config.scara_d3 - k->config.scara_d5 - pose->tran.z);
|
|
set_joint(joints, k, 5, pose->c - (q0 + q1) * RAD_TO_DEG);
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
|
|
if (branch != 0) *branch = (q1 < 0.0) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int scorbot_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
|
|
double j0 = joint(k, joints, 0) * DEG_TO_RAD;
|
|
double j1 = joint(k, joints, 1) * DEG_TO_RAD;
|
|
double j2 = joint(k, joints, 2) * DEG_TO_RAD;
|
|
double radius = 16.0 + 221.0 * cos(j1) + 221.0 * cos(j2);
|
|
pose->tran.x = radius * cos(j0);
|
|
pose->tran.y = radius * sin(j0);
|
|
pose->tran.z = 140.0 + 221.0 * sin(j1) + 221.0 * sin(j2);
|
|
pose->a = joint(k, joints, 3); pose->b = joint(k, joints, 4);
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int scorbot_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
|
|
double r = hypot(pose->tran.x, pose->tran.y) - 16.0;
|
|
double z = pose->tran.z - 140.0;
|
|
double distance = hypot(r, z);
|
|
double angle;
|
|
double j1;
|
|
if (distance < 1e-12 || distance > 442.0 + 1e-9) return SIM_KIN_ERR_RANGE;
|
|
angle = acos(r / distance) * (z < 0.0 ? -1.0 : 1.0);
|
|
j1 = angle + acos(distance / 442.0);
|
|
set_joint(joints, k, 0, atan2(pose->tran.y, pose->tran.x) * RAD_TO_DEG);
|
|
set_joint(joints, k, 1, j1 * RAD_TO_DEG);
|
|
set_joint(joints, k, 2, -asin((221.0 * sin(j1) - z) / 221.0) * RAD_TO_DEG);
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int puma_forward(const sim_kinematics *k, const double *joint_values, sim_posemath_pose *pose, uint32_t *branch) {
|
|
double q0 = joint(k, joint_values, 0) * DEG_TO_RAD;
|
|
double q1 = joint(k, joint_values, 1) * DEG_TO_RAD;
|
|
double q2 = joint(k, joint_values, 2) * DEG_TO_RAD;
|
|
double q3 = joint(k, joint_values, 3) * DEG_TO_RAD;
|
|
double q4 = joint(k, joint_values, 4) * DEG_TO_RAD;
|
|
double q5 = joint(k, joint_values, 5) * DEG_TO_RAD;
|
|
double s1 = sin(q0), s2 = sin(q1), s3 = sin(q2), s4 = sin(q3), s5 = sin(q4), s6 = sin(q5);
|
|
double c1 = cos(q0), c2 = cos(q1), c3 = cos(q2), c4 = cos(q3), c5 = cos(q4), c6 = cos(q5);
|
|
double s23 = c2 * s3 + s2 * c3;
|
|
double c23 = c2 * c3 - s2 * s3;
|
|
double t1, t2, t3, t4, t5;
|
|
PmHomogeneous hom = {0};
|
|
PmRpy rpy = {0};
|
|
t1 = c4 * c5 * c6 - s4 * s6; t2 = s23 * s5 * c6; t3 = s4 * c5 * c6 + c4 * s6; t4 = c23 * t1 - t2; t5 = c23 * s5 * c6;
|
|
hom.rot.x.x = c1 * t4 + s1 * t3; hom.rot.x.y = s1 * t4 - c1 * t3; hom.rot.x.z = -s23 * t1 - t5;
|
|
t1 = -c4 * c5 * s6 - s4 * c6; t2 = s23 * s5 * s6; t3 = c4 * c6 - s4 * c5 * s6; t4 = c23 * t1 + t2; t5 = c23 * s5 * s6;
|
|
hom.rot.y.x = c1 * t4 + s1 * t3; hom.rot.y.y = s1 * t4 - c1 * t3; hom.rot.y.z = -s23 * t1 + t5;
|
|
t1 = c23 * c4 * s5 + s23 * c5;
|
|
hom.rot.z.x = -c1 * t1 - s1 * s4 * s5; hom.rot.z.y = -s1 * t1 + c1 * s4 * s5; hom.rot.z.z = s23 * c4 * s5 - c23 * c5;
|
|
t1 = k->config.puma_a2 * c2 + k->config.puma_a3 * c23 - k->config.puma_d4 * s23;
|
|
hom.tran.x = c1 * t1 - k->config.puma_d3 * s1;
|
|
hom.tran.y = s1 * t1 + k->config.puma_d3 * c1;
|
|
hom.tran.z = -k->config.puma_a3 * s23 - k->config.puma_a2 * s2 - k->config.puma_d4 * c23;
|
|
hom.tran.x += hom.rot.z.x * k->config.puma_d6;
|
|
hom.tran.y += hom.rot.z.y * k->config.puma_d6;
|
|
hom.tran.z += hom.rot.z.z * k->config.puma_d6;
|
|
if (pmMatRpyConvert(&hom.rot, &rpy) != 0) return SIM_KIN_ERR_SINGULAR;
|
|
pose->tran.x = hom.tran.x; pose->tran.y = hom.tran.y; pose->tran.z = hom.tran.z;
|
|
pose->a = rpy.r * RAD_TO_DEG; pose->b = rpy.p * RAD_TO_DEG; pose->c = rpy.y * RAD_TO_DEG;
|
|
if (branch != 0) *branch = (fabs(s5) < 1e-9) ? 8U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int puma_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint64_t flags, uint32_t *branch) {
|
|
PmRotationMatrix rot = {0};
|
|
PmRpy rpy = {pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD, pose->c * DEG_TO_RAD};
|
|
double px, py, pz, sum_sq, kterm, th1, th2, th3, th23, th4, th5, th6;
|
|
double s1, c1, s3, c3, s23, c23, s4, c4, s5, c5, s6, c6, t1, t2, t3;
|
|
if (pmRpyMatConvert(&rpy, &rot) != 0) return SIM_KIN_ERR_SINGULAR;
|
|
px = pose->tran.x - k->config.puma_d6 * rot.z.x;
|
|
py = pose->tran.y - k->config.puma_d6 * rot.z.y;
|
|
pz = pose->tran.z - k->config.puma_d6 * rot.z.z;
|
|
sum_sq = px * px + py * py - k->config.puma_d3 * k->config.puma_d3;
|
|
if (sum_sq < -1e-9) return SIM_KIN_ERR_RANGE;
|
|
if (sum_sq < 0.0) sum_sq = 0.0;
|
|
th1 = atan2(py, px) - atan2(k->config.puma_d3, (flags & 1U) ? -sqrt(sum_sq) : sqrt(sum_sq));
|
|
s1 = sin(th1); c1 = cos(th1);
|
|
kterm = (sum_sq + pz * pz - k->config.puma_a2 * k->config.puma_a2 - k->config.puma_a3 * k->config.puma_a3 - k->config.puma_d4 * k->config.puma_d4) / (2.0 * k->config.puma_a2);
|
|
if (kterm * kterm > k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 + 1e-9) return SIM_KIN_ERR_RANGE;
|
|
if (kterm * kterm > k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4) kterm = copysign(sqrt(k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4), kterm);
|
|
th3 = atan2(k->config.puma_a3, k->config.puma_d4) - atan2(kterm, (flags & 2U) ? -sqrt(fmax(0.0, k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 - kterm * kterm)) : sqrt(fmax(0.0, k->config.puma_a3 * k->config.puma_a3 + k->config.puma_d4 * k->config.puma_d4 - kterm * kterm)));
|
|
s3 = sin(th3); c3 = cos(th3);
|
|
t1 = (-k->config.puma_a3 - k->config.puma_a2 * c3) * pz + (c1 * px + s1 * py) * (k->config.puma_a2 * s3 - k->config.puma_d4);
|
|
t2 = (k->config.puma_a2 * s3 - k->config.puma_d4) * pz + (k->config.puma_a3 + k->config.puma_a2 * c3) * (c1 * px + s1 * py);
|
|
t3 = pz * pz + (c1 * px + s1 * py) * (c1 * px + s1 * py);
|
|
if (t3 < 1e-18) return SIM_KIN_ERR_SINGULAR;
|
|
th23 = atan2(t1, t2); th2 = th23 - th3; s23 = t1 / t3; c23 = t2 / t3;
|
|
t1 = -rot.z.x * s1 + rot.z.y * c1;
|
|
t2 = -rot.z.x * c1 * c23 - rot.z.y * s1 * c23 + rot.z.z * s23;
|
|
if (fabs(t1) < 1e-9 && fabs(t2) < 1e-9) return SIM_KIN_ERR_SINGULAR;
|
|
th4 = atan2(t1, t2); s4 = sin(th4); c4 = cos(th4);
|
|
s5 = rot.z.z * (s23 * c4) - rot.z.x * (c1 * c23 * c4 + s1 * s4) - rot.z.y * (s1 * c23 * c4 - c1 * s4);
|
|
c5 = -rot.z.x * (c1 * s23) - rot.z.y * (s1 * s23) - rot.z.z * c23; th5 = atan2(s5, c5);
|
|
s6 = rot.x.z * (s23 * s4) - rot.x.x * (c1 * c23 * s4 - s1 * c4) - rot.x.y * (s1 * c23 * s4 + c1 * c4);
|
|
c6 = rot.x.x * ((c1 * c23 * c4 + s1 * s4) * c5 - c1 * s23 * s5) + rot.x.y * ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) - rot.x.z * (s23 * c4 * c5 + c23 * s5);
|
|
th6 = atan2(s6, c6);
|
|
if (flags & 4U) { th4 += PM_PI; th5 = -th5; th6 += PM_PI; }
|
|
set_joint(joints, k, 0, th1 * RAD_TO_DEG); set_joint(joints, k, 1, th2 * RAD_TO_DEG); set_joint(joints, k, 2, th3 * RAD_TO_DEG);
|
|
set_joint(joints, k, 3, th4 * RAD_TO_DEG); set_joint(joints, k, 4, th5 * RAD_TO_DEG); set_joint(joints, k, 5, th6 * RAD_TO_DEG);
|
|
if (branch != 0) *branch = (uint32_t)(flags & 7U);
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static void mat4_identity(double m[4][4]) {
|
|
int row, col;
|
|
for (row = 0; row < 4; ++row) for (col = 0; col < 4; ++col) m[row][col] = row == col ? 1.0 : 0.0;
|
|
}
|
|
|
|
static void mat4_multiply(const double left[4][4], const double right[4][4], double out[4][4]) {
|
|
double value[4][4];
|
|
int row, col, inner;
|
|
for (row = 0; row < 4; ++row) for (col = 0; col < 4; ++col) {
|
|
value[row][col] = 0.0;
|
|
for (inner = 0; inner < 4; ++inner) value[row][col] += left[row][inner] * right[inner][col];
|
|
}
|
|
memcpy(out, value, sizeof(value));
|
|
}
|
|
|
|
static int genser_forward(const sim_kinematics *k, const double *joints, sim_posemath_pose *pose) {
|
|
double total[4][4];
|
|
uint32_t index;
|
|
PmRotationMatrix rotation;
|
|
PmRpy rpy;
|
|
mat4_identity(total);
|
|
for (index = 0; index < 6; ++index) {
|
|
double theta = joints[index] * DEG_TO_RAD;
|
|
double alpha = k->config.dh_alpha[index];
|
|
double ct = cos(theta), st = sin(theta), ca = cos(alpha), sa = sin(alpha);
|
|
double link[4][4] = {
|
|
{ct, -st, 0.0, k->config.dh_a[index]},
|
|
{st * ca, ct * ca, -sa, -sa * k->config.dh_d[index]},
|
|
{st * sa, ct * sa, ca, ca * k->config.dh_d[index]},
|
|
{0.0, 0.0, 0.0, 1.0},
|
|
};
|
|
mat4_multiply(total, link, total);
|
|
}
|
|
rotation.x.x = total[0][0]; rotation.y.x = total[0][1]; rotation.z.x = total[0][2];
|
|
rotation.x.y = total[1][0]; rotation.y.y = total[1][1]; rotation.z.y = total[1][2];
|
|
rotation.x.z = total[2][0]; rotation.y.z = total[2][1]; rotation.z.z = total[2][2];
|
|
if (pmMatRpyConvert(&rotation, &rpy) != 0) return SIM_KIN_ERR_SINGULAR;
|
|
pose->tran.x = total[0][3]; pose->tran.y = total[1][3]; pose->tran.z = total[2][3];
|
|
pose->a = rpy.r * RAD_TO_DEG; pose->b = rpy.p * RAD_TO_DEG; pose->c = rpy.y * RAD_TO_DEG;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static double wrap_degrees(double value) {
|
|
value = fmod(value + 180.0, 360.0);
|
|
if (value < 0.0) value += 360.0;
|
|
return value - 180.0;
|
|
}
|
|
|
|
static int solve_linear6(double matrix[6][6], double rhs[6], double result[6]) {
|
|
int pivot, row, col;
|
|
for (pivot = 0; pivot < 6; ++pivot) {
|
|
int best = pivot;
|
|
double best_abs = fabs(matrix[pivot][pivot]);
|
|
for (row = pivot + 1; row < 6; ++row) if (fabs(matrix[row][pivot]) > best_abs) { best = row; best_abs = fabs(matrix[row][pivot]); }
|
|
if (best_abs < 1e-14) return SIM_KIN_ERR_SINGULAR;
|
|
if (best != pivot) {
|
|
for (col = pivot; col < 6; ++col) { double tmp = matrix[pivot][col]; matrix[pivot][col] = matrix[best][col]; matrix[best][col] = tmp; }
|
|
{ double tmp = rhs[pivot]; rhs[pivot] = rhs[best]; rhs[best] = tmp; }
|
|
}
|
|
for (row = pivot + 1; row < 6; ++row) {
|
|
double factor = matrix[row][pivot] / matrix[pivot][pivot];
|
|
for (col = pivot; col < 6; ++col) matrix[row][col] -= factor * matrix[pivot][col];
|
|
rhs[row] -= factor * rhs[pivot];
|
|
}
|
|
}
|
|
for (row = 5; row >= 0; --row) {
|
|
double value = rhs[row];
|
|
for (col = row + 1; col < 6; ++col) value -= matrix[row][col] * result[col];
|
|
result[row] = value / matrix[row][row];
|
|
}
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int genser_inverse(const sim_kinematics *k, const sim_posemath_pose *target, double *joints, uint32_t max_iterations, double tolerance, sim_kinematics_result_info *info) {
|
|
uint32_t iteration;
|
|
double current[SIM_KIN_MAX_JOINTS] = {0};
|
|
double residual = 0.0;
|
|
if (max_iterations == 0) max_iterations = 100;
|
|
if (tolerance == 0.0) tolerance = 1e-9;
|
|
for (iteration = 0; iteration < max_iterations; ++iteration) {
|
|
sim_posemath_pose estimate;
|
|
double error[6];
|
|
double jacobian[6][6];
|
|
double normal[6][6] = {{0}};
|
|
double rhs[6] = {0};
|
|
double delta[6] = {0};
|
|
int column, row, inner;
|
|
if (genser_forward(k, current, &estimate) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
|
|
error[0] = target->tran.x - estimate.tran.x; error[1] = target->tran.y - estimate.tran.y; error[2] = target->tran.z - estimate.tran.z;
|
|
error[3] = wrap_degrees(target->a - estimate.a); error[4] = wrap_degrees(target->b - estimate.b); error[5] = wrap_degrees(target->c - estimate.c);
|
|
residual = 0.0; for (row = 0; row < 6; ++row) residual += error[row] * error[row]; residual = sqrt(residual);
|
|
if (residual <= tolerance) {
|
|
memcpy(joints, current, sizeof(double) * 6);
|
|
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = 0; }
|
|
return SIM_KIN_OK;
|
|
}
|
|
for (column = 0; column < 6; ++column) {
|
|
double perturbed[SIM_KIN_MAX_JOINTS];
|
|
sim_posemath_pose next;
|
|
memcpy(perturbed, current, sizeof(perturbed)); perturbed[column] += 1e-4;
|
|
if (genser_forward(k, perturbed, &next) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
|
|
jacobian[0][column] = (next.tran.x - estimate.tran.x) / 1e-4; jacobian[1][column] = (next.tran.y - estimate.tran.y) / 1e-4; jacobian[2][column] = (next.tran.z - estimate.tran.z) / 1e-4;
|
|
jacobian[3][column] = wrap_degrees(next.a - estimate.a) / 1e-4; jacobian[4][column] = wrap_degrees(next.b - estimate.b) / 1e-4; jacobian[5][column] = wrap_degrees(next.c - estimate.c) / 1e-4;
|
|
}
|
|
for (row = 0; row < 6; ++row) for (column = 0; column < 6; ++column) {
|
|
for (inner = 0; inner < 6; ++inner) normal[row][column] += jacobian[inner][row] * jacobian[inner][column];
|
|
normal[row][column] += row == column ? 1e-6 : 0.0;
|
|
for (inner = 0; inner < 6; ++inner) if (column == row) rhs[row] += jacobian[inner][row] * error[inner];
|
|
}
|
|
if (solve_linear6(normal, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
|
|
for (column = 0; column < 6; ++column) current[column] += delta[column];
|
|
}
|
|
if (info != 0) { info->iterations = max_iterations; info->residual = residual; info->branch_id = 0; }
|
|
return SIM_KIN_ERR_SINGULAR;
|
|
}
|
|
|
|
static int linear_delta_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
|
|
sim_posemath_pose *pose, uint32_t *branch) {
|
|
double R = k->config.delta_radius, L = k->config.delta_rod_length;
|
|
double Ay = R, Bx = -sqrt(3.0) * 0.5 * R, By = -0.5 * R, Cx = -Bx, Cy = By;
|
|
double q1 = joint(k, joints, 0), q2 = joint(k, joints, 1), q3 = joint(k, joints, 2);
|
|
double den = (By - Ay) * Cx - (Cy - Ay) * Bx;
|
|
double w1 = Ay * Ay + q1 * q1, w2 = Bx * Bx + By * By + q2 * q2, w3 = Cx * Cx + Cy * Cy + q3 * q3;
|
|
double a1 = (q2 - q1) * (Cy - Ay) - (q3 - q1) * (By - Ay);
|
|
double b1 = -((w2 - w1) * (Cy - Ay) - (w3 - w1) * (By - Ay)) / 2.0;
|
|
double a2 = -(q2 - q1) * Cx + (q3 - q1) * Bx;
|
|
double b2 = ((w2 - w1) * Cx - (w3 - w1) * Bx) / 2.0;
|
|
double aa = a1 * a1 + a2 * a2 + den * den;
|
|
double bb = 2.0 * (a1 * b1 + a2 * (b2 - Ay * den) - q1 * den * den);
|
|
double cc = (b2 - Ay * den) * (b2 - Ay * den) + b1 * b1 + den * den * (q1 * q1 - L * L);
|
|
double discr = bb * bb - 4.0 * aa * cc;
|
|
double z;
|
|
if (fabs(den) < 1e-18 || fabs(aa) < 1e-18) return SIM_KIN_ERR_SINGULAR;
|
|
if (discr < -1e-12) return SIM_KIN_ERR_RANGE;
|
|
if (discr < 0.0) discr = 0.0;
|
|
z = (-bb + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? sqrt(discr) : -sqrt(discr))) / (2.0 * aa);
|
|
pose->tran.z = z; pose->tran.x = (a1 * z + b1) / den; pose->tran.y = (a2 * z + b2) / den;
|
|
set_optional_axes(k, joints, pose);
|
|
if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int linear_delta_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, uint64_t flags,
|
|
double *joints, uint32_t *branch) {
|
|
double R = k->config.delta_radius, L = k->config.delta_rod_length;
|
|
double Ax = 0.0, Ay = R, Bx = -sqrt(3.0) * 0.5 * R, By = -0.5 * R, Cx = -Bx, Cy = By;
|
|
double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
|
|
double rad[3] = {L * L - (Ax - x) * (Ax - x) - (Ay - y) * (Ay - y), L * L - (Bx - x) * (Bx - x) - (By - y) * (By - y), L * L - (Cx - x) * (Cx - x) - (Cy - y) * (Cy - y)};
|
|
int index;
|
|
for (index = 0; index < 3; ++index) if (rad[index] < -1e-12) return SIM_KIN_ERR_RANGE;
|
|
for (index = 0; index < 3; ++index) {
|
|
double root = sqrt(fmax(0.0, rad[index]));
|
|
set_joint(joints, k, index, z + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -root : root));
|
|
}
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
|
|
if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int rotary_delta_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
|
|
sim_posemath_pose *pose, uint32_t *branch) {
|
|
double pfr = k->config.rotary_platform_radius, tl = k->config.rotary_thigh_length, sl = k->config.rotary_shin_length, fr = k->config.rotary_foot_radius;
|
|
double j0 = joint(k, joints, 0) * DEG_TO_RAD, j1 = joint(k, joints, 1) * DEG_TO_RAD, j2 = joint(k, joints, 2) * DEG_TO_RAD;
|
|
double y1 = -(pfr - fr + tl * cos(j0)), z1 = -tl * sin(j0);
|
|
double y2 = (pfr - fr + tl * cos(j1)) * 0.5, x2 = y2 * sqrt(3.0), z2 = -tl * sin(j1);
|
|
double y3 = (pfr - fr + tl * cos(j2)) * 0.5, x3 = -y3 * sqrt(3.0), z3 = -tl * sin(j2);
|
|
double den = x3 * (y2 - y1) - x2 * (y3 - y1);
|
|
double w1 = y1 * y1 + z1 * z1, w2 = x2 * x2 + y2 * y2 + z2 * z2, w3 = x3 * x3 + y3 * y3 + z3 * z3;
|
|
double a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1), b1 = -((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
|
|
double a2 = -(z2 - z1) * x3 + (z3 - z1) * x2, b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
|
|
double aa = a1 * a1 + a2 * a2 + den * den, bb = 2.0 * (a1 * b1 + a2 * (b2 - y1 * den) - z1 * den * den), cc = (b2 - y1 * den) * (b2 - y1 * den) + b1 * b1 + den * den * (z1 * z1 - sl * sl);
|
|
double discr = bb * bb - 4.0 * aa * cc;
|
|
if (fabs(den) < 1e-18 || fabs(aa) < 1e-18) return SIM_KIN_ERR_SINGULAR;
|
|
if (discr < -1e-12) return SIM_KIN_ERR_RANGE;
|
|
if (discr < 0.0) discr = 0.0;
|
|
pose->tran.z = (-bb + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? sqrt(discr) : -sqrt(discr))) / (2.0 * aa);
|
|
pose->tran.x = (a1 * pose->tran.z + b1) / den; pose->tran.y = (a2 * pose->tran.z + b2) / den;
|
|
set_optional_axes(k, joints, pose);
|
|
if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int rotary_delta_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, uint64_t flags,
|
|
double *joints, uint32_t *branch) {
|
|
double pfr = k->config.rotary_platform_radius, tl = k->config.rotary_thigh_length, sl = k->config.rotary_shin_length, fr = k->config.rotary_foot_radius;
|
|
double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
|
|
double a, b, d, knee_y, knee_z, xr, yr;
|
|
if (fabs(z) < 1e-12) return SIM_KIN_ERR_SINGULAR;
|
|
a = 0.5 * (x * x + (y - fr) * (y - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
|
|
b = (fr - pfr - y) / z;
|
|
d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
|
|
if (d < 0.0) return SIM_KIN_ERR_RANGE;
|
|
knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a;
|
|
set_joint(joints, k, 0, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
xr = x; yr = y; { double c = cos(-2.0 * 3.14159265358979323846 / 3.0), s = sin(-2.0 * 3.14159265358979323846 / 3.0); double tx = xr * c - yr * s; yr = xr * s + yr * c; xr = tx; }
|
|
a = 0.5 * (xr * xr + (yr - fr) * (yr - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z; b = (fr - pfr - yr) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr); if (d < 0.0) return SIM_KIN_ERR_RANGE; knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a; set_joint(joints, k, 1, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
xr = x; yr = y; { double c = cos(2.0 * 3.14159265358979323846 / 3.0), s = sin(2.0 * 3.14159265358979323846 / 3.0); double tx = xr * c - yr * s; yr = xr * s + yr * c; xr = tx; }
|
|
a = 0.5 * (xr * xr + (yr - fr) * (yr - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z; b = (fr - pfr - yr) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr); if (d < 0.0) return SIM_KIN_ERR_RANGE; knee_y = (pfr + a * b + ((flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? -sqrt(d) : sqrt(d))) / (b * b + 1.0); knee_z = b * knee_y - a; set_joint(joints, k, 2, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
|
|
if (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) {
|
|
/* The alternate knee branch is the reflected root of each leg. */
|
|
set_joint(joints, k, 0, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
/* Recompute the rotated legs through the common helper below; the
|
|
* default branch remains byte-for-byte compatible with LinuxCNC. */
|
|
{
|
|
double xr2 = x, yr2 = y;
|
|
double c2 = cos(-2.0 * 3.14159265358979323846 / 3.0), s2 = sin(-2.0 * 3.14159265358979323846 / 3.0);
|
|
double tx2 = xr2 * c2 - yr2 * s2; yr2 = xr2 * s2 + yr2 * c2; xr2 = tx2;
|
|
a = 0.5 * (xr2 * xr2 + (yr2 - fr) * (yr2 - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
|
|
b = (fr - pfr - yr2) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
|
|
if (d < 0.0) return SIM_KIN_ERR_RANGE;
|
|
knee_y = (pfr + a * b - sqrt(d)) / (b * b + 1.0); knee_z = b * knee_y - a;
|
|
set_joint(joints, k, 1, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
xr2 = x; yr2 = y; c2 = cos(2.0 * 3.14159265358979323846 / 3.0); s2 = sin(2.0 * 3.14159265358979323846 / 3.0);
|
|
tx2 = xr2 * c2 - yr2 * s2; yr2 = xr2 * s2 + yr2 * c2; xr2 = tx2;
|
|
a = 0.5 * (xr2 * xr2 + (yr2 - fr) * (yr2 - fr) + z * z + tl * tl - sl * sl - pfr * pfr) / z;
|
|
b = (fr - pfr - yr2) / z; d = tl * tl * (b * b + 1.0) - (a - b * pfr) * (a - b * pfr);
|
|
if (d < 0.0) return SIM_KIN_ERR_RANGE;
|
|
knee_y = (pfr + a * b - sqrt(d)) / (b * b + 1.0); knee_z = b * knee_y - a;
|
|
set_joint(joints, k, 2, atan2(knee_z, knee_y - pfr) * RAD_TO_DEG);
|
|
}
|
|
}
|
|
if (branch != 0) *branch = (flags & SIM_KIN_DELTA_ALTERNATE_ROOT) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
/* Point-vertex tripod kinematics from LinuxCNC's tripodkins.c. The three
|
|
* strut lengths determine X/Y uniquely; the quadratic Z root is selected by
|
|
* the forward flag (above-base by default, below-base when bit zero is set). */
|
|
static int tripod_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
|
|
sim_posemath_pose *pose, uint32_t *branch) {
|
|
double bx = k->config.tripod_bx, cx = k->config.tripod_cx, cy = k->config.tripod_cy;
|
|
double ad = joint(k, joints, 0), bd = joint(k, joints, 1), cd = joint(k, joints, 2);
|
|
double p = ad * ad;
|
|
double q = bd * bd - bx * bx;
|
|
double r = cd * cd - cx * cx - cy * cy;
|
|
double s = -2.0 * bx, t = -2.0 * cx, u = -2.0 * cy;
|
|
double x, y, z2;
|
|
if (fabs(s) < 1e-12 || fabs(u) < 1e-12) return SIM_KIN_ERR_CONFIGURATION;
|
|
x = (q - p) / s;
|
|
y = (r - q - (t - s) * x) / u;
|
|
z2 = p - x * x - y * y;
|
|
if (z2 < -1e-12) return SIM_KIN_ERR_RANGE;
|
|
if (z2 <= 1e-12) return SIM_KIN_ERR_SINGULAR;
|
|
pose->tran.x = x;
|
|
pose->tran.y = y;
|
|
pose->tran.z = (flags & SIM_KIN_TRIPOD_BELOW_ROOT) ? -sqrt(z2) : sqrt(z2);
|
|
pose->a = 0.0; pose->b = 0.0; pose->c = 0.0;
|
|
set_optional_axes(k, joints, pose);
|
|
if (branch != 0) *branch = (flags & SIM_KIN_TRIPOD_BELOW_ROOT) ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int tripod_inverse(const sim_kinematics *k, const sim_posemath_pose *pose,
|
|
double *joints, uint32_t *branch) {
|
|
double bx = k->config.tripod_bx, cx = k->config.tripod_cx, cy = k->config.tripod_cy;
|
|
double x = pose->tran.x, y = pose->tran.y, z = pose->tran.z;
|
|
set_joint(joints, k, 0, sqrt(x * x + y * y + z * z));
|
|
set_joint(joints, k, 1, sqrt((x - bx) * (x - bx) + y * y + z * z));
|
|
set_joint(joints, k, 2, sqrt((x - cx) * (x - cx) + (y - cy) * (y - cy) + z * z));
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
|
|
set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
|
|
if (branch != 0) *branch = z < 0.0 ? 1U : 0U;
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
/* Generalized six-strut Stewart platform. Inverse kinematics is the
|
|
* closed-form leg-length calculation; forward kinematics follows LinuxCNC's
|
|
* Newton solve from a deterministic pose seed. The rotational Jacobian uses
|
|
* the platform attachment vector crossed with the unit strut vector, which
|
|
* is the small-angle derivative used by genhexkins.c. */
|
|
static int genhex_lengths(const sim_kinematics *k, const double q[6], double lengths[SIM_KIN_GENHEX_STRUTS], double jacobian[SIM_KIN_GENHEX_STRUTS][6]) {
|
|
PmRpy rpy;
|
|
PmRotationMatrix rotation;
|
|
size_t index;
|
|
rpy.r = q[3]; rpy.p = q[4]; rpy.y = q[5];
|
|
if (pmRpyMatConvert(&rpy, &rotation) != 0) return SIM_KIN_ERR_SINGULAR;
|
|
for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) {
|
|
PmCartesian platform = {k->config.genhex_platform[index][0], k->config.genhex_platform[index][1], k->config.genhex_platform[index][2]};
|
|
PmCartesian base = {k->config.genhex_base[index][0], k->config.genhex_base[index][1], k->config.genhex_base[index][2]};
|
|
PmCartesian rotated;
|
|
PmCartesian world;
|
|
PmCartesian vector;
|
|
PmCartesian unit;
|
|
PmCartesian cross;
|
|
double length;
|
|
pmMatCartMult(&rotation, &platform, &rotated);
|
|
world.x = q[0] + rotated.x; world.y = q[1] + rotated.y; world.z = q[2] + rotated.z;
|
|
vector.x = world.x - base.x; vector.y = world.y - base.y; vector.z = world.z - base.z;
|
|
if (pmCartUnit(&vector, &unit) != 0 || pmCartMag(&vector, &length) != 0 || !isfinite(length)) return SIM_KIN_ERR_SINGULAR;
|
|
lengths[index] = length;
|
|
if (jacobian != 0) {
|
|
pmCartCartCross(&rotated, &unit, &cross);
|
|
jacobian[index][0] = unit.x; jacobian[index][1] = unit.y; jacobian[index][2] = unit.z;
|
|
jacobian[index][3] = cross.x; jacobian[index][4] = cross.y; jacobian[index][5] = cross.z;
|
|
}
|
|
}
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int genhex_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
|
|
double q[6] = {pose->tran.x, pose->tran.y, pose->tran.z, pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD, pose->c * DEG_TO_RAD};
|
|
double lengths[SIM_KIN_GENHEX_STRUTS];
|
|
size_t index;
|
|
int result = genhex_lengths(k, q, lengths, 0);
|
|
if (result != SIM_KIN_OK) return result;
|
|
for (index = 0; index < SIM_KIN_GENHEX_STRUTS; ++index) joints[index] = lengths[index];
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int genhex_forward(const sim_kinematics *k, const double *joints, uint64_t flags,
|
|
sim_posemath_pose *pose, sim_kinematics_result_info *info) {
|
|
double q[6];
|
|
double lengths[SIM_KIN_GENHEX_STRUTS];
|
|
double jacobian[SIM_KIN_GENHEX_STRUTS][6];
|
|
double residual = 0.0;
|
|
uint32_t iteration;
|
|
int result;
|
|
for (size_t index = 0; index < 6; ++index) q[index] = k->config.genhex_initial_pose[index];
|
|
if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
|
|
for (iteration = 1; iteration <= k->config.genhex_iteration_limit; ++iteration) {
|
|
double matrix[6][6];
|
|
double rhs[6];
|
|
double delta[6] = {0};
|
|
int row;
|
|
result = genhex_lengths(k, q, lengths, jacobian);
|
|
if (result != SIM_KIN_OK) return result;
|
|
residual = 0.0;
|
|
for (row = 0; row < (int)SIM_KIN_GENHEX_STRUTS; ++row) {
|
|
double difference = lengths[row] - joints[row];
|
|
rhs[row] = difference;
|
|
residual += fabs(difference);
|
|
for (int column = 0; column < 6; ++column) matrix[row][column] = jacobian[row][column];
|
|
}
|
|
if (residual <= k->config.genhex_convergence_tolerance) {
|
|
pose->tran.x = q[0]; pose->tran.y = q[1]; pose->tran.z = q[2];
|
|
pose->a = q[3] * RAD_TO_DEG; pose->b = q[4] * RAD_TO_DEG; pose->c = q[5] * RAD_TO_DEG;
|
|
pose->u = 0.0; pose->v = 0.0; pose->w = 0.0;
|
|
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_OK;
|
|
}
|
|
if (residual > k->config.genhex_max_error) {
|
|
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_ERR_RANGE;
|
|
}
|
|
result = solve_linear6(matrix, rhs, delta);
|
|
if (result != SIM_KIN_OK) {
|
|
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return result;
|
|
}
|
|
for (int column = 0; column < 6; ++column) q[column] -= delta[column];
|
|
}
|
|
if (info != 0) { info->iterations = k->config.genhex_iteration_limit; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_ERR_RANGE;
|
|
}
|
|
|
|
static int solve_linear5(double matrix[5][5], double rhs[5], double result[5]) {
|
|
for (int pivot = 0; pivot < 5; ++pivot) {
|
|
int best = pivot;
|
|
double magnitude = fabs(matrix[pivot][pivot]);
|
|
for (int row = pivot + 1; row < 5; ++row) if (fabs(matrix[row][pivot]) > magnitude) { best = row; magnitude = fabs(matrix[row][pivot]); }
|
|
if (magnitude < 1e-14) return SIM_KIN_ERR_SINGULAR;
|
|
if (best != pivot) { for (int col = pivot; col < 5; ++col) { double tmp = matrix[pivot][col]; matrix[pivot][col] = matrix[best][col]; matrix[best][col] = tmp; } { double tmp = rhs[pivot]; rhs[pivot] = rhs[best]; rhs[best] = tmp; } }
|
|
for (int row = pivot + 1; row < 5; ++row) { double factor = matrix[row][pivot] / matrix[pivot][pivot]; for (int col = pivot; col < 5; ++col) matrix[row][col] -= factor * matrix[pivot][col]; rhs[row] -= factor * rhs[pivot]; }
|
|
}
|
|
for (int row = 4; row >= 0; --row) { double value = rhs[row]; for (int col = row + 1; col < 5; ++col) value -= matrix[row][col] * result[col]; result[row] = value / matrix[row][row]; }
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int pentapod_lengths(const sim_kinematics *k, const double q[5], double lengths[SIM_KIN_PENTA_STRUTS]) {
|
|
PmRpy rpy = {q[3], q[4], 0.0};
|
|
PmRotationMatrix rotation, inverse;
|
|
if (pmRpyMatConvert(&rpy, &rotation) != 0 || pmMatInv(&rotation, &inverse) != 0) return SIM_KIN_ERR_SINGULAR;
|
|
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
|
|
/* pentakins_read_hal_pins applies tool-offset to both ends of each
|
|
* strut; preserve that world-frame translation before rotating into
|
|
* the effector frame. */
|
|
PmCartesian base = {k->config.penta_base[index][0] - q[0], k->config.penta_base[index][1] - q[1], k->config.penta_base[index][2] + k->config.penta_tool_offset - q[2]};
|
|
PmCartesian local;
|
|
double radial;
|
|
pmMatCartMult(&inverse, &base, &local);
|
|
radial = hypot(local.x, local.y);
|
|
lengths[index] = hypot(radial - k->config.penta_effector_radius[index], local.z - (k->config.penta_effector_z[index] + k->config.penta_tool_offset));
|
|
if (!isfinite(lengths[index])) return SIM_KIN_ERR_NONFINITE;
|
|
}
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int pentapod_inverse(const sim_kinematics *k, const sim_posemath_pose *pose, double *joints) {
|
|
double q[5] = {pose->tran.x, pose->tran.y, pose->tran.z, pose->a * DEG_TO_RAD, pose->b * DEG_TO_RAD};
|
|
double lengths[SIM_KIN_PENTA_STRUTS];
|
|
int result = pentapod_lengths(k, q, lengths);
|
|
if (result != SIM_KIN_OK) return result;
|
|
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) joints[index] = lengths[index];
|
|
return SIM_KIN_OK;
|
|
}
|
|
|
|
static int pentapod_forward(const sim_kinematics *k, const double *joints, uint64_t flags, sim_posemath_pose *pose, sim_kinematics_result_info *info) {
|
|
double q[5];
|
|
double lengths[SIM_KIN_PENTA_STRUTS];
|
|
double residual = 0.0;
|
|
uint32_t iteration;
|
|
for (size_t index = 0; index < SIM_KIN_PENTA_STRUTS; ++index) {
|
|
/* LinuxCNC's pentakins rejects non-positive strut lengths before
|
|
* entering Newton iteration. */
|
|
if (joints[index] <= 0.0) {
|
|
if (info != 0) { info->iterations = 0; info->residual = 0.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_ERR_RANGE;
|
|
}
|
|
}
|
|
for (size_t index = 0; index < 5; ++index) q[index] = k->config.penta_initial_pose[index];
|
|
if (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) q[2] = -q[2];
|
|
for (iteration = 1; iteration <= k->config.penta_iteration_limit; ++iteration) {
|
|
double jacobian[5][5];
|
|
double rhs[5];
|
|
double delta[5] = {0};
|
|
int result = pentapod_lengths(k, q, lengths);
|
|
if (result != SIM_KIN_OK) return result;
|
|
residual = 0.0;
|
|
for (int row = 0; row < 5; ++row) {
|
|
double perturbed[5];
|
|
rhs[row] = lengths[row] - joints[row]; residual += fabs(rhs[row]);
|
|
for (int column = 0; column < 5; ++column) {
|
|
double next[SIM_KIN_PENTA_STRUTS];
|
|
memcpy(perturbed, q, sizeof(perturbed)); perturbed[column] += 1e-4;
|
|
if (pentapod_lengths(k, perturbed, next) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
|
|
jacobian[row][column] = (next[row] - lengths[row]) * 1e4;
|
|
}
|
|
}
|
|
if (residual <= k->config.penta_convergence_tolerance) {
|
|
pose->tran.x = pentapod_canonical(q[0]); pose->tran.y = pentapod_canonical(q[1]); pose->tran.z = pentapod_canonical(q[2]); pose->a = pentapod_canonical(q[3] * RAD_TO_DEG); pose->b = pentapod_canonical(q[4] * RAD_TO_DEG); pose->c = 0.0; pose->u = pose->v = pose->w = 0.0;
|
|
if (info != 0) { info->iterations = iteration; info->residual = nearbyint(residual * 1000000000.0) / 1000000000.0; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_OK;
|
|
}
|
|
if (residual > k->config.penta_max_error) {
|
|
if (info != 0) { info->iterations = iteration; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_ERR_RANGE;
|
|
}
|
|
if (solve_linear5(jacobian, rhs, delta) != SIM_KIN_OK) return SIM_KIN_ERR_SINGULAR;
|
|
for (int column = 0; column < 5; ++column) q[column] -= delta[column];
|
|
}
|
|
if (info != 0) { info->iterations = k->config.penta_iteration_limit; info->residual = residual; info->branch_id = (flags & SIM_KIN_GENHEX_ALTERNATE_SEED) ? 1U : 0U; }
|
|
return SIM_KIN_ERR_RANGE;
|
|
}
|
|
|
|
int sim_kinematics_forward_ex(const sim_kinematics *k, const double *joints, uint32_t joint_count,
|
|
uint64_t inverse_flags, sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info) {
|
|
double a, b, c, x, y, z;
|
|
initialize_info(info, 0, inverse_flags);
|
|
if (k == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (effective_mode(&k->config) == SIM_KIN_MODE_INVERSE_ONLY) return SIM_KIN_ERR_UNSUPPORTED;
|
|
if (!finite_values(joints, joint_count)) return SIM_KIN_ERR_NONFINITE;
|
|
if (in_limits(k, joints) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
|
|
zero_pose(pose);
|
|
x = joint(k, joints, 0); y = joint(k, joints, 1); z = joint(k, joints, 2);
|
|
if (k->config.type == SIM_KIN_TRIVKINS) { pose->tran.x = x; pose->tran.y = y; pose->tran.z = z; set_optional_axes(k, joints, pose); return SIM_KIN_OK; }
|
|
if (k->config.type == SIM_KIN_SCARA) return scara_forward(k, joints, pose);
|
|
if (k->config.type == SIM_KIN_SCORBOT) return scorbot_forward(k, joints, pose);
|
|
if (k->config.type == SIM_KIN_PUMA) {
|
|
uint32_t branch = 0;
|
|
int result = puma_forward(k, joints, pose, &branch);
|
|
if (info != 0) info->branch_id = branch;
|
|
return result;
|
|
}
|
|
if (k->config.type == SIM_KIN_GENSER) return genser_forward(k, joints, pose);
|
|
if (k->config.type == SIM_KIN_LINEAR_DELTA) {
|
|
uint32_t branch = 0;
|
|
int result = linear_delta_forward(k, joints, inverse_flags, pose, &branch);
|
|
if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
return result;
|
|
}
|
|
if (k->config.type == SIM_KIN_ROTARY_DELTA) {
|
|
uint32_t branch = 0;
|
|
int result = rotary_delta_forward(k, joints, inverse_flags, pose, &branch);
|
|
if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
return result;
|
|
}
|
|
if (k->config.type == SIM_KIN_TRIPOD) {
|
|
uint32_t branch = 0;
|
|
int result = tripod_forward(k, joints, inverse_flags, pose, &branch);
|
|
if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
return result;
|
|
}
|
|
if (k->config.type == SIM_KIN_GENHEX) {
|
|
return genhex_forward(k, joints, inverse_flags, pose, info);
|
|
}
|
|
if (k->config.type == SIM_KIN_PENTAPOD) {
|
|
return pentapod_forward(k, joints, inverse_flags, pose, info);
|
|
}
|
|
if (k->config.type == SIM_KIN_COREXY) {
|
|
pose->tran.x = 0.5 * (x + y);
|
|
pose->tran.y = 0.5 * (x - y);
|
|
pose->tran.z = z;
|
|
set_optional_axes(k, joints, pose);
|
|
return SIM_KIN_OK;
|
|
}
|
|
if (k->config.type == SIM_KIN_ROSE) {
|
|
double theta = z * DEG_TO_RAD;
|
|
pose->tran.x = x * cos(theta);
|
|
pose->tran.y = x * sin(theta);
|
|
pose->tran.z = y;
|
|
return sim_kinematics_validate_pose(pose);
|
|
}
|
|
c = joint(k, joints, 5) * DEG_TO_RAD;
|
|
if (k->config.type == SIM_KIN_XYZAC_TRT) {
|
|
double dy = k->config.y_offset;
|
|
double dz = k->config.z_offset + k->config.tool_offset;
|
|
a = joint(k, joints, 3) * DEG_TO_RAD;
|
|
pose->tran.x = cos(c) * (x - k->config.x_rot_point) + sin(c) * cos(a) * (y - dy - k->config.y_rot_point) + sin(c) * sin(a) * (z - dz - k->config.z_rot_point) + sin(c) * dy + k->config.x_rot_point;
|
|
pose->tran.y = -sin(c) * (x - k->config.x_rot_point) + cos(c) * cos(a) * (y - dy - k->config.y_rot_point) + cos(c) * sin(a) * (z - dz - k->config.z_rot_point) + cos(c) * dy + k->config.y_rot_point;
|
|
pose->tran.z = -sin(a) * (y - dy - k->config.y_rot_point) + cos(a) * (z - dz - k->config.z_rot_point) + dz + k->config.z_rot_point;
|
|
} else if (k->config.type == SIM_KIN_XYZBC_TRT) {
|
|
double dx = k->config.x_offset;
|
|
double dz = k->config.z_offset + k->config.tool_offset;
|
|
b = joint(k, joints, 4) * DEG_TO_RAD;
|
|
pose->tran.x = cos(c) * cos(b) * (x - dx - k->config.x_rot_point) + sin(c) * (y - k->config.y_rot_point) - cos(c) * sin(b) * (z - dz - k->config.z_rot_point) + cos(c) * dx + k->config.x_rot_point;
|
|
pose->tran.y = -sin(c) * cos(b) * (x - dx - k->config.x_rot_point) + cos(c) * (y - k->config.y_rot_point) + sin(c) * sin(b) * (z - dz - k->config.z_rot_point) - sin(c) * dx + k->config.y_rot_point;
|
|
pose->tran.z = sin(b) * (x - dx - k->config.x_rot_point) + cos(b) * (z - dz - k->config.z_rot_point) + dz + k->config.z_rot_point;
|
|
} else if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) {
|
|
double radius = k->config.pivot_length + joint(k, joints, 8);
|
|
double theta = c;
|
|
double phi = (180.0 - joint(k, joints, 4)) * DEG_TO_RAD;
|
|
double rx = radius * sin(phi) * cos(theta);
|
|
double ry = radius * sin(phi) * sin(theta);
|
|
double rz = radius * cos(phi);
|
|
pose->tran.x = x + rx; pose->tran.y = y + ry; pose->tran.z = z + k->config.pivot_length + rz;
|
|
} else if (k->config.type == SIM_KIN_MAXKINS) {
|
|
double pivot_radius = k->config.pivot_length + joint(k, joints, 8);
|
|
double zb;
|
|
double xb;
|
|
double xyr;
|
|
double xytheta;
|
|
double zv;
|
|
double xv;
|
|
b = joint(k, joints, 4) * DEG_TO_RAD;
|
|
zb = pivot_radius * cos(b);
|
|
xb = pivot_radius * sin(b);
|
|
xyr = hypot(x, y);
|
|
xytheta = atan2(y, x) + c;
|
|
zv = joint(k, joints, 6) * sin(b);
|
|
xv = joint(k, joints, 6) * cos(b);
|
|
pose->tran.x = xyr * cos(xytheta) + xb - xv;
|
|
pose->tran.y = xyr * sin(xytheta) - joint(k, joints, 7);
|
|
pose->tran.z = z - zb + zv + k->config.pivot_length;
|
|
} else if (k->config.type == SIM_KIN_ROTATEKINS) {
|
|
pose->tran.x = x * cos(-c) - y * sin(-c);
|
|
pose->tran.y = x * sin(-c) + y * cos(-c);
|
|
pose->tran.z = z;
|
|
} else return SIM_KIN_ERR_UNKNOWN_TYPE;
|
|
set_optional_axes(k, joints, pose);
|
|
return sim_kinematics_validate_pose(pose);
|
|
}
|
|
|
|
int sim_kinematics_forward(const sim_kinematics *k, const double *joints, uint32_t joint_count, sim_posemath_pose *pose) {
|
|
return sim_kinematics_forward_ex(k, joints, joint_count, 0, pose, 0);
|
|
}
|
|
|
|
static void set_joint(double *joints, const sim_kinematics *k, int axis, double value) {
|
|
int index = config_axis(k, axis);
|
|
if (index >= 0) joints[index] = value;
|
|
}
|
|
|
|
static void fill_duplicate_joints(const sim_kinematics *k, double *joints) {
|
|
uint32_t index;
|
|
for (index = 0; index < kinematics_joint_count(&k->config); ++index) {
|
|
int axis = axis_index((char)(k->config.coordinates[index] >= 'a' && k->config.coordinates[index] <= 'z' ? k->config.coordinates[index] - 'a' + 'A' : k->config.coordinates[index]));
|
|
if (axis >= 0) joints[index] = joints[config_axis(k, axis)];
|
|
}
|
|
}
|
|
|
|
int sim_kinematics_inverse_ex(sim_kinematics *k, const sim_posemath_pose *pose, uint32_t joint_count,
|
|
uint64_t forward_flags, double *joints,
|
|
sim_kinematics_result_info *info) {
|
|
double a, b, c;
|
|
initialize_info(info, forward_flags, 0);
|
|
if (k == 0 || pose == 0 || joints == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
|
|
if (effective_mode(&k->config) == SIM_KIN_MODE_FORWARD_ONLY) return SIM_KIN_ERR_UNSUPPORTED;
|
|
if (sim_kinematics_validate_pose(pose) != SIM_KIN_OK) return SIM_KIN_ERR_NONFINITE;
|
|
memset(joints, 0, sizeof(double) * joint_count);
|
|
if (k->config.type == SIM_KIN_TRIVKINS) {
|
|
set_joint(joints, k, 0, pose->tran.x); set_joint(joints, k, 1, pose->tran.y); set_joint(joints, k, 2, pose->tran.z);
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
|
|
} else if (k->config.type == SIM_KIN_SCARA) {
|
|
uint32_t branch = 0;
|
|
int result = scara_inverse(k, pose, joints, forward_flags, &branch);
|
|
if (info != 0) info->branch_id = branch;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_SCORBOT) {
|
|
int result = scorbot_inverse(k, pose, joints);
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_PUMA) {
|
|
uint32_t branch = 0;
|
|
int result = puma_inverse(k, pose, joints, forward_flags, &branch);
|
|
if (info != 0) info->branch_id = branch;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_GENSER) {
|
|
return genser_inverse(k, pose, joints, k->config.max_iterations, k->config.convergence_tolerance, info);
|
|
} else if (k->config.type == SIM_KIN_LINEAR_DELTA) {
|
|
uint32_t branch = 0;
|
|
int result = linear_delta_inverse(k, pose, forward_flags, joints, &branch);
|
|
if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
if (result == SIM_KIN_OK) k->branch_id = branch;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_ROTARY_DELTA) {
|
|
uint32_t branch = 0;
|
|
int result = rotary_delta_inverse(k, pose, forward_flags, joints, &branch);
|
|
if (info != 0) { info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
if (result == SIM_KIN_OK) k->branch_id = branch;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_TRIPOD) {
|
|
uint32_t branch = 0;
|
|
int result = tripod_inverse(k, pose, joints, &branch);
|
|
if (info != 0) { info->forward_flags = branch; info->branch_id = branch; info->iterations = 0; info->residual = 0.0; }
|
|
if (result == SIM_KIN_OK) k->branch_id = branch;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_GENHEX) {
|
|
int result = genhex_inverse(k, pose, joints);
|
|
if (info != 0) { info->forward_flags = pose->tran.z < 0.0 ? SIM_KIN_GENHEX_ALTERNATE_SEED : 0; info->branch_id = pose->tran.z < 0.0 ? 1U : 0U; info->iterations = 0; info->residual = 0.0; }
|
|
if (result == SIM_KIN_OK) k->branch_id = pose->tran.z < 0.0 ? 1U : 0U;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_PENTAPOD) {
|
|
int result = pentapod_inverse(k, pose, joints);
|
|
if (info != 0) { info->forward_flags = pose->tran.z < 0.0 ? SIM_KIN_GENHEX_ALTERNATE_SEED : 0; info->branch_id = pose->tran.z < 0.0 ? 1U : 0U; info->iterations = 0; info->residual = 0.0; }
|
|
if (result == SIM_KIN_OK) k->branch_id = pose->tran.z < 0.0 ? 1U : 0U;
|
|
return result == SIM_KIN_OK ? in_limits(k, joints) : result;
|
|
} else if (k->config.type == SIM_KIN_COREXY) {
|
|
set_joint(joints, k, 0, pose->tran.x + pose->tran.y);
|
|
set_joint(joints, k, 1, pose->tran.x - pose->tran.y);
|
|
set_joint(joints, k, 2, pose->tran.z);
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 4, pose->b);
|
|
set_joint(joints, k, 5, pose->c); set_joint(joints, k, 6, pose->u);
|
|
set_joint(joints, k, 7, pose->v); set_joint(joints, k, 8, pose->w);
|
|
} else if (k->config.type == SIM_KIN_ROSE) {
|
|
uint32_t quadrant;
|
|
double theta;
|
|
double bigtheta;
|
|
if (pose->tran.x >= 0.0 && pose->tran.y >= 0.0) quadrant = 1;
|
|
else if (pose->tran.x < 0.0 && pose->tran.y >= 0.0) quadrant = 2;
|
|
else if (pose->tran.x < 0.0 && pose->tran.y < 0.0) quadrant = 3;
|
|
else quadrant = 4;
|
|
if (k->previous_quadrant == 2 && quadrant == 3) k->revolution_count += 1;
|
|
if (k->previous_quadrant == 3 && quadrant == 2) k->revolution_count -= 1;
|
|
theta = atan2(pose->tran.y, pose->tran.x);
|
|
bigtheta = theta + 2.0 * 3.14159265358979323846264338327950288 * k->revolution_count;
|
|
set_joint(joints, k, 0, hypot(pose->tran.x, pose->tran.y));
|
|
set_joint(joints, k, 1, pose->tran.z);
|
|
set_joint(joints, k, 2, bigtheta * RAD_TO_DEG);
|
|
k->previous_quadrant = quadrant;
|
|
k->branch_id = (uint32_t)k->revolution_count;
|
|
if (info != 0) info->branch_id = k->branch_id;
|
|
} else if (k->config.type == SIM_KIN_XYZAC_TRT) {
|
|
double dy = k->config.y_offset;
|
|
double dz = k->config.z_offset + k->config.tool_offset;
|
|
a = pose->a * DEG_TO_RAD; c = pose->c * DEG_TO_RAD;
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 5, pose->c);
|
|
set_joint(joints, k, 0, cos(c) * (pose->tran.x - k->config.x_rot_point) - sin(c) * (pose->tran.y - k->config.y_rot_point) + k->config.x_rot_point);
|
|
set_joint(joints, k, 1, sin(c) * cos(a) * (pose->tran.x - k->config.x_rot_point) + cos(c) * cos(a) * (pose->tran.y - k->config.y_rot_point) - sin(a) * (pose->tran.z - k->config.z_rot_point) - cos(a) * dy + sin(a) * dz + dy + k->config.y_rot_point);
|
|
set_joint(joints, k, 2, sin(c) * sin(a) * (pose->tran.x - k->config.x_rot_point) + cos(c) * sin(a) * (pose->tran.y - k->config.y_rot_point) + cos(a) * (pose->tran.z - k->config.z_rot_point) - sin(a) * dy - cos(a) * dz + dz + k->config.z_rot_point);
|
|
} else if (k->config.type == SIM_KIN_XYZBC_TRT) {
|
|
double dx = k->config.x_offset;
|
|
double dz = k->config.z_offset + k->config.tool_offset;
|
|
b = pose->b * DEG_TO_RAD; c = pose->c * DEG_TO_RAD;
|
|
if (fabs(cos(b)) < 1e-12) return SIM_KIN_ERR_SINGULAR;
|
|
set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
|
|
set_joint(joints, k, 0, cos(c) * cos(b) * (pose->tran.x - k->config.x_rot_point) - sin(c) * cos(b) * (pose->tran.y - k->config.y_rot_point) + sin(b) * (pose->tran.z - k->config.z_rot_point) - cos(b) * dx - sin(b) * dz + dx + k->config.x_rot_point);
|
|
set_joint(joints, k, 1, sin(c) * (pose->tran.x - k->config.x_rot_point) + cos(c) * (pose->tran.y - k->config.y_rot_point) + k->config.y_rot_point);
|
|
set_joint(joints, k, 2, -cos(c) * sin(b) * (pose->tran.x - k->config.x_rot_point) + sin(c) * sin(b) * (pose->tran.y - k->config.y_rot_point) + cos(b) * (pose->tran.z - k->config.z_rot_point) + sin(b) * dx - cos(b) * dz + dz + k->config.z_rot_point);
|
|
} else if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) {
|
|
double radius = k->config.pivot_length + pose->w;
|
|
double phi = (180.0 - pose->b) * DEG_TO_RAD;
|
|
double theta = pose->c * DEG_TO_RAD;
|
|
set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c); set_joint(joints, k, 8, pose->w);
|
|
set_joint(joints, k, 0, pose->tran.x - radius * sin(phi) * cos(theta));
|
|
set_joint(joints, k, 1, pose->tran.y - radius * sin(phi) * sin(theta));
|
|
set_joint(joints, k, 2, pose->tran.z - k->config.pivot_length - radius * cos(phi));
|
|
} else if (k->config.type == SIM_KIN_MAXKINS) {
|
|
double pivot_radius = k->config.pivot_length + pose->w;
|
|
double zb;
|
|
double xb;
|
|
double xyr;
|
|
double xytheta;
|
|
double zv;
|
|
double xv;
|
|
b = pose->b * DEG_TO_RAD;
|
|
c = pose->c * DEG_TO_RAD;
|
|
zb = pivot_radius * cos(b);
|
|
xb = pivot_radius * sin(b);
|
|
xyr = hypot(pose->tran.x, pose->tran.y);
|
|
xytheta = atan2(pose->tran.y, pose->tran.x) - c;
|
|
zv = pose->u * sin(b);
|
|
xv = pose->u * cos(b);
|
|
set_joint(joints, k, 0, xyr * cos(xytheta) - xb + xv);
|
|
set_joint(joints, k, 1, xyr * sin(xytheta) + pose->v);
|
|
set_joint(joints, k, 2, pose->tran.z + zb + zv - k->config.pivot_length);
|
|
set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
|
|
set_joint(joints, k, 8, pose->w);
|
|
} else if (k->config.type == SIM_KIN_ROTATEKINS) {
|
|
c = pose->c * DEG_TO_RAD;
|
|
set_joint(joints, k, 0, pose->tran.x * cos(c) - pose->tran.y * sin(c));
|
|
set_joint(joints, k, 1, pose->tran.x * sin(c) + pose->tran.y * cos(c));
|
|
set_joint(joints, k, 2, pose->tran.z);
|
|
set_joint(joints, k, 4, pose->b); set_joint(joints, k, 5, pose->c);
|
|
set_joint(joints, k, 8, pose->w);
|
|
} else return SIM_KIN_ERR_UNKNOWN_TYPE;
|
|
set_joint(joints, k, 3, pose->a); set_joint(joints, k, 6, pose->u); set_joint(joints, k, 7, pose->v);
|
|
fill_duplicate_joints(k, joints);
|
|
return in_limits(k, joints);
|
|
}
|
|
|
|
int sim_kinematics_inverse(sim_kinematics *k, const sim_posemath_pose *pose, double *joints, uint32_t joint_count) {
|
|
return sim_kinematics_inverse_ex(k, pose, joint_count, 0, joints, 0);
|
|
}
|
|
|
|
int sim_kinematics_home(const sim_kinematics *k, double *joints, uint32_t joint_count, sim_posemath_pose *pose) {
|
|
if (k == 0 || joints == 0 || pose == 0) return SIM_KIN_ERR_ARGUMENT;
|
|
if (!k->configured || joint_count != kinematics_joint_count(&k->config)) return SIM_KIN_ERR_CONFIGURATION;
|
|
memset(joints, 0, sizeof(double) * joint_count);
|
|
if (k->config.type == SIM_KIN_XYZBCW || k->config.type == SIM_KIN_5AXISKINS) joints[config_axis(k, 8)] = 0.0;
|
|
if (k->config.type == SIM_KIN_TRIPOD) {
|
|
double z = 1.0;
|
|
joints[config_axis(k, 0)] = z;
|
|
joints[config_axis(k, 1)] = hypot(k->config.tripod_bx, z);
|
|
joints[config_axis(k, 2)] = hypot(hypot(k->config.tripod_cx, k->config.tripod_cy), z);
|
|
}
|
|
if (k->config.type == SIM_KIN_GENHEX) {
|
|
sim_posemath_pose seed = {0};
|
|
seed.tran.z = k->config.genhex_initial_pose[2];
|
|
if (sim_kinematics_inverse((sim_kinematics *)k, &seed, joints, joint_count) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
|
|
}
|
|
if (k->config.type == SIM_KIN_PENTAPOD) {
|
|
sim_posemath_pose seed = {0};
|
|
seed.tran.x = k->config.penta_initial_pose[0]; seed.tran.y = k->config.penta_initial_pose[1]; seed.tran.z = k->config.penta_initial_pose[2]; seed.a = k->config.penta_initial_pose[3] * RAD_TO_DEG; seed.b = k->config.penta_initial_pose[4] * RAD_TO_DEG;
|
|
if (sim_kinematics_inverse((sim_kinematics *)k, &seed, joints, joint_count) != SIM_KIN_OK) return SIM_KIN_ERR_RANGE;
|
|
}
|
|
return sim_kinematics_forward(k, joints, joint_count, pose);
|
|
}
|
|
|
|
uint64_t sim_kinematics_state_hash(const sim_kinematics *k, const double *joints, uint32_t joint_count, const sim_posemath_pose *pose) {
|
|
unsigned char bytes[sizeof(sim_kinematics_config) + sizeof(double) * SIM_KIN_MAX_JOINTS + sizeof(sim_posemath_pose) + sizeof(int32_t) + sizeof(uint32_t) * 2];
|
|
double normalized_joints[SIM_KIN_MAX_JOINTS] = {0};
|
|
sim_posemath_pose normalized_pose;
|
|
size_t offset = 0;
|
|
if (k == 0 || joints == 0 || pose == 0 || joint_count > SIM_KIN_MAX_JOINTS) return 0;
|
|
for (size_t index = 0; index < joint_count; ++index) normalized_joints[index] = hash_normalize(joints[index]);
|
|
normalized_pose = *pose;
|
|
normalized_pose.tran.x = hash_normalize(normalized_pose.tran.x); normalized_pose.tran.y = hash_normalize(normalized_pose.tran.y); normalized_pose.tran.z = hash_normalize(normalized_pose.tran.z);
|
|
normalized_pose.a = hash_normalize(normalized_pose.a); normalized_pose.b = hash_normalize(normalized_pose.b); normalized_pose.c = hash_normalize(normalized_pose.c);
|
|
normalized_pose.u = hash_normalize(normalized_pose.u); normalized_pose.v = hash_normalize(normalized_pose.v); normalized_pose.w = hash_normalize(normalized_pose.w);
|
|
memcpy(bytes + offset, &k->config, sizeof(k->config)); offset += sizeof(k->config);
|
|
memcpy(bytes + offset, normalized_joints, sizeof(double) * joint_count); offset += sizeof(double) * joint_count;
|
|
memcpy(bytes + offset, &normalized_pose, sizeof(normalized_pose)); offset += sizeof(normalized_pose);
|
|
memcpy(bytes + offset, &k->revolution_count, sizeof(k->revolution_count)); offset += sizeof(k->revolution_count);
|
|
memcpy(bytes + offset, &k->previous_quadrant, sizeof(k->previous_quadrant)); offset += sizeof(k->previous_quadrant);
|
|
memcpy(bytes + offset, &k->branch_id, sizeof(k->branch_id)); offset += sizeof(k->branch_id);
|
|
return fnv1a64(bytes, offset);
|
|
}
|