309 lines
12 KiB
C
309 lines
12 KiB
C
#ifndef LINUXCNC_WEB_WASM_SIM_KINEMATICS_H
|
|
#define LINUXCNC_WEB_WASM_SIM_KINEMATICS_H
|
|
|
|
#include "sim_posemath.h"
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
enum sim_kinematics_result {
|
|
SIM_KIN_OK = 0,
|
|
SIM_KIN_ERR_ARGUMENT = -700,
|
|
SIM_KIN_ERR_NONFINITE = -701,
|
|
SIM_KIN_ERR_RANGE = -702,
|
|
SIM_KIN_ERR_CONFIGURATION = -703,
|
|
SIM_KIN_ERR_SINGULAR = -704,
|
|
SIM_KIN_ERR_UNKNOWN_TYPE = -705,
|
|
SIM_KIN_ERR_UNSUPPORTED = -706,
|
|
SIM_KIN_ERR_INVALID_MODE = -707,
|
|
SIM_KIN_ERR_MISSING_MODULE = -708,
|
|
SIM_KIN_ERR_SWITCH_INTERRUPTED = -709,
|
|
SIM_KIN_ERR_ABI_VERSION = -710,
|
|
SIM_KIN_ERR_CAPABILITY = -711,
|
|
SIM_KIN_ERR_DYNAMIC_MODULE = -712,
|
|
};
|
|
|
|
enum sim_kinematics_type {
|
|
SIM_KIN_TRIVKINS = 1,
|
|
SIM_KIN_XYZAC_TRT = 2,
|
|
SIM_KIN_XYZBC_TRT = 3,
|
|
SIM_KIN_XYZBCW = 4,
|
|
SIM_KIN_COREXY = 5,
|
|
SIM_KIN_ROSE = 6,
|
|
SIM_KIN_MAXKINS = 7,
|
|
SIM_KIN_ROTATEKINS = 8,
|
|
SIM_KIN_5AXISKINS = 9,
|
|
SIM_KIN_SCARA = 10,
|
|
SIM_KIN_PUMA = 11,
|
|
SIM_KIN_SCORBOT = 12,
|
|
SIM_KIN_GENSER = 13,
|
|
SIM_KIN_LINEAR_DELTA = 14,
|
|
SIM_KIN_ROTARY_DELTA = 15,
|
|
SIM_KIN_TRIPOD = 16,
|
|
SIM_KIN_GENHEX = 17,
|
|
SIM_KIN_PENTAPOD = 18,
|
|
};
|
|
|
|
/* LinuxCNC kinematics mode semantics. Zero means the module default. */
|
|
enum sim_kinematics_mode {
|
|
SIM_KIN_MODE_DEFAULT = 0,
|
|
SIM_KIN_MODE_IDENTITY = 1,
|
|
SIM_KIN_MODE_FORWARD_ONLY = 2,
|
|
SIM_KIN_MODE_INVERSE_ONLY = 3,
|
|
SIM_KIN_MODE_BOTH = 4,
|
|
};
|
|
|
|
/* Delta mechanisms expose the two analytic roots through the flags fields.
|
|
* The default (zero) is LinuxCNC's lower/platform root; bit zero selects the
|
|
* alternate root for callers that need an explicit branch comparison. */
|
|
#define SIM_KIN_DELTA_ALTERNATE_ROOT UINT64_C(1)
|
|
/* Tripod forward flags select the below-base solution when set. */
|
|
#define SIM_KIN_TRIPOD_BELOW_ROOT UINT64_C(1)
|
|
/* Genhex forward flags select the alternate (below-platform) initial seed. */
|
|
#define SIM_KIN_GENHEX_ALTERNATE_SEED UINT64_C(2)
|
|
|
|
#define SIM_KIN_MAX_JOINTS 9u
|
|
#define SIM_KIN_GENHEX_STRUTS 6u
|
|
#define SIM_KIN_PENTA_STRUTS 5u
|
|
#define SIM_KIN_SWITCH_MAX_TYPES 3u
|
|
#define SIM_KIN_USER_ABI_VERSION 1u
|
|
#define SIM_KIN_USER_CAP_FORWARD UINT32_C(1)
|
|
#define SIM_KIN_USER_CAP_INVERSE UINT32_C(2)
|
|
#define SIM_KIN_USER_CAP_STATIC UINT32_C(4)
|
|
#define SIM_KIN_USER_NAME_CAPACITY 32u
|
|
#define SIM_KIN_REGISTRY_ABI_VERSION 1u
|
|
#define SIM_KIN_REGISTRY_NAME_CAPACITY 32u
|
|
#define SIM_KIN_CAP_FORWARD UINT32_C(1)
|
|
#define SIM_KIN_CAP_INVERSE UINT32_C(2)
|
|
#define SIM_KIN_CAP_IDENTITY UINT32_C(4)
|
|
#define SIM_KIN_CAP_ITERATIVE UINT32_C(8)
|
|
#define SIM_KIN_CAP_SWITCHABLE UINT32_C(16)
|
|
#define SIM_KIN_CAP_EXTRA_JOINTS UINT32_C(32)
|
|
#define SIM_KIN_CAP_DUPLICATE_COORDINATES UINT32_C(64)
|
|
#define SIM_KIN_PARAM_MAPPING UINT64_C(1)
|
|
#define SIM_KIN_PARAM_LIMITS UINT64_C(2)
|
|
#define SIM_KIN_PARAM_OFFSETS UINT64_C(4)
|
|
#define SIM_KIN_PARAM_TOOL UINT64_C(8)
|
|
#define SIM_KIN_PARAM_PIVOT UINT64_C(16)
|
|
#define SIM_KIN_PARAM_GEOMETRY UINT64_C(32)
|
|
#define SIM_KIN_PARAM_DH UINT64_C(64)
|
|
#define SIM_KIN_PARAM_SOLVER UINT64_C(128)
|
|
|
|
enum sim_kinematics_user_implementation {
|
|
SIM_KIN_USER_IDENTITY = 1,
|
|
SIM_KIN_USER_GENSER = 2,
|
|
};
|
|
|
|
typedef struct sim_kinematics_config {
|
|
int type;
|
|
int mode;
|
|
uint32_t joint_count;
|
|
/* Total configured joints and the prefix owned by kinematics. */
|
|
uint32_t kinematics_joint_count;
|
|
uint32_t extra_joint_count;
|
|
char coordinates[16];
|
|
double x_rot_point;
|
|
double y_rot_point;
|
|
double z_rot_point;
|
|
double x_offset;
|
|
double y_offset;
|
|
double z_offset;
|
|
double tool_offset;
|
|
double pivot_length;
|
|
double scara_d1;
|
|
double scara_d2;
|
|
double scara_d3;
|
|
double scara_d4;
|
|
double scara_d5;
|
|
double scara_d6;
|
|
double puma_a2;
|
|
double puma_a3;
|
|
double puma_d3;
|
|
double puma_d4;
|
|
double puma_d6;
|
|
uint32_t dh_joint_count;
|
|
double dh_a[SIM_KIN_MAX_JOINTS];
|
|
double dh_alpha[SIM_KIN_MAX_JOINTS];
|
|
double dh_d[SIM_KIN_MAX_JOINTS];
|
|
double delta_radius;
|
|
double delta_rod_length;
|
|
double rotary_platform_radius;
|
|
double rotary_thigh_length;
|
|
double rotary_shin_length;
|
|
double rotary_foot_radius;
|
|
double tripod_bx;
|
|
double tripod_cx;
|
|
double tripod_cy;
|
|
double genhex_base[SIM_KIN_GENHEX_STRUTS][3];
|
|
double genhex_platform[SIM_KIN_GENHEX_STRUTS][3];
|
|
double genhex_base_axis[SIM_KIN_GENHEX_STRUTS][3];
|
|
double genhex_platform_axis[SIM_KIN_GENHEX_STRUTS][3];
|
|
double genhex_screw_lead;
|
|
double genhex_tool_offset;
|
|
double genhex_spindle_offset;
|
|
double genhex_max_error;
|
|
double genhex_convergence_tolerance;
|
|
uint32_t genhex_iteration_limit;
|
|
double genhex_initial_pose[6];
|
|
double penta_base[SIM_KIN_PENTA_STRUTS][3];
|
|
double penta_effector_radius[SIM_KIN_PENTA_STRUTS];
|
|
double penta_effector_z[SIM_KIN_PENTA_STRUTS];
|
|
double penta_tool_offset;
|
|
double penta_max_error;
|
|
double penta_convergence_tolerance;
|
|
uint32_t penta_iteration_limit;
|
|
double penta_initial_pose[5];
|
|
uint32_t max_iterations;
|
|
double convergence_tolerance;
|
|
uint8_t limit_enabled[SIM_KIN_MAX_JOINTS];
|
|
double joint_min[SIM_KIN_MAX_JOINTS];
|
|
double joint_max[SIM_KIN_MAX_JOINTS];
|
|
} sim_kinematics_config;
|
|
|
|
typedef struct sim_kinematics {
|
|
sim_kinematics_config config;
|
|
int axis_joint[SIM_KIN_MAX_JOINTS];
|
|
uint32_t configured;
|
|
uint64_t forward_flags;
|
|
uint64_t inverse_flags;
|
|
sim_posemath_pose last_pose;
|
|
double last_residual;
|
|
uint32_t last_iterations;
|
|
uint32_t branch_id;
|
|
int32_t revolution_count;
|
|
uint32_t previous_quadrant;
|
|
} sim_kinematics;
|
|
|
|
typedef struct sim_kinematics_result_info {
|
|
uint64_t forward_flags;
|
|
uint64_t inverse_flags;
|
|
double residual;
|
|
uint32_t iterations;
|
|
uint32_t branch_id;
|
|
} sim_kinematics_result_info;
|
|
|
|
/* A bounded switchkins container matching LinuxCNC's three registered
|
|
* kinematics slots. The container owns switching state, not the modules. */
|
|
typedef struct sim_kinematics_switch {
|
|
sim_kinematics *modules[SIM_KIN_SWITCH_MAX_TYPES];
|
|
uint8_t registered[SIM_KIN_SWITCH_MAX_TYPES];
|
|
uint8_t fwd_iterates_mask;
|
|
uint8_t switching;
|
|
int active_mode;
|
|
int pending_mode;
|
|
uint32_t generation;
|
|
uint32_t switch_count;
|
|
uint32_t abort_count;
|
|
sim_posemath_pose last_pose[SIM_KIN_SWITCH_MAX_TYPES];
|
|
uint8_t have_last_pose[SIM_KIN_SWITCH_MAX_TYPES];
|
|
} sim_kinematics_switch;
|
|
|
|
/* User kinematics are selected from implementations already linked into the
|
|
* WASM module. No function pointers, host paths, or dynamic libraries enter
|
|
* this descriptor. */
|
|
typedef struct sim_kinematics_user_descriptor {
|
|
uint32_t abi_version;
|
|
uint32_t capabilities;
|
|
int implementation;
|
|
char name[SIM_KIN_USER_NAME_CAPACITY];
|
|
sim_kinematics_config config;
|
|
} sim_kinematics_user_descriptor;
|
|
|
|
typedef struct sim_kinematics_user {
|
|
sim_kinematics_user_descriptor descriptor;
|
|
sim_kinematics module;
|
|
uint32_t initialized;
|
|
uint32_t forward_calls;
|
|
uint32_t inverse_calls;
|
|
} sim_kinematics_user;
|
|
|
|
typedef struct sim_kinematics_capability {
|
|
uint32_t abi_version;
|
|
int type;
|
|
char name[SIM_KIN_REGISTRY_NAME_CAPACITY];
|
|
char required_coordinates[16];
|
|
uint32_t min_joints;
|
|
uint32_t max_joints;
|
|
uint32_t capabilities;
|
|
uint64_t parameter_mask;
|
|
} sim_kinematics_capability;
|
|
|
|
int sim_kinematics_init(sim_kinematics *kinematics, const sim_kinematics_config *config);
|
|
int sim_kinematics_forward(const sim_kinematics *kinematics, const double *joints,
|
|
uint32_t joint_count, sim_posemath_pose *pose);
|
|
int sim_kinematics_forward_ex(const sim_kinematics *kinematics, const double *joints,
|
|
uint32_t joint_count, uint64_t inverse_flags,
|
|
sim_posemath_pose *pose, sim_kinematics_result_info *info);
|
|
int sim_kinematics_inverse(sim_kinematics *kinematics, const sim_posemath_pose *pose,
|
|
double *joints, uint32_t joint_count);
|
|
int sim_kinematics_inverse_ex(sim_kinematics *kinematics, const sim_posemath_pose *pose,
|
|
uint32_t joint_count, uint64_t forward_flags,
|
|
double *joints, sim_kinematics_result_info *info);
|
|
int sim_kinematics_home(const sim_kinematics *kinematics, double *joints, uint32_t joint_count,
|
|
sim_posemath_pose *pose);
|
|
int sim_kinematics_validate_pose(const sim_posemath_pose *pose);
|
|
uint64_t sim_kinematics_state_hash(const sim_kinematics *kinematics,
|
|
const double *joints, uint32_t joint_count,
|
|
const sim_posemath_pose *pose);
|
|
|
|
int sim_kinematics_switch_init(sim_kinematics_switch *switcher, int default_mode,
|
|
uint8_t fwd_iterates_mask);
|
|
int sim_kinematics_switch_register(sim_kinematics_switch *switcher, int mode,
|
|
sim_kinematics *module, uint8_t fwd_iterates);
|
|
int sim_kinematics_switch_begin(sim_kinematics_switch *switcher, int mode);
|
|
int sim_kinematics_switch_abort(sim_kinematics_switch *switcher);
|
|
int sim_kinematics_switch_commit(sim_kinematics_switch *switcher, double *joints,
|
|
uint32_t joint_count, sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_switch_select(sim_kinematics_switch *switcher, int mode,
|
|
double *joints, uint32_t joint_count,
|
|
sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_switch_forward(sim_kinematics_switch *switcher,
|
|
const double *joints, uint32_t joint_count,
|
|
sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_switch_inverse(sim_kinematics_switch *switcher,
|
|
const sim_posemath_pose *pose, double *joints,
|
|
uint32_t joint_count,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_switch_active(const sim_kinematics_switch *switcher);
|
|
uint64_t sim_kinematics_switch_state_hash(const sim_kinematics_switch *switcher,
|
|
const double *joints, uint32_t joint_count,
|
|
const sim_posemath_pose *pose);
|
|
int sim_kinematics_user_init(sim_kinematics_user *user,
|
|
const sim_kinematics_user_descriptor *descriptor);
|
|
int sim_kinematics_user_bind_switch(sim_kinematics_user *user,
|
|
sim_kinematics_switch *switcher, int mode,
|
|
uint8_t fwd_iterates);
|
|
int sim_kinematics_user_forward(sim_kinematics_user *user, const double *joints,
|
|
uint32_t joint_count, sim_posemath_pose *pose,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_user_inverse(sim_kinematics_user *user,
|
|
const sim_posemath_pose *pose, double *joints,
|
|
uint32_t joint_count,
|
|
sim_kinematics_result_info *info);
|
|
int sim_kinematics_user_reject_dynamic(const char *module_path);
|
|
uint64_t sim_kinematics_user_state_hash(const sim_kinematics_user *user,
|
|
const double *joints, uint32_t joint_count,
|
|
const sim_posemath_pose *pose);
|
|
uint32_t sim_kinematics_registry_count(void);
|
|
int sim_kinematics_registry_get(uint32_t index,
|
|
sim_kinematics_capability *capability);
|
|
int sim_kinematics_registry_find(const char *name,
|
|
sim_kinematics_capability *capability);
|
|
int sim_kinematics_registry_require(const char *name, uint32_t capabilities,
|
|
uint64_t parameter_mask,
|
|
sim_kinematics_capability *capability);
|
|
int sim_kinematics_registry_validate_config(const char *name,
|
|
const sim_kinematics_config *config,
|
|
sim_kinematics_capability *capability);
|
|
uint64_t sim_kinematics_registry_state_hash(void);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif
|