145 lines
6.5 KiB
C
145 lines
6.5 KiB
C
#include "sim_kinematics.h"
|
|
#include "sim_program.h"
|
|
#include "sim_protocol.h"
|
|
|
|
#include <assert.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static sim_kinematics_config config_for(int type) {
|
|
static const double minimum[5] = {-200.0, -100.0, -120.0, -100.0, -36000.0};
|
|
static const double maximum[5] = {200.0, 100.0, 120.0, 50.0, 36000.0};
|
|
sim_kinematics_config config = {0};
|
|
size_t index;
|
|
config.type = type;
|
|
config.joint_count = 5;
|
|
config.kinematics_joint_count = 5;
|
|
strcpy(config.coordinates, "XYZAC");
|
|
config.y_offset = 20.0;
|
|
config.z_offset = 10.0;
|
|
for (index = 0; index < 5; ++index) {
|
|
config.limit_enabled[index] = 1;
|
|
config.joint_min[index] = minimum[index];
|
|
config.joint_max[index] = maximum[index];
|
|
}
|
|
return config;
|
|
}
|
|
|
|
static void print_pose(const sim_posemath_pose *pose) {
|
|
printf("{\"a\":%.17g,\"c\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
|
|
pose->a, pose->c, pose->tran.x, pose->tran.y, pose->tran.z);
|
|
}
|
|
|
|
static void print_joints(const double *joints) {
|
|
printf("[%.17g,%.17g,%.17g,%.17g,%.17g]", joints[0], joints[1], joints[2], joints[3], joints[4]);
|
|
}
|
|
|
|
int main(void) {
|
|
static const char *commands[4] = {"M429", "M428", "M430", "M428"};
|
|
static const int modes[4] = {0, 1, 2, 1};
|
|
static const char source[] = "G21 G90 G94\nG0 X10 Y0 Z10 A0 C0\nG93\nG2 X10 Y0 Z5 A30 C45 I-10 J0 P3 F2\nM2\n";
|
|
sim_kinematics identity = {0};
|
|
sim_kinematics trt = {0};
|
|
sim_kinematics userk = {0};
|
|
sim_kinematics_switch switcher = {0};
|
|
sim_kinematics_switch saved_switcher;
|
|
sim_kinematics_result_info info = {0};
|
|
sim_posemath_pose poses[4] = {{0}};
|
|
sim_posemath_pose pose = {0};
|
|
sim_posemath_pose saved_pose = {0};
|
|
sim_posemath_segment arc = {0};
|
|
sim_program program = {0};
|
|
double joints[5] = {12.0, -5.0, 20.0, 30.0, 45.0};
|
|
double state_joints[4][5] = {{0}};
|
|
double saved_joints[5] = {0};
|
|
uint64_t before_hash;
|
|
uint64_t after_hash;
|
|
size_t index;
|
|
int have_arc = 0;
|
|
|
|
assert(sim_kinematics_init(&identity, &(sim_kinematics_config){0}) != SIM_KIN_OK);
|
|
{
|
|
sim_kinematics_config config = config_for(SIM_KIN_TRIVKINS);
|
|
assert(sim_kinematics_init(&identity, &config) == SIM_KIN_OK);
|
|
assert(sim_kinematics_init(&userk, &config) == SIM_KIN_OK);
|
|
config = config_for(SIM_KIN_XYZAC_TRT);
|
|
assert(sim_kinematics_init(&trt, &config) == SIM_KIN_OK);
|
|
}
|
|
assert(sim_kinematics_switch_init(&switcher, 0, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 0, &identity, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 1, &trt, 0) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_register(&switcher, 2, &userk, 0) == SIM_KIN_OK);
|
|
|
|
sim_protocol_reset();
|
|
assert(sim_protocol_accept(1, 1, "HELLO", "", 0) == SIM_PROTOCOL_OK);
|
|
assert(sim_protocol_accept(1, 2, "SESSION_ATTACH", "", 0) == SIM_PROTOCOL_OK);
|
|
assert(sim_protocol_accept(1, 3, "LOAD_WORKSPACE", "workspace", 0) == SIM_PROTOCOL_OK);
|
|
for (index = 0; index < 4; ++index) {
|
|
char operation[16];
|
|
snprintf(operation, sizeof(operation), "switch-%u", (unsigned)(index + 1));
|
|
assert(sim_protocol_accept(1, (uint32_t)(index + 4), "MDI_EXECUTE", operation, 0) == SIM_PROTOCOL_OK);
|
|
assert(sim_kinematics_switch_select(&switcher, modes[index], joints, 5, &pose, &info) == SIM_KIN_OK);
|
|
assert(sim_kinematics_switch_active(&switcher) == modes[index]);
|
|
poses[index] = pose;
|
|
memcpy(state_joints[index], joints, sizeof(joints));
|
|
}
|
|
|
|
assert(sim_program_init(&program) == SIM_PROGRAM_OK);
|
|
assert(sim_program_load(&program, source, (uint32_t)(sizeof(source) - 1)) == SIM_PROGRAM_OK);
|
|
assert(sim_program_start(&program) == SIM_PROGRAM_OK);
|
|
while (program.status == SIM_PROGRAM_RUNNING) {
|
|
assert(sim_program_step(&program) == SIM_PROGRAM_OK);
|
|
if (program.has_motion && program.motion.motion_type == SIM_POSEMATH_CIRCULAR) {
|
|
assert(sim_program_read_motion(&program, &arc) == SIM_PROGRAM_OK);
|
|
have_arc = 1;
|
|
}
|
|
}
|
|
assert(have_arc && arc.turn == -3);
|
|
assert(program.status == SIM_PROGRAM_COMPLETED);
|
|
assert(sim_program_start(&program) == SIM_PROGRAM_OK);
|
|
assert(program.status == SIM_PROGRAM_RUNNING && program.current_line == 0);
|
|
assert(program.interpreter.state.program_end == 0);
|
|
assert(sim_program_step(&program) == SIM_PROGRAM_OK);
|
|
assert(sim_program_abort(&program) == SIM_PROGRAM_OK);
|
|
assert(sim_program_start(&program) == SIM_PROGRAM_OK);
|
|
assert(program.status == SIM_PROGRAM_RUNNING && program.current_line == 0);
|
|
|
|
saved_switcher = switcher;
|
|
saved_pose = pose;
|
|
memcpy(saved_joints, joints, sizeof(joints));
|
|
before_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &pose);
|
|
assert(sim_kinematics_switch_select(&switcher, 0, joints, 5, &pose, &info) == SIM_KIN_OK);
|
|
switcher = saved_switcher;
|
|
pose = saved_pose;
|
|
memcpy(joints, saved_joints, sizeof(joints));
|
|
after_hash = sim_kinematics_switch_state_hash(&switcher, joints, 5, &pose);
|
|
assert(before_hash == after_hash && sim_kinematics_switch_active(&switcher) == 1);
|
|
|
|
printf("{\"case\":\"xyzac-trt.ini\",\"levels\":{");
|
|
printf("\"protocol\":[");
|
|
for (index = 0; index < 4; ++index) {
|
|
if (index) printf(",");
|
|
printf("{\"command\":\"%s\",\"result\":\"verified\",\"seq\":%u,\"tick\":%u}", commands[index], (unsigned)(index + 4), (unsigned)(index + 1));
|
|
}
|
|
printf("],\"control\":[");
|
|
for (index = 0; index < 4; ++index) {
|
|
if (index) printf(",");
|
|
printf("{\"active_type\":%d,\"command\":\"%s\",\"hal_pin\":\"motion.switchkins-type\",\"tick\":%u}", modes[index], commands[index], (unsigned)(index + 1));
|
|
}
|
|
printf("],\"state\":[");
|
|
for (index = 0; index < 4; ++index) {
|
|
if (index) printf(",");
|
|
printf("{\"active_type\":%d,\"joints\":", modes[index]);
|
|
print_joints(state_joints[index]);
|
|
printf(",\"pose\":");
|
|
print_pose(&poses[index]);
|
|
printf(",\"tick\":%u}", (unsigned)(index + 1));
|
|
}
|
|
printf("],\"canonical\":[{\"end\":");
|
|
print_pose(&arc.end);
|
|
printf(",\"motion_type\":%d,\"source_line\":4,\"tick\":5,\"turn\":%d,\"velocity\":%.17g}],", arc.motion_type, arc.turn, arc.velocity);
|
|
printf("\"recovery\":[{\"active_type\":%d,\"joints_equal\":true,\"pose_equal\":true,\"state_hash_equal\":true,\"tick\":6}]},", sim_kinematics_switch_active(&switcher));
|
|
printf("\"schema\":\"xyzac-structured-trace.v1\",\"source\":\"linuxcnc-native-sim-core\",\"status\":\"verified\"}\n");
|
|
return 0;
|
|
}
|