616 lines
43 KiB
C
616 lines
43 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);
|
|
config.pivot_length = 250.0;
|
|
config.limit_enabled[0] = 1; config.joint_min[0] = -1000; config.joint_max[0] = 1000;
|
|
return config;
|
|
}
|
|
|
|
static void round_trip(sim_kinematics *kinematics, const double *input, uint32_t count) {
|
|
double joints[SIM_KIN_MAX_JOINTS] = {0};
|
|
sim_posemath_pose pose = {0};
|
|
sim_posemath_pose repeat = {0};
|
|
assert(sim_kinematics_forward(kinematics, input, count, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(kinematics, &pose, joints, count) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(kinematics, joints, count, &repeat) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - repeat.tran.x) < 1e-9);
|
|
assert(fabs(pose.tran.y - repeat.tran.y) < 1e-9);
|
|
assert(fabs(pose.tran.z - repeat.tran.z) < 1e-9);
|
|
assert(sim_kinematics_state_hash(kinematics, joints, count, &repeat) == sim_kinematics_state_hash(kinematics, joints, count, &repeat));
|
|
}
|
|
|
|
static double canonical(double value) {
|
|
return fabs(value) < 5e-10 ? 0.0 : value;
|
|
}
|
|
|
|
static double canonical_pentapod(double value) {
|
|
double rounded = nearbyint(value * 1000000.0) / 1000000.0;
|
|
return fabs(rounded) < 0.0000005 ? 0.0 : rounded;
|
|
}
|
|
|
|
static void emit_coupled_samples(void) {
|
|
sim_kinematics kinematics = {0};
|
|
sim_kinematics_config config;
|
|
sim_posemath_pose pose = {0};
|
|
double output[SIM_KIN_MAX_JOINTS] = {0};
|
|
double corexy_pose[3];
|
|
double corexy_joints[3];
|
|
double rose_pose[3];
|
|
double rose_joints[3];
|
|
double max_pose[3];
|
|
double max_joints[6];
|
|
double rotate_pose[3];
|
|
double rotate_joints[6];
|
|
double fiveaxis_pose[3];
|
|
double fiveaxis_joints[7];
|
|
double xyzac_pose[3];
|
|
double xyzac_joints[5];
|
|
double xyzbc_pose[3];
|
|
double xyzbc_joints[5];
|
|
double scara_pose[3];
|
|
double scara_joints[6];
|
|
double puma_pose[3];
|
|
double puma_joints[6];
|
|
double scorbot_pose[3];
|
|
double scorbot_joints[5];
|
|
double genser_pose[3];
|
|
double genser_joints[6];
|
|
double ldelta_pose[3];
|
|
double ldelta_joints[3];
|
|
double rdelta_pose[3];
|
|
double rdelta_joints[3];
|
|
double tripod_pose[3];
|
|
double tripod_joints[3];
|
|
double genhex_pose[6];
|
|
double genhex_joints[6];
|
|
double penta_pose[5];
|
|
double penta_joints[5];
|
|
uint32_t rose_branch;
|
|
uint64_t corexy_hash;
|
|
uint64_t rose_hash;
|
|
uint64_t max_hash;
|
|
uint64_t rotate_hash;
|
|
uint64_t fiveaxis_hash;
|
|
uint64_t xyzac_hash;
|
|
uint64_t xyzbc_hash;
|
|
uint64_t scara_hash;
|
|
uint64_t puma_hash;
|
|
uint64_t scorbot_hash;
|
|
uint64_t genser_hash;
|
|
uint64_t ldelta_hash;
|
|
uint64_t rdelta_hash;
|
|
uint64_t tripod_hash;
|
|
uint64_t genhex_hash;
|
|
uint64_t penta_hash;
|
|
uint32_t ldelta_branch;
|
|
uint32_t ldelta_alt_branch;
|
|
uint32_t rdelta_branch;
|
|
uint32_t rdelta_alt_branch;
|
|
int ldelta_no_solution;
|
|
int ldelta_limit_error;
|
|
int rdelta_singular;
|
|
int ldelta_alt_status;
|
|
int rdelta_alt_status;
|
|
int ldelta_geometry_status;
|
|
int rdelta_geometry_status;
|
|
int tripod_no_solution;
|
|
int tripod_singular;
|
|
int tripod_limit_error;
|
|
int tripod_geometry_status;
|
|
uint32_t tripod_branch;
|
|
uint32_t tripod_alt_branch;
|
|
int tripod_alt_status;
|
|
int genhex_nonpositive;
|
|
int genhex_iter_limit_error;
|
|
int genhex_max_error_status;
|
|
int genhex_singular;
|
|
int genhex_limit_error;
|
|
int genhex_alt_status;
|
|
uint32_t genhex_branch;
|
|
uint32_t genhex_alt_branch;
|
|
uint32_t genhex_iterations;
|
|
double genhex_residual;
|
|
int penta_nonpositive;
|
|
int penta_iter_limit_error;
|
|
int penta_max_error_status;
|
|
int penta_singular;
|
|
int penta_limit_error;
|
|
int penta_alt_status;
|
|
uint32_t penta_branch;
|
|
uint32_t penta_alt_branch;
|
|
uint32_t penta_iterations;
|
|
uint32_t penta_alt_iterations;
|
|
uint32_t penta_nonpositive_iterations;
|
|
uint32_t penta_iter_limit_iterations;
|
|
uint32_t penta_max_error_iterations;
|
|
uint32_t penta_singular_iterations;
|
|
double penta_residual;
|
|
sim_posemath_pose tripod_alt_pose = {0};
|
|
sim_posemath_pose genhex_alt_pose = {0};
|
|
sim_posemath_pose penta_alt_pose = {0};
|
|
uint32_t ldelta_iterations;
|
|
uint32_t rdelta_iterations;
|
|
sim_posemath_pose ldelta_alt_pose = {0};
|
|
sim_posemath_pose rdelta_alt_pose = {0};
|
|
sim_posemath_pose mapping_pose = {0};
|
|
sim_kinematics_result_info delta_info = {0};
|
|
sim_kinematics_result_info genser_info = {0};
|
|
|
|
config = config_for(SIM_KIN_COREXY, "XYZ");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){6, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
|
corexy_pose[0] = canonical(pose.tran.x); corexy_pose[1] = canonical(pose.tran.y); corexy_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
memcpy(corexy_joints, output, sizeof(corexy_joints));
|
|
corexy_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
|
|
|
config = config_for(SIM_KIN_ROSE, "XYZ");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){2, 7, 170}, 3, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){2, 7, 190}, 3, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
rose_pose[0] = canonical(pose.tran.x); rose_pose[1] = canonical(pose.tran.y); rose_pose[2] = canonical(pose.tran.z);
|
|
memcpy(rose_joints, output, sizeof(rose_joints));
|
|
rose_branch = kinematics.branch_id;
|
|
rose_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
|
|
|
config = config_for(SIM_KIN_MAXKINS, "XYZABC");
|
|
config.pivot_length = 0.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 30, 90}, 6, &pose) == SIM_KIN_OK);
|
|
max_pose[0] = canonical(pose.tran.x); max_pose[1] = canonical(pose.tran.y); max_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse_ex(&kinematics, &pose, 6, 0, output, &genser_info) == SIM_KIN_OK);
|
|
memcpy(max_joints, output, sizeof(max_joints));
|
|
max_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_ROTATEKINS, "XYZABC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6, &pose) == SIM_KIN_OK);
|
|
rotate_pose[0] = canonical(pose.tran.x); rotate_pose[1] = canonical(pose.tran.y); rotate_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
|
memcpy(rotate_joints, output, sizeof(rotate_joints));
|
|
rotate_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_5AXISKINS, "XYZBCWY");
|
|
config.pivot_length = 250.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7, &pose) == SIM_KIN_OK);
|
|
fiveaxis_pose[0] = canonical(pose.tran.x); fiveaxis_pose[1] = canonical(pose.tran.y); fiveaxis_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 7) == SIM_KIN_OK);
|
|
memcpy(fiveaxis_joints, output, sizeof(fiveaxis_joints));
|
|
fiveaxis_hash = sim_kinematics_state_hash(&kinematics, output, 7, &pose);
|
|
|
|
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
|
config.x_rot_point = 20.0; config.y_offset = 3.0; config.tool_offset = 100.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){100, 50, 200, 30, 45}, 5, &pose) == SIM_KIN_OK);
|
|
xyzac_pose[0] = canonical(pose.tran.x); xyzac_pose[1] = canonical(pose.tran.y); xyzac_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
|
memcpy(xyzac_joints, output, sizeof(xyzac_joints));
|
|
xyzac_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
|
|
|
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
|
config.x_offset = 5.0; config.z_offset = 2.0; config.tool_offset = 50.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){100, 50, 200, 30, 45}, 5, &pose) == SIM_KIN_OK);
|
|
xyzbc_pose[0] = canonical(pose.tran.x); xyzbc_pose[1] = canonical(pose.tran.y); xyzbc_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
|
memcpy(xyzbc_joints, output, sizeof(xyzbc_joints));
|
|
xyzbc_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
|
|
|
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
|
config.scara_d1 = 490.0; config.scara_d2 = 340.0; config.scara_d3 = 50.0; config.scara_d4 = 250.0; config.scara_d5 = 50.0; config.scara_d6 = 50.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6, &pose) == SIM_KIN_OK);
|
|
scara_pose[0] = canonical(pose.tran.x); scara_pose[1] = canonical(pose.tran.y); scara_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
|
memcpy(scara_joints, output, sizeof(scara_joints)); scara_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_PUMA, "XYZABC");
|
|
config.puma_a2 = 400.0; config.puma_a3 = 50.0; config.puma_d3 = 100.0; config.puma_d4 = 400.0; config.puma_d6 = 95.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, -30, 40, 25, -15}, 6, &pose) == SIM_KIN_OK);
|
|
puma_pose[0] = canonical(pose.tran.x); puma_pose[1] = canonical(pose.tran.y); puma_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
|
memcpy(puma_joints, output, sizeof(puma_joints)); puma_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_SCORBOT, "XYZAB");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){25, 35, -20, 10, -5}, 5, &pose) == SIM_KIN_OK);
|
|
scorbot_pose[0] = canonical(pose.tran.x); scorbot_pose[1] = canonical(pose.tran.y); scorbot_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 5) == SIM_KIN_OK);
|
|
memcpy(scorbot_joints, output, sizeof(scorbot_joints)); scorbot_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
|
|
|
config = config_for(SIM_KIN_GENSER, "XYZABC");
|
|
config.dh_joint_count = 6;
|
|
config.max_iterations = 200;
|
|
config.convergence_tolerance = 1e-7;
|
|
config.dh_a[0] = 0.0; config.dh_a[1] = 85.0; config.dh_a[2] = 380.0; config.dh_a[3] = 100.0;
|
|
config.dh_alpha[0] = 0.0; config.dh_alpha[1] = -1.5707963267948966; config.dh_alpha[2] = 0.0; config.dh_alpha[3] = -1.5707963267948966; config.dh_alpha[4] = 1.5707963267948966; config.dh_alpha[5] = -1.5707963267948966;
|
|
config.dh_d[0] = 350.0; config.dh_d[1] = 0.0; config.dh_d[2] = 0.0; config.dh_d[3] = 425.0; config.dh_d[4] = 0.0; config.dh_d[5] = 235.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6, &pose) == SIM_KIN_OK);
|
|
genser_pose[0] = canonical(pose.tran.x); genser_pose[1] = canonical(pose.tran.y); genser_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 6) == SIM_KIN_OK);
|
|
memcpy(genser_joints, output, sizeof(genser_joints)); genser_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_LINEAR_DELTA, "XYZ");
|
|
config.joint_count = 4;
|
|
config.kinematics_joint_count = 3;
|
|
config.extra_joint_count = 1;
|
|
config.delta_radius = 130.25; config.delta_rod_length = 269.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward_ex(&kinematics, (double[]){300, 300, 300}, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
|
ldelta_branch = delta_info.branch_id; ldelta_iterations = delta_info.iterations;
|
|
ldelta_pose[0] = canonical(pose.tran.x); ldelta_pose[1] = canonical(pose.tran.y); ldelta_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_forward_ex(&kinematics, (double[]){300, 300, 300}, 3, SIM_KIN_DELTA_ALTERNATE_ROOT, &ldelta_alt_pose, &delta_info) == SIM_KIN_OK);
|
|
ldelta_alt_status = SIM_KIN_OK; ldelta_alt_branch = delta_info.branch_id;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
memcpy(ldelta_joints, output, sizeof(ldelta_joints)); ldelta_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
|
ldelta_no_solution = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {1000.0, 0.0, 0.0}}, output, 3);
|
|
config.limit_enabled[0] = 1; config.joint_min[0] = -10.0; config.joint_max[0] = 10.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
ldelta_limit_error = sim_kinematics_inverse(&kinematics, &pose, output, 3);
|
|
config.limit_enabled[0] = 0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
config.delta_radius = 0.0; config.delta_rod_length = 269.0;
|
|
ldelta_geometry_status = sim_kinematics_init(&kinematics, &config);
|
|
config.delta_radius = 130.25; config.delta_rod_length = 269.0;
|
|
|
|
config = config_for(SIM_KIN_ROTARY_DELTA, "XYZ");
|
|
config.rotary_platform_radius = 10.0; config.rotary_thigh_length = 10.0; config.rotary_shin_length = 14.0; config.rotary_foot_radius = 6.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward_ex(&kinematics, (double[]){10, 10, 10}, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
|
rdelta_branch = delta_info.branch_id; rdelta_iterations = delta_info.iterations;
|
|
rdelta_pose[0] = canonical(pose.tran.x); rdelta_pose[1] = canonical(pose.tran.y); rdelta_pose[2] = canonical(pose.tran.z);
|
|
assert(sim_kinematics_forward_ex(&kinematics, (double[]){10, 10, 10}, 3, SIM_KIN_DELTA_ALTERNATE_ROOT, &rdelta_alt_pose, &delta_info) == SIM_KIN_OK);
|
|
rdelta_alt_status = SIM_KIN_OK; rdelta_alt_branch = delta_info.branch_id;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
memcpy(rdelta_joints, output, sizeof(rdelta_joints)); rdelta_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
|
rdelta_singular = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 0.0, 0.0}}, output, 3);
|
|
config.rotary_platform_radius = 0.0;
|
|
rdelta_geometry_status = sim_kinematics_init(&kinematics, &config);
|
|
|
|
/* Non-default coordinate order must use the same axis-to-joint mapping
|
|
* in rotary Delta inverse as it does in forward. */
|
|
config = config_for(SIM_KIN_ROTARY_DELTA, "YZX");
|
|
config.rotary_platform_radius = 10.0; config.rotary_thigh_length = 10.0; config.rotary_shin_length = 14.0; config.rotary_foot_radius = 6.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30}, 3, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, output, 3) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, output, 3, &mapping_pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - mapping_pose.tran.x) < 1e-9);
|
|
assert(fabs(pose.tran.y - mapping_pose.tran.y) < 1e-9);
|
|
assert(fabs(pose.tran.z - mapping_pose.tran.z) < 1e-9);
|
|
|
|
config = config_for(SIM_KIN_TRIPOD, "XYZ");
|
|
config.tripod_bx = 10.0; config.tripod_cx = 5.0; config.tripod_cy = 7.071;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 4.0}}, 3, 0, output, &delta_info) == SIM_KIN_OK);
|
|
memcpy(tripod_joints, output, sizeof(tripod_joints));
|
|
assert(sim_kinematics_forward_ex(&kinematics, output, 3, 0, &pose, &delta_info) == SIM_KIN_OK);
|
|
tripod_branch = delta_info.branch_id;
|
|
tripod_pose[0] = canonical(pose.tran.x); tripod_pose[1] = canonical(pose.tran.y); tripod_pose[2] = canonical(pose.tran.z);
|
|
tripod_hash = sim_kinematics_state_hash(&kinematics, output, 3, &pose);
|
|
assert(sim_kinematics_forward_ex(&kinematics, output, 3, SIM_KIN_TRIPOD_BELOW_ROOT, &tripod_alt_pose, &delta_info) == SIM_KIN_OK);
|
|
tripod_alt_status = SIM_KIN_OK; tripod_alt_branch = delta_info.branch_id;
|
|
tripod_no_solution = sim_kinematics_forward(&kinematics, (double[]){1.0, 1.0, 20.0}, 3, &pose);
|
|
assert(sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 0.0}}, output, 3) == SIM_KIN_OK);
|
|
tripod_singular = sim_kinematics_forward(&kinematics, output, 3, &pose);
|
|
config.limit_enabled[0] = 1; config.joint_min[0] = 0.0; config.joint_max[0] = 1.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
tripod_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {2.0, 3.0, 4.0}}, output, 3);
|
|
config.limit_enabled[0] = 0;
|
|
config.tripod_bx = 0.0;
|
|
tripod_geometry_status = sim_kinematics_init(&kinematics, &config);
|
|
config.tripod_bx = 10.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_home(&kinematics, output, 3, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.z - 1.0) < 1e-9);
|
|
|
|
config = config_for(SIM_KIN_GENHEX, "XYZABC");
|
|
config.genhex_initial_pose[2] = 50.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {10.0, -5.0, 60.0}, .a = 5.0, .b = -3.0, .c = 10.0}, 6, 0, output, &genser_info) == SIM_KIN_OK);
|
|
memcpy(genhex_joints, output, sizeof(genhex_joints));
|
|
assert(sim_kinematics_forward_ex(&kinematics, output, 6, 0, &pose, &genser_info) == SIM_KIN_OK);
|
|
genhex_pose[0] = canonical(pose.tran.x); genhex_pose[1] = canonical(pose.tran.y); genhex_pose[2] = canonical(pose.tran.z);
|
|
genhex_pose[3] = canonical(pose.a); genhex_pose[4] = canonical(pose.b); genhex_pose[5] = canonical(pose.c);
|
|
genhex_iterations = genser_info.iterations; genhex_residual = genser_info.residual; genhex_branch = genser_info.branch_id;
|
|
genhex_hash = sim_kinematics_state_hash(&kinematics, output, 6, &pose);
|
|
genhex_alt_status = sim_kinematics_forward_ex(&kinematics, output, 6, SIM_KIN_GENHEX_ALTERNATE_SEED, &genhex_alt_pose, &genser_info);
|
|
genhex_alt_branch = genser_info.branch_id;
|
|
genhex_nonpositive = sim_kinematics_forward(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0, 1.0}, 6, &pose);
|
|
config.genhex_iteration_limit = 1;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
genhex_iter_limit_error = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
|
config.genhex_iteration_limit = 120; config.genhex_max_error = 0.000001;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
genhex_max_error_status = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
|
config.genhex_max_error = 500.0; config.limit_enabled[0] = 1; config.joint_min[0] = 0.0; config.joint_max[0] = 1.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
genhex_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {10.0, -5.0, 60.0}, .a = 5.0, .b = -3.0, .c = 10.0}, output, 6);
|
|
config.limit_enabled[0] = 0;
|
|
config.genhex_base[0][0] = 1.0; config.genhex_base[0][1] = 0.0; config.genhex_base[0][2] = 0.0;
|
|
config.genhex_platform[0][0] = 0.0; config.genhex_platform[0][1] = 1.0; config.genhex_platform[0][2] = 0.0;
|
|
for (int duplicate = 1; duplicate < 6; ++duplicate) {
|
|
memcpy(config.genhex_base[duplicate], config.genhex_base[0], sizeof(config.genhex_base[0]));
|
|
memcpy(config.genhex_platform[duplicate], config.genhex_platform[0], sizeof(config.genhex_platform[0]));
|
|
}
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
genhex_singular = sim_kinematics_forward(&kinematics, genhex_joints, 6, &pose);
|
|
|
|
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse_ex(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 0.01, .b = -0.01}, 5, 0, output, &genser_info) == SIM_KIN_OK);
|
|
memcpy(penta_joints, output, sizeof(penta_joints));
|
|
assert(sim_kinematics_forward_ex(&kinematics, output, 5, 0, &pose, &genser_info) == SIM_KIN_OK);
|
|
penta_pose[0] = canonical_pentapod(pose.tran.x); penta_pose[1] = canonical_pentapod(pose.tran.y); penta_pose[2] = canonical_pentapod(pose.tran.z); penta_pose[3] = canonical_pentapod(pose.a); penta_pose[4] = canonical_pentapod(pose.b);
|
|
pose.tran.x = penta_pose[0]; pose.tran.y = penta_pose[1]; pose.tran.z = penta_pose[2]; pose.a = penta_pose[3]; pose.b = penta_pose[4];
|
|
penta_iterations = genser_info.iterations; penta_residual = nearbyint(genser_info.residual * 1000000000.0) / 1000000000.0; penta_branch = genser_info.branch_id;
|
|
penta_hash = sim_kinematics_state_hash(&kinematics, output, 5, &pose);
|
|
penta_alt_status = sim_kinematics_forward_ex(&kinematics, output, 5, SIM_KIN_GENHEX_ALTERNATE_SEED, &penta_alt_pose, &genser_info); penta_alt_branch = genser_info.branch_id; penta_alt_iterations = genser_info.iterations;
|
|
penta_nonpositive = sim_kinematics_forward_ex(&kinematics, (double[]){0.0, 1.0, 1.0, 1.0, 1.0}, 5, 0, &pose, &genser_info); penta_nonpositive_iterations = genser_info.iterations;
|
|
config.penta_initial_pose[3] = 0.0; config.penta_initial_pose[4] = 0.0;
|
|
config.penta_iteration_limit = 1; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_iter_limit_error = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_iter_limit_iterations = genser_info.iterations;
|
|
config.penta_iteration_limit = 120; config.penta_max_error = 0.000001; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_max_error_status = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_max_error_iterations = genser_info.iterations;
|
|
config.penta_max_error = 1000.0; config.limit_enabled[0] = 1; config.joint_min[0] = 0.0; config.joint_max[0] = 1.0; assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_limit_error = sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, output, 5);
|
|
config.limit_enabled[0] = 0; config.penta_base[0][0] = 1.0; config.penta_base[0][1] = 1.0; config.penta_base[0][2] = 1.0; for (int duplicate = 1; duplicate < 5; ++duplicate) memcpy(config.penta_base[duplicate], config.penta_base[0], sizeof(config.penta_base[0])); assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK); penta_singular = sim_kinematics_forward_ex(&kinematics, penta_joints, 5, 0, &pose, &genser_info); penta_singular_iterations = genser_info.iterations;
|
|
|
|
printf("{\"schema\":\"kinematics-coupled-samples.v1\","
|
|
"\"corexy\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"rose\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"branch\":%u,\"state_hash\":%llu},"
|
|
"\"maxkins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"rotatekins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"fiveaxiskins\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"xyzac_trt\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"xyzbc_trt\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"scara\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"puma\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"scorbot\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"genser\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
|
"\"lineardelta\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"rotarydelta\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"tripod\":{\"pose\":[%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f],\"state_hash\":%llu},"
|
|
"\"genhex\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
|
"\"pentapod\":{\"pose\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"inverse\":[%.9f,%.9f,%.9f,%.9f,%.9f],\"state_hash\":%llu,\"iterations\":%u,\"residual\":%.12f},"
|
|
"\"delta_edges\":{\"lineardelta\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"no_solution\":%d,\"limit_error\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
|
"\"rotarydelta\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"singular\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
|
"\"tripod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"no_solution\":%d,\"singular\":%d,\"limit_error\":%d,\"geometry_error\":%d,\"branch\":%u,\"iterations\":%u},"
|
|
"\"genhex\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"nonpositive\":%d,\"iteration_limit\":%d,\"max_error\":%d,\"singular\":%d,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f},"
|
|
"\"pentapod\":{\"alternate_status\":%d,\"alternate_branch\":%u,\"alternate_pose\":[%.9f,%.9f,%.9f],\"alternate_iterations\":%u,\"nonpositive\":%d,\"nonpositive_iterations\":%u,\"iteration_limit\":%d,\"iteration_limit_iterations\":%u,\"max_error\":%d,\"max_error_iterations\":%u,\"singular\":%d,\"singular_iterations\":%u,\"limit_error\":%d,\"branch\":%u,\"iterations\":%u,\"residual\":%.12f}}}\n",
|
|
corexy_pose[0], corexy_pose[1], corexy_pose[2], corexy_joints[0], corexy_joints[1], corexy_joints[2], (unsigned long long)corexy_hash,
|
|
rose_pose[0], rose_pose[1], rose_pose[2], rose_joints[0], rose_joints[1], rose_joints[2], rose_branch, (unsigned long long)rose_hash,
|
|
max_pose[0], max_pose[1], max_pose[2], max_joints[0], max_joints[1], max_joints[2], max_joints[3], max_joints[4], max_joints[5], (unsigned long long)max_hash,
|
|
rotate_pose[0], rotate_pose[1], rotate_pose[2], rotate_joints[0], rotate_joints[1], rotate_joints[2], rotate_joints[3], rotate_joints[4], rotate_joints[5], (unsigned long long)rotate_hash,
|
|
fiveaxis_pose[0], fiveaxis_pose[1], fiveaxis_pose[2], fiveaxis_joints[0], fiveaxis_joints[1], fiveaxis_joints[2], fiveaxis_joints[3], fiveaxis_joints[4], fiveaxis_joints[5], fiveaxis_joints[6], (unsigned long long)fiveaxis_hash,
|
|
xyzac_pose[0], xyzac_pose[1], xyzac_pose[2], xyzac_joints[0], xyzac_joints[1], xyzac_joints[2], xyzac_joints[3], xyzac_joints[4], (unsigned long long)xyzac_hash,
|
|
xyzbc_pose[0], xyzbc_pose[1], xyzbc_pose[2], xyzbc_joints[0], xyzbc_joints[1], xyzbc_joints[2], xyzbc_joints[3], xyzbc_joints[4], (unsigned long long)xyzbc_hash,
|
|
scara_pose[0], scara_pose[1], scara_pose[2], scara_joints[0], scara_joints[1], scara_joints[2], scara_joints[3], scara_joints[4], scara_joints[5], (unsigned long long)scara_hash,
|
|
puma_pose[0], puma_pose[1], puma_pose[2], puma_joints[0], puma_joints[1], puma_joints[2], puma_joints[3], puma_joints[4], puma_joints[5], (unsigned long long)puma_hash,
|
|
scorbot_pose[0], scorbot_pose[1], scorbot_pose[2], scorbot_joints[0], scorbot_joints[1], scorbot_joints[2], scorbot_joints[3], scorbot_joints[4], (unsigned long long)scorbot_hash,
|
|
genser_pose[0], genser_pose[1], genser_pose[2], genser_joints[0], genser_joints[1], genser_joints[2], genser_joints[3], genser_joints[4], genser_joints[5], (unsigned long long)genser_hash, genser_info.iterations, genser_info.residual,
|
|
ldelta_pose[0], ldelta_pose[1], ldelta_pose[2], ldelta_joints[0], ldelta_joints[1], ldelta_joints[2], (unsigned long long)ldelta_hash,
|
|
rdelta_pose[0], rdelta_pose[1], rdelta_pose[2], rdelta_joints[0], rdelta_joints[1], rdelta_joints[2], (unsigned long long)rdelta_hash,
|
|
tripod_pose[0], tripod_pose[1], tripod_pose[2], tripod_joints[0], tripod_joints[1], tripod_joints[2], (unsigned long long)tripod_hash,
|
|
genhex_pose[0], genhex_pose[1], genhex_pose[2], genhex_pose[3], genhex_pose[4], genhex_pose[5], genhex_joints[0], genhex_joints[1], genhex_joints[2], genhex_joints[3], genhex_joints[4], genhex_joints[5], (unsigned long long)genhex_hash, genhex_iterations, genhex_residual,
|
|
penta_pose[0], penta_pose[1], penta_pose[2], penta_pose[3], penta_pose[4], penta_joints[0], penta_joints[1], penta_joints[2], penta_joints[3], penta_joints[4], (unsigned long long)penta_hash, penta_iterations, penta_residual,
|
|
ldelta_alt_status, ldelta_alt_branch, ldelta_alt_pose.tran.x, ldelta_alt_pose.tran.y, ldelta_alt_pose.tran.z, ldelta_no_solution, ldelta_limit_error, ldelta_geometry_status, ldelta_branch, ldelta_iterations,
|
|
rdelta_alt_status, rdelta_alt_branch, rdelta_alt_pose.tran.x, rdelta_alt_pose.tran.y, rdelta_alt_pose.tran.z, rdelta_singular, rdelta_geometry_status, rdelta_branch, rdelta_iterations,
|
|
tripod_alt_status, tripod_alt_branch, tripod_alt_pose.tran.x, tripod_alt_pose.tran.y, tripod_alt_pose.tran.z, tripod_no_solution, tripod_singular, tripod_limit_error, tripod_geometry_status, tripod_branch, delta_info.iterations,
|
|
genhex_alt_status, genhex_alt_branch, genhex_alt_pose.tran.x, genhex_alt_pose.tran.y, genhex_alt_pose.tran.z, genhex_nonpositive, genhex_iter_limit_error, genhex_max_error_status, genhex_singular, genhex_limit_error, genhex_branch, genhex_iterations, genhex_residual,
|
|
penta_alt_status, penta_alt_branch, penta_alt_pose.tran.x, penta_alt_pose.tran.y, penta_alt_pose.tran.z, penta_alt_iterations, penta_nonpositive, penta_nonpositive_iterations, penta_iter_limit_error, penta_iter_limit_iterations, penta_max_error_status, penta_max_error_iterations, penta_singular, penta_singular_iterations, penta_limit_error, penta_branch, penta_iterations, penta_residual);
|
|
}
|
|
|
|
int sim_kinematics_test_run(void) {
|
|
sim_kinematics kinematics = {0};
|
|
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS, "XYZABCUVW");
|
|
double joints[9] = {10, 20, 30, 4, 5, 6, 7, 8, 9};
|
|
sim_posemath_pose pose = {0};
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, joints, 9);
|
|
|
|
config = config_for(SIM_KIN_TRIVKINS, "xyyz");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3, 4}, 4, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - 1.0) < 1e-12);
|
|
assert(fabs(pose.tran.y - 2.0) < 1e-12);
|
|
assert(fabs(pose.tran.z - 4.0) < 1e-12);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 4) == SIM_KIN_OK);
|
|
/* Duplicate Y maps to the first Y joint deterministically. */
|
|
assert(fabs(joints[1] - joints[2]) < 1e-12);
|
|
assert(sim_kinematics_state_hash(&kinematics, joints, 4, &pose) == sim_kinematics_state_hash(&kinematics, joints, 4, &pose));
|
|
|
|
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
|
config.limit_enabled[1] = 1; config.joint_min[1] = -2.0; config.joint_max[1] = 2.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){0, 3, 0}, 3, &pose) == SIM_KIN_ERR_RANGE);
|
|
assert(sim_kinematics_home(&kinematics, joints, 3, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x) < 1e-12 && fabs(pose.tran.y) < 1e-12 && fabs(pose.tran.z) < 1e-12);
|
|
|
|
/* Extra joints are configured but are not passed to kinematics. */
|
|
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
|
config.joint_count = 4;
|
|
config.kinematics_joint_count = 3;
|
|
config.extra_joint_count = 1;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3, 99}, 4, &pose) == SIM_KIN_ERR_CONFIGURATION);
|
|
assert(sim_kinematics_home(&kinematics, joints, 3, &pose) == SIM_KIN_OK);
|
|
|
|
config = config_for(SIM_KIN_TRIVKINS, "XYZ");
|
|
config.mode = SIM_KIN_MODE_FORWARD_ONLY;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 3) == SIM_KIN_ERR_UNSUPPORTED);
|
|
config.mode = SIM_KIN_MODE_INVERSE_ONLY;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){1, 2, 3}, 3, &pose) == SIM_KIN_ERR_UNSUPPORTED);
|
|
|
|
config.mode = SIM_KIN_MODE_BOTH;
|
|
config.max_iterations = 25;
|
|
config.convergence_tolerance = 1e-10;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
{
|
|
sim_kinematics_result_info info = {0};
|
|
assert(sim_kinematics_forward_ex(&kinematics, (double[]){1, 2, 3}, 3, 7, &pose, &info) == SIM_KIN_OK);
|
|
assert(info.inverse_flags == 7);
|
|
assert(sim_kinematics_inverse_ex(&kinematics, &pose, 3, 9, joints, &info) == SIM_KIN_OK);
|
|
assert(info.forward_flags == 9);
|
|
}
|
|
|
|
config = config_for(SIM_KIN_XYZAC_TRT, "XYZAC");
|
|
config.x_rot_point = 20; config.y_offset = 3; config.tool_offset = 100;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
joints[0] = 100; joints[1] = 50; joints[2] = 200; joints[3] = 30; joints[4] = 45;
|
|
round_trip(&kinematics, joints, 5);
|
|
|
|
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
|
config.x_offset = 5; config.z_offset = 2; config.tool_offset = 50;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
joints[0] = 100; joints[1] = 50; joints[2] = 200; joints[3] = 30; joints[4] = 45;
|
|
round_trip(&kinematics, joints, 5);
|
|
|
|
config = config_for(SIM_KIN_XYZBCW, "XYZBCW");
|
|
config.pivot_length = 250;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
joints[0] = 10; joints[1] = 20; joints[2] = 30; joints[3] = 35; joints[4] = 40; joints[5] = -2;
|
|
round_trip(&kinematics, joints, 6);
|
|
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){NAN, 0, 0, 0, 0, 0}, 6, &pose) == SIM_KIN_ERR_NONFINITE);
|
|
config.type = 99;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_UNKNOWN_TYPE);
|
|
config = config_for(SIM_KIN_XYZBC_TRT, "XYZBC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
pose.a = 0; pose.b = 90; pose.c = 0;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_ERR_SINGULAR);
|
|
|
|
config = config_for(SIM_KIN_COREXY, "XYZ");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){6, 2, 3}, 3, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - 4.0) < 1e-12 && fabs(pose.tran.y - 2.0) < 1e-12);
|
|
round_trip(&kinematics, (double[]){6, 2, 3}, 3);
|
|
config = config_for(SIM_KIN_COREXY, "YZ");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
|
|
|
{
|
|
sim_kinematics second = {0};
|
|
sim_posemath_pose q2 = {{-1.0, 0.1, 7.0}, 0, 0, 0, 0, 0, 0};
|
|
sim_posemath_pose q3 = {{-1.0, -0.1, 7.0}, 0, 0, 0, 0, 0, 0};
|
|
uint64_t first_hash;
|
|
config = config_for(SIM_KIN_ROSE, "XYZ");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_init(&second, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &q2, joints, 3) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &q3, joints, 3) == SIM_KIN_OK);
|
|
assert(kinematics.revolution_count == 1 && joints[2] > 180.0 && joints[2] < 190.0);
|
|
first_hash = sim_kinematics_state_hash(&kinematics, joints, 3, &q3);
|
|
assert(sim_kinematics_inverse(&kinematics, &q3, joints, 3) == SIM_KIN_OK);
|
|
assert(first_hash == sim_kinematics_state_hash(&kinematics, joints, 3, &q3));
|
|
assert(sim_kinematics_inverse(&second, &q2, joints + 3, 3) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&second, &q3, joints + 3, 3) == SIM_KIN_OK);
|
|
assert(first_hash == sim_kinematics_state_hash(&second, joints + 3, 3, &q3));
|
|
assert(sim_kinematics_inverse(&kinematics, &q2, joints, 3) == SIM_KIN_OK);
|
|
assert(kinematics.revolution_count == 0 && joints[2] > 170.0 && joints[2] < 180.0);
|
|
config.joint_count = 4;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
|
}
|
|
|
|
config = config_for(SIM_KIN_MAXKINS, "XYZABC");
|
|
config.pivot_length = 0.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){3, 4, 5, 0, 30, 90}, 6);
|
|
config.pivot_length = 0.666;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){0, 0, 5, 0, 30, 0}, 6, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - 0.333) < 1e-12);
|
|
|
|
config = config_for(SIM_KIN_ROTATEKINS, "XYZABC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){3, 4, 5, 0, 0, 90}, 6, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - 4.0) < 1e-12 && fabs(pose.tran.y + 3.0) < 1e-12);
|
|
|
|
config = config_for(SIM_KIN_5AXISKINS, "XYZBCWY");
|
|
config.pivot_length = 250.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){10, 20, 30, 35, 40, -2, 20}, 7, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.x - 10.0 - (250.0 - 2.0) * sin((180.0 - 35.0) * 0.017453292519943295769) * cos(40.0 * 0.017453292519943295769)) < 1e-9);
|
|
config = config_for(SIM_KIN_5AXISKINS, "XYZBC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
|
|
|
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
|
config.scara_d1 = 490.0; config.scara_d2 = 340.0; config.scara_d3 = 50.0; config.scara_d4 = 250.0; config.scara_d5 = 50.0; config.scara_d6 = 50.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){30, -40, 25, 15, 5, -3}, 6, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.c - 5.0) < 1e-9);
|
|
config = config_for(SIM_KIN_SCARA, "XYZC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
|
config = config_for(SIM_KIN_SCARA, "XYZCAB");
|
|
config.scara_d2 = 1.0; config.scara_d4 = 1.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
pose.tran.x = 10.0; pose.tran.y = 0.0; pose.tran.z = 0.0; pose.c = 0.0;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_RANGE);
|
|
|
|
config = config_for(SIM_KIN_PUMA, "XYZABC");
|
|
config.puma_a2 = 400.0; config.puma_a3 = 50.0; config.puma_d3 = 100.0; config.puma_d4 = 400.0; config.puma_d6 = 95.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){10, 20, -30, 40, 25, -15}, 6);
|
|
pose.tran.x = 0.0; pose.tran.y = 0.0; pose.tran.z = 0.0; pose.a = 0.0; pose.b = 0.0; pose.c = 0.0;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_RANGE || sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_SINGULAR);
|
|
|
|
config = config_for(SIM_KIN_SCORBOT, "XYZAB");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){25, 35, -20, 10, -5}, 5);
|
|
pose.tran.x = 1000.0; pose.tran.y = 0.0; pose.tran.z = 0.0;
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_ERR_RANGE);
|
|
|
|
config = config_for(SIM_KIN_GENSER, "XYZABC");
|
|
config.dh_joint_count = 6; config.max_iterations = 200; config.convergence_tolerance = 1e-7;
|
|
config.dh_a[0] = 0.0; config.dh_a[1] = 85.0; config.dh_a[2] = 380.0; config.dh_a[3] = 100.0;
|
|
config.dh_alpha[0] = 0.0; config.dh_alpha[1] = -1.5707963267948966; config.dh_alpha[2] = 0.0; config.dh_alpha[3] = -1.5707963267948966; config.dh_alpha[4] = 1.5707963267948966; config.dh_alpha[5] = -1.5707963267948966;
|
|
config.dh_d[0] = 350.0; config.dh_d[1] = 0.0; config.dh_d[2] = 0.0; config.dh_d[3] = 425.0; config.dh_d[4] = 0.0; config.dh_d[5] = 235.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
round_trip(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6);
|
|
config.max_iterations = 1;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, (double[]){0, 10, -20, 30, 15, -10}, 6, &pose) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 6) == SIM_KIN_ERR_SINGULAR);
|
|
config.dh_alpha[0] = NAN;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
|
|
|
|
/* Pentapod owns XYZAB only: yaw is not mapped, and LinuxCNC applies the
|
|
* tool offset to both base and effector HAL geometry. */
|
|
config = config_for(SIM_KIN_PENTAPOD, "XYZAB");
|
|
config.penta_initial_pose[3] = 2.0 * 0.017453292519943295769;
|
|
config.penta_initial_pose[4] = -3.0 * 0.017453292519943295769;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
pose = (sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0, .c = 17.0};
|
|
assert(sim_kinematics_inverse(&kinematics, &pose, joints, 5) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.c) < 1e-12);
|
|
config.penta_tool_offset = 25.0;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_inverse(&kinematics, &(sim_posemath_pose){.tran = {0.0, 500.0, 300.0}, .a = 2.0, .b = -3.0}, joints, 5) == SIM_KIN_OK);
|
|
assert(sim_kinematics_forward(&kinematics, joints, 5, &pose) == SIM_KIN_OK);
|
|
assert(fabs(pose.tran.z - 300.0) < 1e-9);
|
|
config.penta_tool_offset = NAN;
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_NONFINITE);
|
|
config = config_for(SIM_KIN_PENTAPOD, "XYZABC");
|
|
assert(sim_kinematics_init(&kinematics, &config) == SIM_KIN_ERR_CONFIGURATION);
|
|
|
|
emit_coupled_samples();
|
|
return 0;
|
|
}
|