#include "sim_kinematics.h" #include #include #include #include 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; }