175 lines
5.7 KiB
C
175 lines
5.7 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,
|
|
};
|
|
|
|
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
|
|
|
|
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;
|
|
|
|
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);
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif
|