wangdequan/cnc_wams
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

按建议，继续下一步工作

Browse Source 
结论：已将 LinuxCNC TP 源码纳入 wasm-port 的可复现 vendor 清单和 native probe 构建，新增真实 tpCreate/tpAddLine/tpRunCycle 线性运动验证，并通过 native probes 全量验证。
This commit is contained in:
wangdequan
2026-06-07 07:39:26 +08:00
parent 078f1f6f8e
commit e328394184
72 changed files with 27200 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.h vendored Normal file
View File

@@ -0,0 +1,62 @@
/********************************************************************
* Description: cubic.h
* Cubic polynomial interpolation code
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#ifndef CUBIC_H
#define CUBIC_H
/*
Coefficients of a cubic polynomial,
a * x^3 + b * x^2 + c * x + d
*/
typedef struct {
double a;
double b;
double c;
double d;
} CUBIC_COEFF;
typedef struct {
int configured;
double segmentTime;
int interpolationRate;
double interpolationTime;
double interpolationIncrement;
double x0, x1, x2, x3;
double wp0, wp1;
double velp0, velp1;
int filled;
int needNextPoint;
CUBIC_COEFF coeff;
} CUBIC_STRUCT;
extern int cubicInit(CUBIC_STRUCT * ci);
extern int cubicSetSegmentTime(CUBIC_STRUCT * ci, double time);
extern double cubicGetSegmentTime(CUBIC_STRUCT * ci);
extern int cubicSetInterpolationRate(CUBIC_STRUCT * ci, int rate);
extern int cubicGetInterpolationRate(CUBIC_STRUCT * ci);
extern int cubicAddPoint(CUBIC_STRUCT * ci, double point);
extern int cubicOffset(CUBIC_STRUCT * ci, double offset);
extern double cubicGetInterpolationIncrement(CUBIC_STRUCT * ci);
extern CUBIC_COEFF cubicGetCubicCoeff(CUBIC_STRUCT * ci);
extern int cubicFilled(CUBIC_STRUCT * ci);
extern double cubicInterpolate(CUBIC_STRUCT * ci, double *x, /* same as
return val
*/
double *v, /* velocity */
double *a, /* accel */
double *j); /* jerk */
extern int cubicNeedNextPoint(CUBIC_STRUCT * ci);
extern int cubicDrain(CUBIC_STRUCT * ci);
#endif /* CUBIC_H */

215
wasm-port/vendor/linuxcnc/src/emc/kinematics/kinematics.h vendored Normal file
View File

@@ -0,0 +1,215 @@
/********************************************************************
* Description: kinematics.h
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef __LINUXCNC_KINEMATICS_H
#define __LINUXCNC_KINEMATICS_H
#include "emcpos.h" /* EmcPose */
#include "rtapi_bool.h"
/*
The type of kinematics used.
KINEMATICS_IDENTITY means that the joints and world coordinates are the
same, as for slideway machines (XYZ milling machines). The EMC will allow
changing from joint to world mode and vice versa. Also, the EMC will set
the actual world position to be the actual joint positions (not commanded)
by calling the forward kinematics each trajectory cycle.
KINEMATICS_FORWARD_ONLY means that only the forward kinematics exist.
Since the EMC requires at least the inverse kinematics, this should simply
terminate the EMC.
KINEMATICS_INVERSE_ONLY means that only the inverse kinematics exist.
The forwards won't be called, and the EMC will only allow changing from
joint to world mode at the home position.
KINEMATICS_BOTH means that both the forward and inverse kins are defined.
Like KINEMATICS_IDENTITY, the EMC will allow changing between world and
joint modes. However, the kins are assumed to be somewhat expensive
computationally, and the forwards won't be called at the trajectory rate
to compute actual world coordinates from actual joint values.
*/
typedef enum {
KINEMATICS_IDENTITY = 1,/* forward=inverse, both well-behaved */
KINEMATICS_FORWARD_ONLY,/* forward but no inverse */
KINEMATICS_INVERSE_ONLY,/* inverse but no forward */
KINEMATICS_BOTH /* forward and inverse both */
} KINEMATICS_TYPE;
/* the forward flags are passed to the forward kinematics so that they
can resolve ambiguities in the world coordinates for a given joint set,
e.g., for hexpods, this would be platform-below-base, platform-above-base.
The flags are also passed to the inverse kinematics and are set by them,
which is how they are changed from their initial value. For example, for
hexapods you could do a coordinated move that brings the platform up from
below the base to above the base. The forward flags would be set to
indicate this. */
typedef unsigned long int KINEMATICS_FORWARD_FLAGS;
/* the inverse flags are passed to the inverse kinematics so that they
can resolve ambiguities in the joint angles for a given world coordinate,
e.g., for robots, this would be elbow-up, elbow-down, etc.
The flags are also passed to the forward kinematics and are set by them,
which is how they are changed from their initial value. For example, for
robots you could do a joint move that brings the elbow from a down
configuration to an up configuration. The inverse flags would be set to
indicate this. */
typedef unsigned long int KINEMATICS_INVERSE_FLAGS;
/* the forward kinematics take joint values and determine world coordinates,
given forward kinematics flags to resolve any ambiguities. The inverse
flags are set to indicate their value appropriate to the joint values
passed in. */
extern int kinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
/* the inverse kinematics take world coordinates and determine joint values,
given the inverse kinematics flags to resolve any ambiguities. The forward
flags are set to indicate their value appropriate to the world coordinates
passed in. */
extern int kinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
/* the home kinematics function sets all its arguments to their proper
values at the known home position. When called, these should be set,
when known, to initial values, e.g., from an INI file. If the home
kinematics can accept arbitrary starting points, these initial values
should be used.
*/
extern int kinematicsHome(struct EmcPose * world,
double *joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern KINEMATICS_TYPE kinematicsType(void);
/* parameters for use with switchkins.c */
typedef struct kinematics_parms {
char* sparm; // module string parameter passed to kins
char* kinsname; // must agree with module(file) name
char* halprefix; // for hal pin hames
char* required_coordinates;
int max_joints;
int allow_duplicates;
int fwd_iterates_mask; // identify kins types that use iterative
// forward kinematics (typ: genhex)
// bitmask: 0x0 none
// bitmask: 0x1 bit0: switchkins_type==0
// bitmask: 0x2 bit1: switchkins_type==1
// bitmask: 0x4 bit2: switchkins_type==2
int gui_kinstype; // may be reqd for parallel kins with vismach
// to select switchkins_type for gui pins
} kparms;
/* map letters in a coordinates string to joint numbers
** sequentially. Axis indices are 0:x,1:y,...,etc
** Example: coordinates=XYZYAC
** Result: axis_idx_for_jno[0] = 0 ==> X
** axis_idx_for_jno[1] = 1 ==> Y
** axis_idx_for_jno[2] = 2 ==> Z
** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed)
** axis_idx_for_jno[4] = 1 ==> A
** axis_idx_for_jno[5] = 1 ==> C
*/
extern int map_coordinates_to_jnumbers(const char *coordinates,
const int max_joints,
const int allow_duplicates,
int axis_idx_for_jno[]);
extern int mapped_joints_to_position(const int max_joints,
const double* joints,
EmcPose* pose);
extern int position_to_mapped_joints(const int max_joints,
const EmcPose* pos,
double* joints);
extern int identityKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int identityKinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int identityKinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
extern int kinematicsSwitchable(void);
extern int kinematicsSwitch(int switchkins_type);
//NOTE: switchable kinematics may require Interp::Synch
// before/after invoking kinematicsSwitch()
// A convenient command to synch is: M66 E0 L0
#define KINS_NOT_SWITCHABLE \
extern int kinematicsSwitchable() {return 0;} \
extern int kinematicsSwitch(int switchkins_type) { (void)switchkins_type; return 0;} \
EXPORT_SYMBOL(kinematicsSwitchable); \
EXPORT_SYMBOL(kinematicsSwitch);
// support for template for user-defined switchkins_type==2
extern int userkKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int userkKinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int userkKinematicsInverse(const struct EmcPose * world,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
#endif
//*********************************************************************
// xyzac,xyzbc;
extern int trtKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
extern int xyzacKinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int xyzacKinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
extern int xyzbcKinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int xyzbcKinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
//*********************************************************************

60
wasm-port/vendor/linuxcnc/src/emc/motion/axis.h vendored Normal file
View File

@@ -0,0 +1,60 @@
#ifndef AXIS_H
#define AXIS_H
#include <rtapi_bool.h>
#include <hal.h>
#ifdef __cplusplus
extern "C" {
#endif
void axis_init_all(void);
void axis_initialize_external_offsets(void);
int axis_init_hal_io(int mot_comp_id);
void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, bool homing_is_active);
bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bool all_homed);
void axis_check_constraints(double pos[], int failing_axes[]);
void axis_jog_cont(int axis_num, double vel, long servo_period);
void axis_jog_incr(int axis_num, double offset, double vel, long servo_period);
void axis_jog_abs(int axis_num, double offset, double vel);
bool axis_jog_abort_all(bool immediate);
bool axis_jog_abort(int axis_num, bool immediate);
bool axis_jog_is_active(void);
void axis_output_to_hal(double *pcmd_p[]);
void axis_set_max_pos_limit(int axis_num, double maxLimit);
void axis_set_min_pos_limit(int axis_num, double minLimit);
void axis_set_vel_limit(int axis_num, double vel);
void axis_set_acc_limit(int axis_num, double acc);
void axis_set_jerk_limit(int axis_num, double jerk);
void axis_set_ext_offset_vel_limit(int axis_num, double ext_offset_vel);
void axis_set_ext_offset_acc_limit(int axis_num, double ext_offset_acc);
void axis_set_locking_joint(int axis_num, int joint);
double axis_get_min_pos_limit(int axis_num);
double axis_get_max_pos_limit(int axis_num);
double axis_get_vel_limit(int axis_num);
double axis_get_acc_limit(int axis_num);
int axis_get_locking_joint(int axis_num);
double axis_get_compound_velocity(void);
double axis_get_ext_offset_curr_pos(int axis_num);
double axis_get_teleop_vel_cmd(int axis_num);
void axis_sync_teleop_tp_to_carte_pos(int extfactor, double *pcmd_p[]);
void axis_sync_carte_pos_to_teleop_tp(int extfactor, double *pcmd_p[]);
void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]);
int axis_update_coord_with_bound(double *pcmd_p[], double servo_period);
int axis_calc_motion(double servo_period);
#ifdef __cplusplus
}
#endif
#endif /* AXIS_H */

355
wasm-port/vendor/linuxcnc/src/emc/motion/mot_priv.h vendored Normal file
View File

@@ -0,0 +1,355 @@
/*******************************************************************
* Description: mot_priv.h
* Macros and declarations local to the realtime sources.
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#ifndef MOT_PRIV_H
#define MOT_PRIV_H
/***********************************************************************
* TYPEDEFS, ENUMS, ETC. *
************************************************************************/
/* First we define structures for data shared with the HAL */
/* HAL visible data notations:
RPA: read only parameter
WPA: write only parameter
WRPA: read/write parameter
RPI: read only pin
WPI: write only pin
WRPI: read/write pin
*/
/* joint data */
#include <hal.h>
#include "../motion/motion.h"
typedef struct {
// creating a lot of pins for spindle control to be very flexible
// the user needs only a subset of these
// simplest way of spindle control (output start/stop)
hal_bit_t *spindle_on; /* spindle spin output */
// same thing for 2 directions
hal_bit_t *spindle_forward; /* spindle spin-forward output */
hal_bit_t *spindle_reverse; /* spindle spin-reverse output */
// simple velocity control (as long as the output is active the spindle
// should accelerate/decelerate
hal_bit_t *spindle_incr_speed; /* spindle spin-increase output */
hal_bit_t *spindle_decr_speed; /* spindle spin-decrease output */
// simple output for brake
hal_bit_t *spindle_brake; /* spindle brake output */
// output of a prescribed speed (to hook-up to a velocity controller)
hal_float_t *spindle_speed_out; /* spindle speed output */
hal_float_t *spindle_speed_out_rps; /* spindle speed output */
hal_float_t *spindle_speed_out_abs; /* spindle speed output absolute*/
hal_float_t *spindle_speed_out_rps_abs; /* spindle speed output absolute*/
hal_float_t *spindle_speed_cmd_rps; /* spindle speed command without SO applied */
hal_float_t *spindle_speed_in; /* spindle speed measured */
hal_bit_t *spindle_index_enable; /* spindle inde I/O pin */
hal_bit_t *spindle_inhibit;
hal_float_t *spindle_revs;
hal_bit_t *spindle_is_atspeed;
hal_bit_t *spindle_amp_fault;
// spindle orient
hal_float_t *spindle_orient_angle; /* out: desired spindle angle, degrees */
hal_s32_t *spindle_orient_mode; /* out: 0: least travel; 1: cw; 2: ccw */
hal_bit_t *spindle_orient; /* out: signal orient in progress */
hal_bit_t *spindle_locked; /* out: signal orient complete, spindle locked */
hal_bit_t *spindle_is_oriented; /* in: orientation completed */
hal_s32_t *spindle_orient_fault; /* in: error code of failed operation */
} spindle_hal_t;
typedef struct {
hal_float_t *coarse_pos_cmd;/* RPI: commanded position, w/o comp */
hal_float_t *joint_vel_cmd; /* RPI: commanded velocity, w/o comp */
hal_float_t *joint_acc_cmd; /* RPI: commanded acceleration, w/o comp */
hal_float_t *joint_jerk_cmd;/* RPI: commanded jerk, w/o comp */
hal_float_t *backlash_corr; /* RPI: correction for backlash */
hal_float_t *backlash_filt; /* RPI: filtered backlash correction */
hal_float_t *backlash_vel; /* RPI: backlash speed variable */
hal_float_t *motor_offset; /* RPI: motor offset, for checking homing stability */
hal_float_t *motor_pos_cmd; /* WPI: commanded position, with comp */
hal_float_t *motor_pos_fb; /* RPI: position feedback, with comp */
hal_float_t *joint_pos_cmd; /* WPI: commanded position w/o comp, not ofs */
hal_float_t *joint_pos_fb; /* RPI: position feedback, w/o comp */
hal_float_t *f_error; /* RPI: following error */
hal_float_t *f_error_lim; /* RPI: following error limit */
hal_float_t *free_pos_cmd; /* RPI: free traj planner pos cmd */
hal_float_t *free_vel_lim; /* RPI: free traj planner vel limit */
hal_bit_t *free_tp_enable; /* RPI: free traj planner is running */
hal_bit_t *kb_jjog_active; /* RPI: executing keyboard jog */
hal_bit_t *wheel_jjog_active;/* RPI: executing handwheel jog */
hal_bit_t *active; /* RPI: joint is active, whatever that means */
hal_bit_t *in_position; /* RPI: joint is in position */
hal_bit_t *error; /* RPI: joint has an error */
hal_bit_t *phl; /* RPI: joint is at positive hard limit */
hal_bit_t *nhl; /* RPI: joint is at negative hard limit */
hal_bit_t *f_errored; /* RPI: joint had too much following error */
hal_bit_t *faulted; /* RPI: joint amp faulted */
hal_bit_t *pos_lim_sw; /* RPI: positive limit switch input */
hal_bit_t *neg_lim_sw; /* RPI: negative limit switch input */
hal_bit_t *amp_fault; /* RPI: amp fault input */
hal_bit_t *amp_enable; /* WPI: amp enable output */
hal_bit_t *unlock; /* WPI: command that axis should unlock for rotation */
hal_bit_t *is_unlocked; /* RPI: axis is currently unlocked */
hal_s32_t *jjog_counts; /* WPI: jogwheel position input */
hal_bit_t *jjog_enable; /* RPI: enable jogwheel */
hal_float_t *jjog_scale; /* RPI: distance to jog on each count */
hal_float_t *jjog_accel_fraction; /* RPI: to limit wheel jog accel */
hal_bit_t *jjog_vel_mode; /* RPI: true for "velocity mode" jogwheel */
} joint_hal_t;
typedef struct {
hal_float_t *posthome_cmd; // IN pin extrajoint
} extrajoint_hal_t;
/* machine data */
typedef struct {
hal_bit_t *probe_input; /* RPI: probe switch input */
hal_bit_t *enable; /* RPI: motion inhibit input */
hal_float_t *adaptive_feed; /* RPI: adaptive feedrate, 0.0 to 1.0 */
hal_bit_t *feed_hold; /* RPI: set TRUE to stop motion maskable with g53 P1*/
hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/
hal_bit_t *homing_inhibit; /* RPI: set TRUE to inhibit homing*/
hal_bit_t *jog_inhibit; /* RPI: set TRUE to inhibit jogging*/
hal_bit_t *jog_stop; /* RPI: set TRUE to stop jogging following accel values*/
hal_bit_t *jog_stop_immediate; /* RPI: set TRUE to stop jogging immediately*/
hal_bit_t *jog_is_active; /* RPI: TRUE if active jogging*/
hal_bit_t *tp_reverse; /* Set true if trajectory planner is running in reverse*/
hal_bit_t *motion_enabled; /* RPI: motion enable for all joints */
hal_bit_t *is_all_homed; /* RPI: TRUE if all active joints is homed */
hal_bit_t *in_position; /* RPI: all joints are in position */
hal_bit_t *coord_mode; /* RPA: TRUE if coord, FALSE if free */
hal_bit_t *teleop_mode; /* RPA: TRUE if teleop mode */
hal_bit_t *coord_error; /* RPA: TRUE if coord mode error */
hal_bit_t *on_soft_limit; /* RPA: TRUE if outside a limit */
hal_s32_t *program_line; /* RPA: program line causing current motion */
hal_s32_t *motion_type; /* RPA: type (feed/rapid) of currently commanded motion */
hal_float_t *current_vel; /* RPI: velocity magnitude in machine units */
hal_float_t *requested_vel; /* RPI: requested velocity magnitude in machine units */
hal_float_t *distance_to_go;/* RPI: distance to go in current move*/
hal_bit_t debug_bit_0; /* RPA: generic param, for debugging */
hal_bit_t debug_bit_1; /* RPA: generic param, for debugging */
hal_float_t debug_float_0; /* RPA: generic param, for debugging */
hal_float_t debug_float_1; /* RPA: generic param, for debugging */
hal_float_t debug_float_2; /* RPA: generic param, for debugging */
hal_float_t debug_float_3; /* RPA: generic param, for debugging */
hal_s32_t debug_s32_0; /* RPA: generic param, for debugging */
hal_s32_t debug_s32_1; /* RPA: generic param, for debugging */
hal_bit_t *synch_do[EMCMOT_MAX_DIO]; /* WPI array: output pins for motion synched IO */
hal_bit_t *synch_di[EMCMOT_MAX_DIO]; /* RPI array: input pins for motion synched IO */
hal_float_t *analog_input[EMCMOT_MAX_AIO]; /* RPI array: input pins for analog Inputs */
hal_float_t *analog_output[EMCMOT_MAX_AIO]; /* RPI array: output pins for analog Inputs */
hal_bit_t *misc_error[EMCMOT_MAX_MISC_ERROR]; /* RPI array: output pins for misc error Inputs */
// FIXME - debug only, remove later
hal_float_t traj_pos_out; /* RPA: traj internals, for debugging */
hal_float_t traj_vel_out; /* RPA: traj internals, for debugging */
hal_u32_t traj_active_tc; /* RPA: traj internals, for debugging */
hal_float_t tc_pos[4]; /* RPA: traj internals, for debugging */
hal_float_t tc_vel[4]; /* RPA: traj internals, for debugging */
hal_float_t tc_acc[4]; /* RPA: traj internals, for debugging */
// realtime overrun detection
hal_u32_t *last_period; /* pin: last period in clocks */
hal_float_t *last_period_ns; /* pin: last period in nanoseconds */
hal_float_t *tooloffset_x;
hal_float_t *tooloffset_y;
hal_float_t *tooloffset_z;
hal_float_t *tooloffset_a;
hal_float_t *tooloffset_b;
hal_float_t *tooloffset_c;
hal_float_t *tooloffset_u;
hal_float_t *tooloffset_v;
hal_float_t *tooloffset_w;
spindle_hal_t spindle[EMCMOT_MAX_SPINDLES]; /*spindle data */
joint_hal_t joint[EMCMOT_MAX_JOINTS]; /* data for each joint */
extrajoint_hal_t ejoint[EMCMOT_MAX_EXTRAJOINTS]; /* data for each extrajoint */
hal_bit_t *eoffset_active; /* ext offsets active */
hal_bit_t *eoffset_limited; /* ext offsets exceed limit */
hal_float_t *feed_upm; /* feed G-code units per minute*/
hal_float_t *feed_inches_per_minute; /* feed inches per minute*/
hal_float_t *feed_inches_per_second; /* feed inches per second*/
hal_float_t *feed_mm_per_minute; /* feed mm per minute*/
hal_float_t *feed_mm_per_second; /* feed mm per second*/
hal_float_t *switchkins_type;
/* Interp State Pins */
hal_s32_t *interp_line_number;
hal_s32_t *interp_motion_type;
hal_float_t *interp_feedrate;
/* New Geometric Metadata Pins */
hal_float_t *interp_arc_radius;
hal_float_t *interp_arc_center_x;
hal_float_t *interp_arc_center_y;
hal_float_t *interp_arc_center_z;
hal_float_t *interp_straight_heading;
hal_float_t *interp_normal_heading;
hal_bit_t *iscircle;
} emcmot_hal_data_t;
/***********************************************************************
* GLOBAL VARIABLE DECLARATIONS *
************************************************************************/
/* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */
extern emcmot_hal_data_t *emcmot_hal_data;
/* pointer to array of joint structs with all joint data */
/* the actual array may be in shared memory or in kernel space, as
determined by the init code in motion.c */
extern emcmot_joint_t joints[EMCMOT_MAX_JOINTS];
/* Variable defs */
extern KINEMATICS_FORWARD_FLAGS fflags;
extern KINEMATICS_INVERSE_FLAGS iflags;
/* these variable have the 1/servo cycle time */
/* Struct pointers */
extern struct emcmot_struct_t *emcmotStruct;
extern struct emcmot_command_t *emcmotCommand;
extern struct emcmot_status_t *emcmotStatus;
extern struct emcmot_config_t *emcmotConfig;
extern struct emcmot_internal_t *emcmotInternal;
extern struct emcmot_error_t *emcmotError;
/***********************************************************************
* PUBLIC FUNCTION PROTOTYPES *
************************************************************************/
/* function definitions */
extern void emcmotCommandHandler(void *arg, long period);
extern void emcmotController(void *arg, long period);
extern void emcmotSetCycleTime(unsigned long nsec);
/* these are related to synchronized I/O */
extern void emcmotDioWrite(int index, char value);
extern void emcmotAioWrite(int index, double value);
extern void emcmotSetRotaryUnlock(int axis, int unlock);
extern int emcmotGetRotaryIsUnlocked(int axis);
//
// Try to change the Motion mode to Teleop.
//
// This function can be called at any time. Returns without changing
// mode if Teleop is not currently allowed. This code doesn't actually
// make the transition, it just sets a flag requesting the transition.
// The real transition to Teleop mode is done in emcmotController().
//
void switch_to_teleop_mode(void);
/* recalculates jog limits */
extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num);
/* handles 'homed' flags, see command.c for details */
extern void clearHomes(int joint_num);
extern void emcmot_config_change(void);
extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */
int joint_is_lockable(int joint_num);
#define ALL_JOINTS emcmotConfig->numJoints
// number of kinematics-only joints:
#define NO_OF_KINS_JOINTS (ALL_JOINTS - emcmotConfig->numExtraJoints)
#define IS_EXTRA_JOINT(jno) (jno >= NO_OF_KINS_JOINTS)
// 0-based Joint numbering:
// kinematic-only jno.s: [0 ... (NO_OF_KINS_JOINTS -1) ]
// extrajoint jno.s: [NO_OF_KINS_JOINTS ... (ALL_JOINTS -1) ]
/* rtapi_get_time() returns a nanosecond value. In time, we should use a u64
value for all calcs and only do the conversion to seconds when it is
really needed. */
#define etime() (((double) rtapi_get_time()) / 1.0e9)
/* macros for reading, writing bit flags */
/* motion flags */
#define GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0)
#define SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;
#define GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0)
#define SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT;
#define GET_MOTION_TELEOP_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0)
#define SET_MOTION_TELEOP_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_TELEOP_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_TELEOP_BIT;
#define GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0)
#define SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;
#define GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0)
#define SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;
#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type)
#define SET_TRAK_PLANNER_TYPE(tp) (emcmotStatus->planner_type = tp)
/* joint flags */
#define GET_JOINT_ENABLE_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0)
#define SET_JOINT_ENABLE_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ENABLE_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ENABLE_BIT;
#define SET_JOINT_ACTIVE_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ACTIVE_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ACTIVE_BIT;
#define SET_JOINT_INPOS_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_INPOS_BIT; else (joint)->flag &= ~EMCMOT_JOINT_INPOS_BIT;
#define GET_JOINT_ERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ERROR_BIT ? 1 : 0)
#define SET_JOINT_ERROR_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ERROR_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ERROR_BIT;
#define GET_JOINT_PHL_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_MAX_HARD_LIMIT_BIT ? 1 : 0)
#define SET_JOINT_PHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MAX_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MAX_HARD_LIMIT_BIT;
#define GET_JOINT_NHL_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_MIN_HARD_LIMIT_BIT ? 1 : 0)
#define SET_JOINT_NHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MIN_HARD_LIMIT_BIT;
#define GET_JOINT_FERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FERROR_BIT ? 1 : 0)
#define SET_JOINT_FERROR_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_FERROR_BIT; else (joint)->flag &= ~EMCMOT_JOINT_FERROR_BIT;
#define GET_JOINT_FAULT_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FAULT_BIT ? 1 : 0)
#define SET_JOINT_FAULT_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_FAULT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_FAULT_BIT;
#if defined(__KERNEL__)
#define HAVE_CPU_KHZ
#endif
#endif /* MOT_PRIV_H */

780
wasm-port/vendor/linuxcnc/src/emc/motion/motion.h vendored Normal file
View File

@@ -0,0 +1,780 @@
/********************************************************************
* Description: motion.h
* Data structures used throughout emc2.
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved
********************************************************************/
/* jmk says: This file is a mess! */
/*
Misc ramblings:
The terms axis and joint are used inconsistently throughout EMC.
For all new code, the usages are as follows:
axis - one of the nine degrees of freedom, x, y, z, a, b, c, u, v, w
these refer to axes in Cartesian space, which may or
may not match up with joints (see below). On Cartesian
machines they do match up, but for hexapods, robots, and
other non-Cartesian machines they don't.
joint - one of the physical degrees of freedom of the machine
these might be linear (leadscrews) or rotary (rotary
tables, robot arm joints). There can be any number of
joints. The kinematics code is responsible for translating
from axis space to joint space and back.
There are three main kinds of data needed by the motion controller
1) data shared with higher level stuff - commands, status, etc.
2) data that is local to the motion controller
3) data shared with lower level stuff - hal pins
In addition, some internal data (2) should be shared for trouble
shooting purposes, even though it is "internal" to the motion
controller. Depending on the type of data, it can either be
treated as type (1), and made available to the higher level
code, or it can be treated as type (3), and made available to
the hal, so that halscope can monitor it.
This file should ONLY contain structures and declarations for
type (1) items - those that are shared with higher level code.
Type (2) items should be declared in mot_priv.h, along
with type (3) items.
In the interest of retaining my sanity, I'm not gonna attempt
to move everything to its proper location yet....
However, all new items will be defined in the proper place,
and some existing items may be moved from one struct definition
to another.
*/
#ifndef MOTION_H
#define MOTION_H
#include <rtapi_stdint.h>
#include <stdarg.h>
#include <rtapi_bool.h>
#include <rtapi_limits.h>
#include <posemath.h> /* PmCartesian, PmPose, pmCartMag() */
#include <emcpos.h> /* EmcPose */
#include "../kinematics/cubic.h" /* CUBIC_STRUCT, CUBIC_COEFF */
#include <emcmotcfg.h> /* EMCMOT_MAX_JOINTS */
#include <kinematics.h>
#include "simple_tp.h"
#include "state_tag.h"
#include "../tp/tp_types.h"
// define a special value to denote an invalid motion ID
// NB: do not ever generate a motion id of MOTION_INVALID_ID
// this should be really be tested for in command.c
#define MOTION_INVALID_ID INT_MIN
#define MOTION_ID_VALID(x) ((x) != MOTION_INVALID_ID)
#include <rtapi.h> /* must precede rtapi_atomic.h in kernel mode */
#include <rtapi_atomic.h>
#ifdef __cplusplus
extern "C" {
#endif
/* This enum lists all the possible commands */
typedef enum {
EMCMOT_ABORT = 1, /* abort all motion */
EMCMOT_ENABLE, /* enable servos for active joints */
EMCMOT_DISABLE, /* disable servos for active joints */
EMCMOT_PAUSE, /* pause motion */
EMCMOT_REVERSE, /* run reverse motion */
EMCMOT_FORWARD, /* run reverse motion */
EMCMOT_RESUME, /* resume motion */
EMCMOT_STEP, /* resume motion until id encountered */
EMCMOT_FREE, /* set mode to free (joint) motion */
EMCMOT_COORD, /* set mode to coordinated motion */
EMCMOT_TELEOP, /* set mode to teleop */
EMCMOT_SPINDLE_SCALE, /* set scale factor for spindle speed */
EMCMOT_SS_ENABLE, /* enable/disable scaling the spindle speed */
EMCMOT_FEED_SCALE, /* set scale factor for feedrate */
EMCMOT_RAPID_SCALE, /* set scale factor for rapids */
EMCMOT_FS_ENABLE, /* enable/disable scaling feedrate */
EMCMOT_FH_ENABLE, /* enable/disable feed_hold */
EMCMOT_AF_ENABLE, /* enable/disable adaptive feedrate */
EMCMOT_OVERRIDE_LIMITS, /* temporarily ignore limits until jog done */
EMCMOT_SET_LINE, /* queue up a linear move */
EMCMOT_SET_CIRCLE, /* queue up a circular move */
EMCMOT_SET_TELEOP_VECTOR, /* Move at a given velocity but in
world cartesian coordinates, not
in joint space like EMCMOT_JOG_* */
EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */
EMCMOT_PROBE, /* go to pos, stop if probe trips, record
trip pos */
EMCMOT_RIGID_TAP, /* go to pos, with sync to spindle speed,
then return to initial pos */
EMCMOT_SET_VEL, /* set the velocity for subsequent moves */
EMCMOT_SET_VEL_LIMIT, /* set the max vel for all moves (tooltip) */
EMCMOT_SET_ACC, /* set the max accel for moves (tooltip) */
EMCMOT_SET_JERK, /* set the max jerk for moves (tooltip) */
EMCMOT_SET_PLANNER_TYPE, /* set planner type (0=trapezoidal, 1=S-curve) */
EMCMOT_SET_TERM_COND, /* set termination condition (stop, blend) */
EMCMOT_SET_NUM_JOINTS, /* set the number of joints */
EMCMOT_SET_NUM_SPINDLES, /* set the number of spindles */
EMCMOT_SET_WORLD_HOME, /* set pose for world home */
EMCMOT_SET_DEBUG, /* sets the debug level */
EMCMOT_SET_DOUT, /* sets or unsets a DIO, this can be immediate or synched with motion */
EMCMOT_SET_AOUT, /* sets or unsets a AIO, this can be immediate or synched with motion */
EMCMOT_SET_SPINDLESYNC, /* synchronize motion to spindle encoder */
EMCMOT_SPINDLE_ON, /* start the spindle */
EMCMOT_SPINDLE_OFF, /* stop the spindle */
EMCMOT_SPINDLE_INCREASE, /* spindle faster */
EMCMOT_SPINDLE_DECREASE, /* spindle slower */
EMCMOT_SPINDLE_BRAKE_ENGAGE, /* engage the spindle brake */
EMCMOT_SPINDLE_BRAKE_RELEASE, /* release the spindle brake */
EMCMOT_SPINDLE_ORIENT, /* orient the spindle */
EMCMOT_SET_OFFSET, /* set tool offsets */
EMCMOT_SET_MAX_FEED_OVERRIDE,
EMCMOT_SETUP_ARC_BLENDS,
EMCMOT_SET_PROBE_ERR_INHIBIT,
EMCMOT_ENABLE_WATCHDOG, /* enable watchdog sound, parport */
EMCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */
EMCMOT_JOG_CONT, /* continuous jog */
EMCMOT_JOG_INCR, /* incremental jog */
EMCMOT_JOG_ABS, /* absolute jog */
EMCMOT_JOG_ABORT, /* abort one joint num or axis num */
EMCMOT_JOINT_ACTIVATE, /* make joint active */
EMCMOT_JOINT_DEACTIVATE, /* make joint inactive */
EMCMOT_JOINT_HOME, /* home a joint or all joints */
EMCMOT_JOINT_UNHOME, /* unhome a joint or all joints*/
EMCMOT_SET_JOINT_POSITION_LIMITS, /* set the joint position +/- limits */
EMCMOT_SET_JOINT_BACKLASH, /* set the joint backlash */
EMCMOT_SET_JOINT_MIN_FERROR, /* minimum following error, input units */
EMCMOT_SET_JOINT_MAX_FERROR, /* maximum following error, input units */
EMCMOT_SET_JOINT_VEL_LIMIT, /* set the max joint vel */
EMCMOT_SET_JOINT_ACC_LIMIT, /* set the max joint accel */
EMCMOT_SET_JOINT_HOMING_PARAMS, /* sets joint homing parameters */
EMCMOT_SET_JOINT_JERK_LIMIT, /* set the max joint jerk */
EMCMOT_UPDATE_JOINT_HOMING_PARAMS, /* updates some joint homing parameters */
EMCMOT_SET_JOINT_MOTOR_OFFSET, /* set the offset between joint and motor */
EMCMOT_SET_JOINT_COMP, /* set a compensation triplet for a joint (nominal, forw., rev.) */
EMCMOT_SET_AXIS_POSITION_LIMITS, /* set the axis position +/- limits */
EMCMOT_SET_AXIS_VEL_LIMIT, /* set the max axis vel */
EMCMOT_SET_AXIS_ACC_LIMIT, /* set the max axis acc */
EMCMOT_SET_AXIS_LOCKING_JOINT, /* set the axis locking joint */
EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */
EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */
} cmd_code_t;
/* this enum lists the possible results of a command */
typedef enum {
EMCMOT_COMMAND_OK = 0, /* cmd honored */
EMCMOT_COMMAND_UNKNOWN_COMMAND, /* cmd not understood */
EMCMOT_COMMAND_INVALID_COMMAND, /* cmd can't be handled now */
EMCMOT_COMMAND_INVALID_PARAMS, /* bad cmd params */
EMCMOT_COMMAND_BAD_EXEC /* error trying to initiate */
} cmd_status_t;
/* termination conditions for queued motions */
#define EMCMOT_TERM_COND_STOP 1
#define EMCMOT_TERM_COND_BLEND 2
#define EMCMOT_TERM_COND_TANGENT 3
/*********************************
COMMAND STRUCTURE
*********************************/
/* This is the command structure. There is one of these in shared
memory, and all commands from higher level code come thru it.
*/
typedef struct emcmot_command_t {
cmd_code_t command; /* command code (enum) */
int commandNum; /* increment this for new command */
double motor_offset; /* offset from joint to motor position */
double maxLimit; /* pos value for position limit, output */
double minLimit; /* neg value for position limit, output */
double min_pos_speed; /* spindle minimum positive speed */
double max_neg_speed; /* spindle maximum negative speed */
EmcPose pos; /* line/circle endpt, or teleop vector */
PmCartesian center; /* center for circle */
PmCartesian normal; /* normal vec for circle */
int turn; /* turns for circle or joint number for a locking indexer*/
double vel; /* max velocity */
double ini_maxvel; /* max velocity allowed by machine
constraints (the INI file) */
int motion_type; /* this move is because of traverse, feed, arc, or toolchange */
double spindlesync; /* user units per spindle revolution, 0 = no sync */
double acc; /* max acceleration */
double jerk; /* jerk for traj */
double ini_maxjerk;
int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */
double backlash; /* amount of backlash */
int id; /* id for motion */
int termCond; /* termination condition */
double tolerance; /* tolerance for path deviation in CONTINUOUS mode */
int joint; /* which joint index to use for below */
int axis; /* which axis index to use for below */
int spindle; /* which spindle to use */
double scale; /* velocity scale or spindle_speed scale arg */
double offset; /* input, output, or home offset arg */
double home; /* joint home position */
double home_final_vel; /* joint velocity for moving from OFFSET to HOME */
double search_vel; /* home search velocity */
double latch_vel; /* home latch velocity */
int flags; /* homing config flags, other boolean args */
int home_sequence; /* order in homing sequence */
int volatile_home; /* joint should get unhomed when we get unhome -2
(generated by task upon estop, etc) */
double minFerror; /* min following error */
double maxFerror; /* max following error */
int wdWait; /* cycle to wait before toggling wd */
int debug; /* debug level, from DEBUG in INI file */
unsigned char now, out, start, end; /* these are related to synched AOUT/DOUT. now=whether now or synched, out = which gets set, start=start value, end=end value */
unsigned char mode; /* used for turning overrides etc. on/off */
double comp_nominal, comp_forward, comp_reverse; /* compensation triplet, nominal, forward, reverse */
unsigned char probe_type; /* ~1 = error if probe operation is unsuccessful (ngc default)
|1 = suppress error, report in # instead
~2 = move until probe trips (ngc default)
|2 = move until probe clears */
int probe_jog_err_inhibit; // setting to inhibit probe tripped while jogging error.
int probe_home_err_inhibit; // setting to inhibit probe tripped while homeing error.
EmcPose tool_offset; /* TLO */
double orientation; /* angle for spindle orient */
int state; /*spindle state seems to just be 0 for off and 1 for on andypugh 2025-04-03*/
char direction; /* CANON_DIRECTION flag for spindle orient */
double timeout; /* of wait for spindle orient to complete */
unsigned char wait_for_spindle_at_speed; // EMCMOT_SPINDLE_ON now carries this, for next feed move
int arcBlendOptDepth;
int arcBlendEnable;
int arcBlendFallbackEnable;
int arcBlendGapCycles;
double arcBlendRampFreq;
double arcBlendTangentKinkRatio;
double maxFeedScale;
double ext_offset_vel; /* velocity for an external axis offset */
double ext_offset_acc; /* acceleration for an external axis offset */
struct state_tag_t tag;
} emcmot_command_t;
/*! \todo FIXME - these packed bits might be replaced with chars
memory is cheap, and being able to access them without those
damn macros would be nice
*/
/* motion flag type */
typedef unsigned short EMCMOT_MOTION_FLAG;
/*
motion status flag structure-- looks like:
MSB LSB
v---------------v------------------v
| | | | T | CE | C | IP | EN |
^---------------^------------------^
where:
EN is 1 if calculations are enabled, 0 if not
IP is 1 if all joints in position, 0 if not
C is 1 if coordinated mode, 0 if in free mode
CE is 1 if coordinated mode error, 0 if not
T is 1 if we are in teleop mode.
*/
/* bit masks */
#define EMCMOT_MOTION_ENABLE_BIT 0x0001
#define EMCMOT_MOTION_INPOS_BIT 0x0002
#define EMCMOT_MOTION_COORD_BIT 0x0004
#define EMCMOT_MOTION_ERROR_BIT 0x0008
#define EMCMOT_MOTION_TELEOP_BIT 0x0010
/* joint flag type */
typedef unsigned short EMCMOT_JOINT_FLAG;
/*
joint status flag structure-- looks like:
MSB LSB
----------v-----------------v--------------------v-------------------v
| AF | FE | AH | HD | H | HS | NHL | PHL | - | - | ER | IP | AC | EN |
----------^-----------------^--------------------^-------------------^
x = unused
where:
EN is 1 if joint amplifier is enabled, 0 if not
AC is 1 if joint is active for calculations, 0 if not
IP is 1 if joint is in position, 0 if not (free mode only)
ER is 1 if joint has an error, 0 if not
PHL is 1 if joint is on maximum hardware limit, 0 if not
NHL is 1 if joint is on minimum hardware limit, 0 if not
HS is 1 if joint home switch is tripped, 0 if not
H is 1 if joint is homing, 0 if not
HD is 1 if joint has been homed, 0 if not
AH is 1 if joint is at home position, 0 if not
FE is 1 if joint exceeded following error, 0 if not
AF is 1 if amplifier is faulted, 0 if not
Suggestion: Split this in to an Error and a Status flag register..
Then a simple test on each of the two flags can be performed
rather than testing each bit... Saving on a global per joint
fault and ready status flag.
*/
/* bit masks */
#define EMCMOT_JOINT_ENABLE_BIT 0x0001
#define EMCMOT_JOINT_ACTIVE_BIT 0x0002
#define EMCMOT_JOINT_INPOS_BIT 0x0004
#define EMCMOT_JOINT_ERROR_BIT 0x0008
#define EMCMOT_JOINT_MAX_HARD_LIMIT_BIT 0x0010
#define EMCMOT_JOINT_MIN_HARD_LIMIT_BIT 0x0020
#define EMCMOT_JOINT_FERROR_BIT 0x0040
#define EMCMOT_JOINT_FAULT_BIT 0x0080
/*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly
documented. This is my feeble attempt to understand exactly
what they mean.
According to Fred, teleop is never used with machine tools,
although that may not be true for machines with non-trivial
kinematics.
"coord", or coordinated mode, means that all the joints are
synchronized, and move together as commanded by the higher
level code. It is the normal mode when machining. In
coordinated mode, commands are assumed to be in the cartesean
reference frame, and if the machine is non-cartesean, the
commands are translated by the kinematics to drive each
joint in joint space as needed.
"free" mode means commands are interpreted in joint space.
It is used for jogging individual joints, although
it does not preclude multiple joints moving at once (I think).
Homing is also done in free mode, in fact machines with
non-trivial kinematics must be homed before they can go
into either coord or teleop mode.
'teleop' is what you probably want if you are 'jogging'
a hexapod. The jog commands as implemented by the motion
controller are joint jogs, which work in free mode. But
if you want to jog a hexapod or similar machine along
one particular cartesean axis, you need to operate more
than one joint. That's what 'teleop' is for.
*/
/* compensation structures */
typedef struct {
double nominal; /* nominal (command) position */
float fwd_trim; /* correction for forward movement */
float rev_trim; /* correction for reverse movement */
float fwd_slope; /* slopes between here and next pt */
float rev_slope;
} emcmot_comp_entry_t;
#define EMCMOT_COMP_SIZE 256
typedef struct {
int entries; /* number of entries in the array */
emcmot_comp_entry_t *entry; /* current entry in array */
emcmot_comp_entry_t array[EMCMOT_COMP_SIZE+2];
/* +2 because array has -HUGE_VAL and +HUGE_VAL entries at the ends */
} emcmot_comp_t;
/* motion controller states */
typedef enum {
EMCMOT_MOTION_DISABLED = 0,
EMCMOT_MOTION_FREE,
EMCMOT_MOTION_TELEOP,
EMCMOT_MOTION_COORD
} motion_state_t;
typedef enum {
EMCMOT_ORIENT_NONE = 0,
EMCMOT_ORIENT_COMPLETE,
EMCMOT_ORIENT_IN_PROGRESS,
EMCMOT_ORIENT_FAULTED,
} orient_state_t;
/* flags for enabling spindle scaling, feed scaling,
adaptive feed, and feed hold */
#define SS_ENABLED 0x01
#define FS_ENABLED 0x02
#define AF_ENABLED 0x04
#define FH_ENABLED 0x08
/* This structure contains all of the data associated with
a single joint. Note that this structure does not need
to be in shared memory (but it can, if desired for debugging
reasons). The portions of this structure that are considered
"status" and need to be made available to user space are
copied to a much smaller struct called emcmot_joint_status_t
which is located in shared memory.
*/
typedef struct {
/* configuration info - changes rarely */
int type; /* 0 = linear, 1 = rotary */
double max_pos_limit; /* upper soft limit on joint pos */
double min_pos_limit; /* lower soft limit on joint pos */
double max_jog_limit; /* jog limits change when not homed */
double min_jog_limit;
double vel_limit; /* upper limit of joint speed */
double acc_limit; /* upper limit of joint accel */
double jerk_limit; /* upper limit of joint jerk */
double min_ferror; /* zero speed following error limit */
double max_ferror; /* max speed following error limit */
double backlash; /* amount of backlash */
emcmot_comp_t comp; /* leadscrew correction data */
/* status info - changes regularly */
/* many of these need to be made available to higher levels */
/* they can either be copied to the status struct, or an array of
joint structs can be made part of the status */
EMCMOT_JOINT_FLAG flag; /* see above for bit details */
double coarse_pos; /* trajectory point, before interp */
double pos_cmd; /* commanded joint position */
double vel_cmd; /* commanded joint velocity */
double acc_cmd; /* commanded joint acceleration */
double jerk_cmd; /* comanded joint jerk */
double backlash_corr; /* correction for backlash */
double backlash_filt; /* filtered backlash correction */
double backlash_vel; /* backlash velocity variable */
double motor_pos_cmd; /* commanded position, with comp */
double motor_pos_fb; /* position feedback, with comp */
double pos_fb; /* position feedback, comp removed */
double ferror; /* following error */
double ferror_limit; /* limit depends on speed */
double ferror_high_mark; /* max following error */
simple_tp_t free_tp; /* planner for free mode motion */
int kb_jjog_active; /* non-zero during a keyboard jog */
int wheel_jjog_active; /* non-zero during a wheel jog */
/* internal info - changes regularly, not usually accessed from user
space */
CUBIC_STRUCT cubic; /* cubic interpolator data */
int on_pos_limit; /* non-zero if on limit */
int on_neg_limit; /* non-zero if on limit */
double motor_offset; /* diff between internal and motor pos, used
to set position to zero during homing */
int old_jjog_counts; /* prior value, used for deltas */
double big_vel; /* used for "debouncing" velocity */
} emcmot_joint_t;
/* This structure contains only the "status" data associated with
a joint. "Status" data is that data that should be reported to
user space on a continuous basis. An array of these structs is
part of the main status structure, and is filled in with data
copied from the emcmot_joint_t structs every servo period.
For now this struct contains more data than it really needs, but
paring it down will take time (and probably needs to be done one
or two items at a time, with much testing). My main goal right
now is to get get the large joint struct out of status.
*/
typedef struct {
EMCMOT_JOINT_FLAG flag; /* see above for bit details */
bool homed;
bool homing;
double pos_cmd; /* commanded joint position */
double pos_fb; /* position feedback, comp removed */
double vel_cmd; /* current velocity */
double acc_cmd; /* current acceleration */
double ferror; /* following error */
double ferror_high_mark; /* max following error */
/*! \todo FIXME - the following are not really "status", but taskintf.cc expects
them to be in the status structure. I don't know how or if they are
used by the user space code. Ideally they will be removed from here,
but each one will need to be investigated individually.
*/
double backlash; /* amount of backlash */
double max_pos_limit; /* upper soft limit on joint pos */
double min_pos_limit; /* lower soft limit on joint pos */
double min_ferror; /* zero speed following error limit */
double max_ferror; /* max speed following error limit */
} emcmot_joint_status_t;
typedef struct {
double speed; // spindle speed in RPMs
double scale; // spindle override value
double net_scale; // scale or zero if inhibited
double css_factor;
double xoffset;
int state;
int direction; // 0 stopped, 1 forward, -1 reverse
int brake; // 0 released, 1 engaged
int locked; // spindle lock engaged after orient
int orient_fault; // fault code from motion.spindle-orient-fault
int orient_state; // orient_state_t
int spindle_index_enable; /* hooked to a canon encoder index-enable */
double spindleRevs; /* position of spindle in revolutions */
double spindleSpeedIn; /* velocity of spindle in revolutions per minute */
int at_speed;
int fault; /* amplifier fault */
double max_pos_speed; /* spindle speed limits */
double min_pos_speed; /* signed values, so max_neg = 0 */
double max_neg_speed; /* and min_neg = -1e99 indicates no limit */
double min_neg_speed;
double home_angle;
double home_search_vel;
int home_sequence;
double increment;
} spindle_status_t;
typedef struct {
double teleop_vel_cmd; /* commanded axis velocity */
double max_pos_limit; /* upper soft limit on axis pos */
double min_pos_limit; /* lower soft limit on axis pos */
} emcmot_axis_status_t;
/*********************************
STATUS STRUCTURE
*********************************/
/* This is the status structure. There is one of these in shared
memory, and it reports motion controller status to higher level
code in user space. For the most part, this structure contains
higher level variables - low level stuff is made visible to the
HAL and troubleshooting, etc, is done using the HAL oscilloscope.
*/
/*! \todo FIXME - this struct is broken into two parts... at the top are
structure members that I understand, and that are needed for emc2.
Other structure members follow. All the later ones need to be
evaluated - either they move up, or they go away.
*/
typedef struct emcmot_status_t {
unsigned char head; /* flag count for mutex detect */
/* these three are updated only when a new command is handled */
cmd_code_t commandEcho; /* echo of input command */
int commandNumEcho; /* echo of input command number */
cmd_status_t commandStatus; /* result of most recent command */
/* these are config info, updated when a command changes them */
double feed_scale; /* velocity scale factor for all motion but rapids */
double rapid_scale; /* velocity scale factor for rapids */
unsigned char enables_new; /* flags for FS, SS, etc */
/* the above set is the enables in effect for new moves */
/* the rest are updated every cycle */
double net_feed_scale; /* net scale factor for all motion */
unsigned char enables_queued; /* flags for FS, SS, etc */
/* the above set is the enables in effect for the
currently executing move */
motion_state_t motion_state; /* operating state: FREE, COORD, etc. */
EMCMOT_MOTION_FLAG motionFlag; /* see above for bit details */
EmcPose carte_pos_cmd; /* commanded Cartesian position */
int carte_pos_cmd_ok; /* non-zero if command is valid */
EmcPose carte_pos_fb; /* actual Cartesian position */
int carte_pos_fb_ok; /* non-zero if feedback is valid */
EmcPose world_home; /* cartesean coords of home position */
emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */
emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */
int spindleSync; /* spindle used for synchronised moves. -1 = none */
spindle_status_t spindle_status[EMCMOT_MAX_SPINDLES]; /* all spindle data */
int on_soft_limit; /* non-zero if any joint is on soft limit */
int probeVal; /* debounced value of probe input */
int probeTripped; /* Has the probe signal changed since start
of probe command? */
int probing; /* Currently looking for a probe signal? */
unsigned char probe_type;
EmcPose probedPos; /* Axis positions stored as soon as possible
after last probeTripped */
int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by G-code */
int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by G-code */
double analog_input[EMCMOT_MAX_AIO]; /* inputs to the motion controller, queried by G-code */
double analog_output[EMCMOT_MAX_AIO]; /* outputs to the motion controller, queried by G-code */
int misc_error[EMCMOT_MAX_MISC_ERROR]; /* Random Error pins*/
struct state_tag_t tag; /* Current interp state corresponding
to motion line */
/*! \todo FIXME - all structure members beyond this point are in limbo */
/* dynamic status-- changes every cycle */
uint64_t heartbeat; /* Incremented every time the motion controller is done. */
int config_num; /* incremented whenever configuration
changed. */
int id; /* id for executing motion */
int depth; /* motion queue depth */
int activeDepth; /* depth of active blend elements */
int queueFull; /* Flag to indicate the tc queue is full */
int paused; /* Flag to signal motion paused */
int overrideLimitMask; /* non-zero means one or more limits ignored */
/* 1 << (joint-num*2) = ignore neg limit */
/* 2 << (joint-num*2) = ignore pos limit */
int reverse_run;
/* static status-- only changes upon input commands, e.g., config */
double vel; /* scalar max vel */
double acc; /* scalar max accel */
double jerk; /* jerk for traj */
int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */
int motionType;
double distance_to_go; /* in this move */
EmcPose dtg;
double current_vel;
double requested_vel;
/* S-curve motion state - for accurate jerk output */
double current_acc; /* current path acceleration */
double current_jerk; /* current path jerk (accurate value from TP) */
double decel_dist; /* S-curve deceleration distance (dlen1) for debugging */
PmCartesian current_dir; /* current motion direction unit vector */
unsigned int tcqlen;
EmcPose tool_offset;
int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */
unsigned char tail; /* flag count for mutex detect */
int external_offsets_applied;
EmcPose eoffset_pose;
int numExtraJoints;
int stepping;
bool jogging_active;
} emcmot_status_t;
/*********************************
CONFIG STRUCTURE
*********************************/
/* This is the config structure. This is currently in shared memory,
but I have no idea why... there are commands to set most of the
items in this structure. It seems we should either put the struct
in private memory and manipulate it with commands, or we should
put it in shared memory and manipulate it directly - not both.
The structure contains static or rarely changed information that
describes the machine configuration.
later: I think I get it now - the struct is in shared memory so
user space can read the config at any time, but commands are used
to change the config so they only take effect when the realtime
code processes the command.
*/
/*! \todo FIXME - this struct is broken into two parts... at the top are
structure members that I understand, and that are needed for emc2.
Other structure members follow. All the later ones need to be
evaluated - either they move up, or they go away.
*/
typedef struct emcmot_config_t {
unsigned char head; /* flag count for mutex detect */
int config_num; /* Incremented everytime configuration
changed, should match status.config_num */
int numJoints; /* The number of total joints in the system (which
must be between 1 and EMCMOT_MAX_JOINTS,
inclusive). includes extra joints*/
int numExtraJoints; /* The number of extra joints in the system (which
must be between 1 and EMCMOT_MAX_EXTRAJOINTS,
inclusive). */
int numSpindles; /* The number of spindles, 1 to EMCMOT_MAX_SPINDLES */
KINEMATICS_TYPE kinType;
int numDIO; /* userdefined number of digital IO. default is 4. (EMCMOT_MAX_DIO=64),
but can be altered at motmod insmod time */
int numAIO; /* userdefined number of analog IO. default is 4. (EMCMOT_MAX_AIO=16),
but can be altered at motmod insmod time */
int numMiscError; /* userdefined number of Misc Errors. default is 0.
but can be altered at motmod insmod time */
/*! \todo FIXME - all structure members beyond this point are in limbo */
double trajCycleTime; /* the rate at which the trajectory loop
runs.... (maybe) */
double servoCycleTime; /* the rate of the servo loop - Not the same
as the traj time */
int interpolationRate; /* grep control.c for an explanation....
approx line 50 */
double limitVel; /* scalar upper limit on vel */
int debug; /* copy of DEBUG, from INI file */
unsigned char tail; /* flag count for mutex detect */
int arcBlendOptDepth;
int arcBlendEnable;
int arcBlendFallbackEnable;
int arcBlendGapCycles;
double arcBlendRampFreq;
double arcBlendTangentKinkRatio;
double maxFeedScale;
int inhibit_probe_jog_error;
int inhibit_probe_home_error;
} emcmot_config_t;
/* error structure - lockfree MPSC ring buffer. See emcmotutil.c. */
typedef struct emcmot_error_t {
char error[EMCMOT_ERROR_NUM][EMCMOT_ERROR_LEN];
rtapi_atomic_ullong write_reserve;
rtapi_atomic_ullong write_commit;
rtapi_atomic_ullong read_seq;
} emcmot_error_t;
typedef struct emcmot_internal_t {
unsigned char head; /* flag count for mutex detect */
unsigned char tail; /* flag count for mutex detect */
int split; /* number of split command reads */
int enabling; /* starts up disabled */
int coordinating; /* starts up in free mode */
int teleoperating; /* starts up in free mode */
int overriding; /* non-zero means we've initiated an joint
move while overriding limits */
TP_STRUCT coord_tp; /* coordinated mode planner */
int idForStep; /* status id while stepping */
} emcmot_internal_t;
/* error ring buffer access functions */
extern int emcmotErrorInit(emcmot_error_t * errlog);
extern int emcmotErrorPut(emcmot_error_t * errlog, const char *error);
extern int emcmotErrorPutfv(emcmot_error_t * errlog, const char *fmt, va_list ap);
extern int emcmotErrorPutf(emcmot_error_t * errlog, const char *fmt, ...);
extern int emcmotErrorGet(emcmot_error_t * errlog, char *error);
#define GET_JOINT_ACTIVE_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ACTIVE_BIT ? 1 : 0)
#define GET_JOINT_INPOS_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_INPOS_BIT ? 1 : 0)
#ifdef __cplusplus
}
#endif
#endif /* MOTION_H */

107
wasm-port/vendor/linuxcnc/src/emc/motion/simple_tp.h vendored Normal file
View File

@@ -0,0 +1,107 @@
/********************************************************************
* Description: simple_tp.h
* A simple, single axis trajectory planner
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved
********************************************************************/
/* simple_tp.c and simple_tp.h define a simple, single axis trajectory
planner. It is based on the "free mode trajectory planner" that was
originally written as part of EMC2's control.c, but the code has
been pulled out of control.c and given a somewhat object oriented
API to allow it to be used for both teleop and free mode.
*/
#ifndef SIMPLE_TP_H
#define SIMPLE_TP_H
// stopping criterion:
#define TINY_DP(max_acc,period) (max_acc*period*period*0.001)
#ifdef __cplusplus
extern "C" {
#endif
typedef struct simple_tp_t {
double pos_cmd; /* position command */
double max_vel; /* velocity limit */
double max_acc; /* acceleration limit */
double max_jerk; /* jerk limit */
int enable; /* if zero, motion stops ASAP */
double curr_pos; /* current position */
double curr_vel; /* current velocity */
int active; /* non-zero if motion in progress */
double curr_acc; /* current acceleration */
double curr_jerk; /* current acceleration */
double last_move_length; /* current acceleration */
double last_pos_cmd;
int use_trapezoid;
double curr_max_vel;
int total_n;
int curr_n;
int n0;
int n1;
int n2;
int n3;
int n4;
int n5;
int n6;
int fix_verr;
double verr;
double vc;
double ve;
double vm;
double jm;
double j2;
double j4;
double v1;
double v2;
double v3;
double v5;
double v6;
double v7;
double a1;
double a2;
double a3;
double a5;
double a6;
double a7;
double prograss;
int status;
} simple_tp_t;
/* I could write a bunch of functions to read and write the first four
structure members, and to read the last three, but that seems silly.
*/
/* The update() function does all the work. If 'enable' is true, it
computes a new value of 'curr_pos', which moves toward 'pos_cmd'
while obeying the 'max_vel' and 'max_accel' limits. It also sets
'active' if movement is in progress, and clears it when motion
stops at the commanded position. The command or either of the
limits can be changed at any time. If 'enable' is false, it
ramps the velocity to zero, then clears 'active' and sets
'pos_cmd' to match 'curr_pos', to avoid motion the next time it
is enabled. 'period' is the period between calls, in seconds.
*/
extern void simple_tp_update(simple_tp_t *tp, double period);
extern void simple_tp_update_normal(simple_tp_t *tp, double period);
extern void simple_scurve_tp_update(simple_tp_t *tp, double period);
#ifdef __cplusplus
}
#endif
#endif /* SIMPLE_TP_H */

304
wasm-port/vendor/linuxcnc/src/emc/nml_intf/emcpose.c vendored Normal file
View File

@@ -0,0 +1,304 @@
/********************************************************************
* Description: emcpose.c
*
* Miscellaneous functions to handle EmcPose operations
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2014 All rights reserved.
*
********************************************************************/
#include "emcpose.h"
#include <posemath.h>
#include <rtapi_math.h>
//#define EMCPOSE_PEDANTIC
void emcPoseZero(EmcPose * const pos) {
#ifdef EMCPOSE_PEDANTIC
if(!pos) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pos->tran.x = 0.0;
pos->tran.y = 0.0;
pos->tran.z = 0.0;
pos->a = 0.0;
pos->b = 0.0;
pos->c = 0.0;
pos->u = 0.0;
pos->v = 0.0;
pos->w = 0.0;
}
int emcPoseAdd(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out)
{
#ifdef EMCPOSE_PEDANTIC
if (!p1 || !p2) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pmCartCartAdd(&p1->tran, &p2->tran, &out->tran);
out->a = p1->a + p2->a;
out->b = p1->b + p2->b;
out->c = p1->c + p2->c;
out->u = p1->u + p2->u;
out->v = p1->v + p2->v;
out->w = p1->w + p2->w;
return EMCPOSE_ERR_OK;
}
int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out)
{
#ifdef EMCPOSE_PEDANTIC
if (!p1 || !p2) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pmCartCartSub(&p1->tran, &p2->tran, &out->tran);
out->a = p1->a - p2->a;
out->b = p1->b - p2->b;
out->c = p1->c - p2->c;
out->u = p1->u - p2->u;
out->v = p1->v - p2->v;
out->w = p1->w - p2->w;
return EMCPOSE_ERR_OK;
}
int emcPoseSelfAdd(EmcPose * const self, EmcPose const * const p2)
{
return emcPoseAdd(self, p2, self);
}
int emcPoseSelfSub(EmcPose * const self, EmcPose const * const p2)
{
return emcPoseSub(self, p2, self);
}
int emcPoseToPmCartesian(EmcPose const * const pose,
PmCartesian * const xyz, PmCartesian * const abc, PmCartesian * const uvw)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_INPUT_MISSING;
}
if (!xyz | !abc || !uvw) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
#endif
//Direct copy of translation struct for xyz
*xyz = pose->tran;
//Convert ABCUVW axes into 2 pairs of 3D lines
abc->x = pose->a;
abc->y = pose->b;
abc->z = pose->c;
uvw->x = pose->u;
uvw->y = pose->v;
uvw->z = pose->w;
return EMCPOSE_ERR_OK;
}
/**
* Collect PmCartesian elements into 9D EmcPose structure.
*/
int pmCartesianToEmcPose(PmCartesian const * const xyz,
PmCartesian const * const abc, PmCartesian const * const uvw, EmcPose * const pose)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!xyz || !abc || !uvw) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
//Direct copy of translation struct for xyz
pose->tran = *xyz;
pose->a = abc->x;
pose->b = abc->y;
pose->c = abc->z;
pose->u = uvw->x;
pose->v = uvw->y;
pose->w = uvw->z;
return EMCPOSE_ERR_OK;
}
int emcPoseSetXYZ(PmCartesian const * const xyz, EmcPose * const pose)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!xyz) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pose->tran.x = xyz->x;
pose->tran.y = xyz->y;
pose->tran.z = xyz->z;
return EMCPOSE_ERR_OK;
}
int emcPoseSetABC(PmCartesian const * const abc, EmcPose * const pose)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!abc) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pose->a = abc->x;
pose->b = abc->y;
pose->c = abc->z;
return EMCPOSE_ERR_OK;
}
int emcPoseSetUVW(PmCartesian const * const uvw, EmcPose * const pose)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!uvw) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
pose->u = uvw->x;
pose->v = uvw->y;
pose->w = uvw->z;
return EMCPOSE_ERR_OK;
}
int emcPoseGetXYZ(EmcPose const * const pose, PmCartesian * const xyz)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!xyz) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
xyz->x = pose->tran.x;
xyz->y = pose->tran.y;
xyz->z = pose->tran.z;
return EMCPOSE_ERR_OK;
}
int emcPoseGetABC(EmcPose const * const pose, PmCartesian * const abc)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!abc) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
abc->x = pose->a;
abc->y = pose->b;
abc->z = pose->c;
return EMCPOSE_ERR_OK;
}
int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw)
{
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
if (!uvw) {
return EMCPOSE_ERR_INPUT_MISSING;
}
#endif
uvw->x = pose->u;
uvw->y = pose->v;
uvw->z = pose->w;
return EMCPOSE_ERR_OK;
}
/**
* Find the magnitude of an EmcPose position, treating it like a single vector.
*/
int emcPoseMagnitude(EmcPose const * const pose, double * const out) {
#ifdef EMCPOSE_PEDANTIC
if (!pose) {
return EMCPOSE_ERR_INPUT_MISSING;
}
if (!out) {
return EMCPOSE_ERR_OUTPUT_MISSING;
}
#endif
double mag = 0.0;
mag += pmSq(pose->tran.x);
mag += pmSq(pose->tran.y);
mag += pmSq(pose->tran.z);
mag += pmSq(pose->a);
mag += pmSq(pose->b);
mag += pmSq(pose->c);
mag += pmSq(pose->u);
mag += pmSq(pose->v);
mag += pmSq(pose->w);
mag = pmSqrt(mag);
*out = mag;
return EMCPOSE_ERR_OK;
}
/**
* Return true for a numerically valid pose, or false for an invalid pose (or null pointer).
*/
int emcPoseValid(EmcPose const * const pose)
{
if (!pose ||
isnan(pose->tran.x) ||
isnan(pose->tran.y) ||
isnan(pose->tran.z) ||
isnan(pose->a) ||
isnan(pose->b) ||
isnan(pose->c) ||
isnan(pose->u) ||
isnan(pose->v) ||
isnan(pose->w)) {
return 0;
} else {
return 1;
}
}

25
wasm-port/vendor/linuxcnc/src/emc/nml_intf/motion_types.h vendored Normal file
View File

@@ -0,0 +1,25 @@
#ifndef __LINUXCNC_MOTION_TYPES_H
#define __LINUXCNC_MOTION_TYPES_H
// Copyright 2008, Chris Radek <chris@timeguy.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#define EMC_MOTION_TYPE_TRAVERSE 1
#define EMC_MOTION_TYPE_FEED 2
#define EMC_MOTION_TYPE_ARC 3
#define EMC_MOTION_TYPE_TOOLCHANGE 4
#define EMC_MOTION_TYPE_PROBING 5
#define EMC_MOTION_TYPE_INDEXROTARY 6
#endif

1871
wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

281
wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.h vendored Normal file
View File

@@ -0,0 +1,281 @@
/********************************************************************
* Description: blendmath.h
* Circular arc blend math functions
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2014 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef BLENDMATH_H
#define BLENDMATH_H
#include <posemath.h>
#include "tc_types.h"
#include "sp_scurve.h"
#define BLEND_ACC_RATIO_TANGENTIAL 0.5
#define BLEND_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(BLEND_ACC_RATIO_TANGENTIAL)))
#define BLEND_KINK_FACTOR 0.25
typedef enum {
BLEND_NONE,
BLEND_LINE_LINE,
BLEND_LINE_ARC,
BLEND_ARC_LINE,
BLEND_ARC_ARC,
} blend_type_t;
/**
* 3D Input geometry for a spherical blend arc.
* This structure contains all of the basic geometry in 3D for a blend arc.
*/
typedef struct {
PmCartesian u1; /* unit vector along line 1 */
PmCartesian u2; /* unit vector along line 2 */
PmCartesian P; /* Intersection point */
PmCartesian normal; /* normal unit vector to plane containing lines */
PmCartesian binormal; /* binormal unit vector to plane containing lines */
PmCartesian u_tan1; /* Actual tangent vector to 1 (used for arcs only) */
PmCartesian u_tan2; /* Actual tangent vector to 2 (used for arcs only) */
PmCartesian center1; /* Local approximation of center for arc 1 */
PmCartesian center2; /* Local approximation of center for arc 2 */
double radius1; /* Local approximation of radius */
double radius2;
double theta_tan;
double v_max1; /* maximum velocity in direction u_tan1 */
double v_max2; /* maximum velocity in direction u_tan2 */
} BlendGeom3;
/**
* 9D Input geometry for a spherical blend arc.
*/
#ifdef BLEND_9D
typedef struct {
//Not implemented yet
} BlendGeom9;
#endif
/**
* Blend arc parameters (abstracted).
* This structure holds blend arc parameters that have been abstracted from the
* physical geometry. This data is used to find the maximum radius given the
* constraints on the blend. By abstracting the parameters from the geometry,
* the same calculations can be used with any input geometry (lines, arcs, 6 or
* 9 dimensional lines).
*/
typedef struct {
double tolerance; /* Net blend tolerance (min of line 1 and 2) */
double L1; /* Available part of line 1 to blend over */
double L2; /* Available part of line 2 to blend over */
double v_req; /* requested velocity for the blend arc */
double a_max; /* max acceleration allowed for blend */
/* These fields are considered "output", and may be refactored into a
* separate structure in the future */
double theta; /* Intersection angle, half of angle between -u1 and u2 */
double phi; /* supplement of intersection angle, angle between u1 and u2 */
double a_n_max; /* max normal acceleration allowed */
double R_plan; /* planned radius for blend arc */
double d_plan; /* distance along each line to arc endpoints */
double v_goal; /* desired velocity at max feed override */
double v_plan; /* planned max velocity at max feed override */
double v_actual; /* velocity at feedscale = 1.0 */
double s_arc; /* arc length */
int consume; /* Consume the previous segment */
double line_length;
//Arc specific stuff
int convex1;
int convex2;
double phi1_max;
double phi2_max;
} BlendParameters;
/**
* Output geometry in 3D.
* Stores the three points representing a simple 3D spherical arc.
*/
typedef struct {
PmCartesian arc_start; /* start point for blend arc */
PmCartesian arc_end; /* end point for blend arc */
PmCartesian arc_center; /* center point for blend arc */
double trim1; /* length (line) or angle (arc) to cut from prev_tc */
double trim2; /* length (line) or angle (arc) to cut from tc */
} BlendPoints3;
#ifdef BLEND_9D
typedef struct {
//Not implemented yet
} BlendPoints9;
#endif
double findMaxTangentAngle(double v, double acc, double cycle_time);
double findKinkAccel(double kink_angle, double v_plan, double cycle_time);
double fsign(double f);
int clip_min(double * const x, double min);
int clip_max(double * const x, double max);
double saturate(double x, double max);
double bisaturate(double x, double max, double min);
int sat_inplace(double * const x, double max);
int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end);
int findIntersectionAngle(PmCartesian const * const u1,
PmCartesian const * const u2, double * const theta);
double pmCartMin(PmCartesian const * const in);
int calculateInscribedDiameter(PmCartesian const * const normal,
PmCartesian const * const bounds, double * const diameter);
int findAccelScale(PmCartesian const * const acc,
PmCartesian const * const bounds,
PmCartesian * const scale);
int pmUnitCartsColinear(PmCartesian const * const u1,
PmCartesian const * const u2);
int pmCartCartParallel(PmCartesian const * const u1,
PmCartesian const * const u2,
double tol);
int pmCartCartAntiParallel(PmCartesian const * const u1,
PmCartesian const * const u2,
double tol);
int pmCircLineCoplanar(PmCircle const * const circ,
PmCartLine const * const line, double tol);
int blendCoplanarCheck(PmCartesian const * const normal,
PmCartesian const * const u1_tan,
PmCartesian const * const u2_tan,
double tol);
int blendCalculateNormals3(BlendGeom3 * const geom);
int blendComputeParameters(BlendParameters * const param);
int blendCheckConsume(BlendParameters * const param,
BlendPoints3 const * const points,
TC_STRUCT const * const prev_tc, int gap_cycles);
int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom,
BlendParameters const * const param);
int blendGeom3Init(BlendGeom3 * const geom,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc);
int blendParamKinematics(BlendGeom3 * const geom,
BlendParameters * const param,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc,
PmCartesian const * const acc_bound,
PmCartesian const * const vel_bound,
double maxFeedScale);
int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc,
PmCartesian const * const acc_bound,
PmCartesian const * const vel_bound,
double maxFeedScale);
int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc,
PmCartesian const * const acc_bound,
PmCartesian const * const vel_bound,
double maxFeedScale);
int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc,
PmCartesian const * const acc_bound,
PmCartesian const * const vel_bound,
double maxFeedScale);
int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param,
TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc,
PmCartesian const * const acc_bound,
PmCartesian const * const vel_bound,
double maxFeedScale);
int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
BlendParameters * const param, BlendGeom3 const * const geom,
PmCircle const * const circ1, PmCircle const * const circ2);
int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
BlendParameters * const param, BlendGeom3 const * const geom,
PmCartLine const * const line1, PmCircle const * const circ2);
int blendArcLinePostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
BlendParameters * const param, BlendGeom3 const * const geom,
PmCircle const * const circ1, PmCartLine const * const line2);
int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points,
BlendGeom3 const * const geom, BlendParameters const * const param);
//Not implemented yet
int blendGeom3Print(BlendGeom3 const * const geom);
int blendParamPrint(BlendParameters const * const param);
int blendPoints3Print(BlendPoints3 const * const points);
double pmCartAbsMax(PmCartesian const * const v);
int findSpiralArcLengthFit(PmCircle const * const circle,
SpiralArcLengthFit * const fit);
int pmCircleAngleFromProgress(PmCircle const * const circle,
SpiralArcLengthFit const * const fit,
double progress,
double * const angle);
double pmCircleEffectiveMinRadius(const PmCircle *circle);
static inline double findVPeak(double a_t_max, double distance)
{
return pmSqrt(a_t_max * distance);
}
static inline double findSCurveVPeak(double a_t_max, double j_t_max, double distance)
{
// Parameter validation
if (a_t_max <= 0.0 || j_t_max <= 0.0 || distance <= 0.0) {
return 0.0;
}
double triangular_v = findVPeak(a_t_max, distance);
double req_v;
int result = findSCurveVSpeed(distance, a_t_max, j_t_max, &req_v);
// If the S-curve calculation fails, revert to the simpler triangular velocity calculation.
if (result != 1) {
return triangular_v;
}
// Take the smaller value between the S-curve velocity and the triangular velocity.
return fmin(req_v, triangular_v);
}
#endif

138
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.c vendored Normal file
View File

@@ -0,0 +1,138 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "block.h"
static inline double cruckig_profile_total_duration(const CRuckigProfile *p) {
return p->t_sum[6] + p->brake.duration + p->accel.duration;
}
static void remove_profile(CRuckigProfile *valid_profiles, size_t *valid_profile_counter, size_t index) {
for (size_t i = index; i < *valid_profile_counter - 1; ++i) {
valid_profiles[i] = valid_profiles[i + 1];
}
*valid_profile_counter -= 1;
}
static void interval_from_profiles(CRuckigInterval *iv, const CRuckigProfile *profile_left, const CRuckigProfile *profile_right) {
const double left_duration = cruckig_profile_total_duration(profile_left);
const double right_duration = cruckig_profile_total_duration(profile_right);
if (left_duration < right_duration) {
iv->left = left_duration;
iv->right = right_duration;
iv->profile = *profile_right;
} else {
iv->left = right_duration;
iv->right = left_duration;
iv->profile = *profile_left;
}
iv->valid = true;
}
void cruckig_block_init(CRuckigBlock *block) {
cruckig_profile_init(&block->p_min);
block->t_min = 0.0;
block->a.valid = false;
block->b.valid = false;
}
void cruckig_block_set_min_profile(CRuckigBlock *block, const CRuckigProfile *profile) {
block->p_min = *profile;
block->t_min = cruckig_profile_total_duration(profile);
block->a.valid = false;
block->b.valid = false;
}
bool cruckig_block_calculate(CRuckigBlock *block, CRuckigProfile *valid_profiles,
size_t valid_profile_counter, size_t max_profiles) {
(void)max_profiles;
if (valid_profile_counter == 1) {
cruckig_block_set_min_profile(block, &valid_profiles[0]);
return true;
} else if (valid_profile_counter == 2) {
if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[1].t_sum[6]) < 8 * DBL_EPSILON) {
cruckig_block_set_min_profile(block, &valid_profiles[0]);
return true;
}
/* numerical_robust = true */
{
const size_t idx_min = (valid_profiles[0].t_sum[6] < valid_profiles[1].t_sum[6]) ? 0 : 1;
const size_t idx_else_1 = (idx_min + 1) % 2;
cruckig_block_set_min_profile(block, &valid_profiles[idx_min]);
interval_from_profiles(&block->a, &valid_profiles[idx_min], &valid_profiles[idx_else_1]);
return true;
}
/* Only happens due to numerical issues */
} else if (valid_profile_counter == 4) {
/* Find "identical" profiles */
if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[1].t_sum[6]) < 32 * DBL_EPSILON && valid_profiles[0].direction != valid_profiles[1].direction) {
remove_profile(valid_profiles, &valid_profile_counter, 1);
} else if (fabs(valid_profiles[2].t_sum[6] - valid_profiles[3].t_sum[6]) < 256 * DBL_EPSILON && valid_profiles[2].direction != valid_profiles[3].direction) {
remove_profile(valid_profiles, &valid_profile_counter, 3);
} else if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[3].t_sum[6]) < 256 * DBL_EPSILON && valid_profiles[0].direction != valid_profiles[3].direction) {
remove_profile(valid_profiles, &valid_profile_counter, 3);
} else {
return false;
}
} else if (valid_profile_counter % 2 == 0) {
return false;
}
/* Find index of fastest profile */
size_t idx_min = 0;
for (size_t i = 1; i < valid_profile_counter; ++i) {
if (valid_profiles[i].t_sum[6] < valid_profiles[idx_min].t_sum[6]) {
idx_min = i;
}
}
cruckig_block_set_min_profile(block, &valid_profiles[idx_min]);
if (valid_profile_counter == 3) {
const size_t idx_else_1 = (idx_min + 1) % 3;
const size_t idx_else_2 = (idx_min + 2) % 3;
interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_2]);
return true;
} else if (valid_profile_counter == 5) {
const size_t idx_else_1 = (idx_min + 1) % 5;
const size_t idx_else_2 = (idx_min + 2) % 5;
const size_t idx_else_3 = (idx_min + 3) % 5;
const size_t idx_else_4 = (idx_min + 4) % 5;
if (valid_profiles[idx_else_1].direction == valid_profiles[idx_else_2].direction) {
interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_2]);
interval_from_profiles(&block->b, &valid_profiles[idx_else_3], &valid_profiles[idx_else_4]);
} else {
interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_4]);
interval_from_profiles(&block->b, &valid_profiles[idx_else_2], &valid_profiles[idx_else_3]);
}
return true;
}
return false;
}
/* cruckig_block_is_blocked is now inlined in block.h */
const CRuckigProfile* cruckig_block_get_profile(const CRuckigBlock *block, double t) {
if (block->b.valid && t >= block->b.right) {
return &block->b.profile;
}
if (block->a.valid && t >= block->a.right) {
return &block->a.profile;
}
return &block->p_min;
}

43
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.h vendored Normal file
View File

@@ -0,0 +1,43 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_BLOCK_H
#define CRUCKIG_BLOCK_H
#include "cruckig_internal.h"
#include "profile.h"
typedef struct {
double left, right;
CRuckigProfile profile;
bool valid;
} CRuckigInterval;
typedef struct {
CRuckigProfile p_min;
double t_min;
CRuckigInterval a;
CRuckigInterval b;
} CRuckigBlock;
void cruckig_block_init(CRuckigBlock *block);
void cruckig_block_set_min_profile(CRuckigBlock *block, const CRuckigProfile *profile);
/* Calculate block from valid profiles. Returns true if successful. */
bool cruckig_block_calculate(CRuckigBlock *block, CRuckigProfile *valid_profiles,
size_t valid_profile_counter, size_t max_profiles);
/* Inlined for hot-path performance (called in tight synchronization loop) */
CRUCKIG_FORCE_INLINE bool cruckig_block_is_blocked(const CRuckigBlock *block, double t) {
return (t < block->t_min)
|| (block->a.valid && block->a.left < t && t < block->a.right)
|| (block->b.valid && block->b.left < t && t < block->b.right);
}
const CRuckigProfile* cruckig_block_get_profile(const CRuckigBlock *block, double t);
#endif /* CRUCKIG_BLOCK_H */

201
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.c vendored Normal file
View File

@@ -0,0 +1,201 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "brake.h"
#include "utils.h"
static const double brake_eps = 2.2e-14;
void cruckig_brake_init(CRuckigBrakeProfile *bp) {
bp->duration = 0.0;
bp->t[0] = 0.0;
bp->t[1] = 0.0;
bp->j[0] = 0.0;
bp->j[1] = 0.0;
bp->a[0] = 0.0;
bp->a[1] = 0.0;
bp->v[0] = 0.0;
bp->v[1] = 0.0;
bp->p[0] = 0.0;
bp->p[1] = 0.0;
}
static inline double brake_v_at_t(double v0, double a0, double j, double t) {
return v0 + t * (a0 + j * t / 2);
}
static inline double brake_v_at_a_zero(double v0, double a0, double j) {
return v0 + (a0 * a0) / (2 * j);
}
static void acceleration_brake(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax);
static void velocity_brake(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax);
static void acceleration_brake(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax) {
bp->j[0] = -jMax;
const double t_to_a_max = (a0 - aMax) / jMax;
const double t_to_a_zero = a0 / jMax;
const double v_at_a_max = brake_v_at_t(v0, a0, -jMax, t_to_a_max);
const double v_at_a_zero_val = brake_v_at_t(v0, a0, -jMax, t_to_a_zero);
if ((v_at_a_zero_val > vMax && jMax > 0) || (v_at_a_zero_val < vMax && jMax < 0)) {
velocity_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax);
} else if ((v_at_a_max < vMin && jMax > 0) || (v_at_a_max > vMin && jMax < 0)) {
const double t_to_v_min = -(v_at_a_max - vMin) / aMax;
const double t_to_v_max = -aMax / (2 * jMax) - (v_at_a_max - vMax) / aMax;
bp->t[0] = t_to_a_max + brake_eps;
{
double val = t_to_v_min < (t_to_v_max - brake_eps) ? t_to_v_min : (t_to_v_max - brake_eps);
bp->t[1] = val > 0.0 ? val : 0.0;
}
} else {
bp->t[0] = t_to_a_max + brake_eps;
}
}
static void velocity_brake(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax) {
(void)aMax;
bp->j[0] = -jMax;
const double t_to_a_min = (a0 - aMin) / jMax;
const double t_to_v_max = a0 / jMax + sqrt(a0 * a0 + 2 * jMax * (v0 - vMax)) / fabs(jMax);
const double t_to_v_min = a0 / jMax + sqrt(a0 * a0 / 2 + jMax * (v0 - vMin)) / fabs(jMax);
const double t_min_to_v_max = t_to_v_max < t_to_v_min ? t_to_v_max : t_to_v_min;
if (t_to_a_min < t_min_to_v_max) {
const double v_at_a_min = brake_v_at_t(v0, a0, -jMax, t_to_a_min);
const double t_to_v_max_with_constant = -(v_at_a_min - vMax) / aMin;
const double t_to_v_min_with_constant = aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin;
bp->t[0] = (t_to_a_min - brake_eps) > 0.0 ? (t_to_a_min - brake_eps) : 0.0;
{
double val = t_to_v_max_with_constant < t_to_v_min_with_constant ? t_to_v_max_with_constant : t_to_v_min_with_constant;
bp->t[1] = val > 0.0 ? val : 0.0;
}
} else {
bp->t[0] = (t_min_to_v_max - brake_eps) > 0.0 ? (t_min_to_v_max - brake_eps) : 0.0;
}
}
void cruckig_brake_get_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax) {
bp->t[0] = 0.0;
bp->t[1] = 0.0;
bp->j[0] = 0.0;
bp->j[1] = 0.0;
if (jMax == 0.0 || aMax == 0.0 || aMin == 0.0) {
return; /* Ignore braking for zero-limits */
}
if (a0 > aMax) {
acceleration_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax);
} else if (a0 < aMin) {
acceleration_brake(bp, v0, a0, vMin, vMax, aMin, aMax, -jMax);
} else if ((v0 > vMax && brake_v_at_a_zero(v0, a0, -jMax) > vMin) || (a0 > 0 && brake_v_at_a_zero(v0, a0, jMax) > vMax)) {
velocity_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax);
} else if ((v0 < vMin && brake_v_at_a_zero(v0, a0, jMax) < vMax) || (a0 < 0 && brake_v_at_a_zero(v0, a0, -jMax) < vMin)) {
velocity_brake(bp, v0, a0, vMin, vMax, aMin, aMax, -jMax);
}
}
void cruckig_brake_get_second_order_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0,
double vMax, double vMin, double aMax, double aMin) {
bp->t[0] = 0.0;
bp->t[1] = 0.0;
bp->j[0] = 0.0;
bp->j[1] = 0.0;
bp->a[0] = 0.0;
bp->a[1] = 0.0;
if (aMax == 0.0 || aMin == 0.0) {
return; /* Ignore braking for zero-limits */
}
if (v0 > vMax) {
bp->a[0] = aMin;
bp->t[0] = (vMax - v0) / aMin + brake_eps;
} else if (v0 < vMin) {
bp->a[0] = aMax;
bp->t[0] = (vMin - v0) / aMax + brake_eps;
}
}
void cruckig_brake_get_velocity_brake_trajectory(CRuckigBrakeProfile *bp, double a0,
double aMax, double aMin, double jMax) {
bp->t[0] = 0.0;
bp->t[1] = 0.0;
bp->j[0] = 0.0;
bp->j[1] = 0.0;
if (jMax == 0.0) {
return; /* Ignore braking for zero-limits */
}
if (a0 > aMax) {
bp->j[0] = -jMax;
bp->t[0] = (a0 - aMax) / jMax + brake_eps;
} else if (a0 < aMin) {
bp->j[0] = jMax;
bp->t[0] = -(a0 - aMin) / jMax + brake_eps;
}
}
void cruckig_brake_get_second_order_velocity_brake_trajectory(CRuckigBrakeProfile *bp) {
bp->t[0] = 0.0;
bp->t[1] = 0.0;
bp->j[0] = 0.0;
bp->j[1] = 0.0;
}
void cruckig_brake_finalize(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as) {
if (bp->t[0] <= 0.0 && bp->t[1] <= 0.0) {
bp->duration = 0.0;
return;
}
bp->duration = bp->t[0];
bp->p[0] = *ps;
bp->v[0] = *vs;
bp->a[0] = *as;
cruckig_integrate(bp->t[0], *ps, *vs, *as, bp->j[0], ps, vs, as);
if (bp->t[1] > 0.0) {
bp->duration += bp->t[1];
bp->p[1] = *ps;
bp->v[1] = *vs;
bp->a[1] = *as;
cruckig_integrate(bp->t[1], *ps, *vs, *as, bp->j[1], ps, vs, as);
}
}
void cruckig_brake_finalize_second_order(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as) {
if (bp->t[0] <= 0.0) {
bp->duration = 0.0;
return;
}
bp->duration = bp->t[0];
bp->p[0] = *ps;
bp->v[0] = *vs;
cruckig_integrate(bp->t[0], *ps, *vs, bp->a[0], 0.0, ps, vs, as);
}

38
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.h vendored Normal file
View File

@@ -0,0 +1,38 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_BRAKE_H
#define CRUCKIG_BRAKE_H
#include "cruckig_internal.h"
/* Two-phase brake profile */
typedef struct {
double duration;
double t[2];
double j[2];
double a[2];
double v[2];
double p[2];
} CRuckigBrakeProfile;
void cruckig_brake_init(CRuckigBrakeProfile *bp);
/* Calculate brake trajectories */
void cruckig_brake_get_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, double a0,
double vMax, double vMin, double aMax, double aMin, double jMax);
void cruckig_brake_get_second_order_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0,
double vMax, double vMin, double aMax, double aMin);
void cruckig_brake_get_velocity_brake_trajectory(CRuckigBrakeProfile *bp, double a0,
double aMax, double aMin, double jMax);
void cruckig_brake_get_second_order_velocity_brake_trajectory(CRuckigBrakeProfile *bp);
/* Finalize by integrating */
void cruckig_brake_finalize(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as);
void cruckig_brake_finalize_second_order(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as);
#endif /* CRUCKIG_BRAKE_H */

950
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.c vendored Normal file
View File

@@ -0,0 +1,950 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "calculator.h"
#include "position.h"
#include "velocity.h"
#include "utils.h"
static const double eps = DBL_EPSILON;
CRuckigCalculator* cruckig_calculator_create(size_t dofs) {
CRuckigCalculator *calc = (CRuckigCalculator*)cruckig_calloc(1, sizeof(CRuckigCalculator));
if (!calc) return NULL;
calc->degrees_of_freedom = dofs;
calc->new_phase_control = (double*)cruckig_calloc(dofs, sizeof(double));
calc->pd = (double*)cruckig_calloc(dofs, sizeof(double));
calc->possible_t_syncs = (double*)cruckig_calloc(3 * dofs + 1, sizeof(double));
calc->idx = (size_t*)cruckig_calloc(3 * dofs + 1, sizeof(size_t));
calc->blocks = (CRuckigBlock*)cruckig_calloc(dofs, sizeof(CRuckigBlock));
calc->inp_min_velocity = (double*)cruckig_calloc(dofs, sizeof(double));
calc->inp_min_acceleration = (double*)cruckig_calloc(dofs, sizeof(double));
calc->inp_per_dof_control_interface = (CRuckigControlInterface*)cruckig_calloc(dofs, sizeof(CRuckigControlInterface));
calc->inp_per_dof_synchronization = (CRuckigSynchronization*)cruckig_calloc(dofs, sizeof(CRuckigSynchronization));
calc->segment_input = NULL; /* Created on demand for waypoint calculation */
if (!calc->new_phase_control || !calc->pd || !calc->possible_t_syncs ||
!calc->idx || !calc->blocks || !calc->inp_min_velocity ||
!calc->inp_min_acceleration || !calc->inp_per_dof_control_interface ||
!calc->inp_per_dof_synchronization) {
cruckig_calculator_destroy(calc);
return NULL;
}
for (size_t i = 0; i < dofs; ++i) {
cruckig_block_init(&calc->blocks[i]);
}
return calc;
}
void cruckig_calculator_destroy(CRuckigCalculator *calc) {
if (!calc) return;
cruckig_free(calc->new_phase_control);
cruckig_free(calc->pd);
cruckig_free(calc->possible_t_syncs);
cruckig_free(calc->idx);
cruckig_free(calc->blocks);
cruckig_free(calc->inp_min_velocity);
cruckig_free(calc->inp_min_acceleration);
cruckig_free(calc->inp_per_dof_control_interface);
cruckig_free(calc->inp_per_dof_synchronization);
cruckig_input_destroy(calc->segment_input);
cruckig_free(calc);
}
/* Is the trajectory (in principle) phase synchronizable? */
static bool is_input_collinear(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigDirection limiting_direction,
size_t limiting_dof)
{
const size_t dofs = calc->degrees_of_freedom;
/* Compute pd = target_position - current_position */
for (size_t dof = 0; dof < dofs; ++dof) {
calc->pd[dof] = inp->target_position[dof] - inp->current_position[dof];
}
/* Find scale vector and scale DOF */
const double *scale_vector = NULL;
size_t scale_dof = 0;
bool scale_dof_found = false;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) {
continue;
}
if (calc->inp_per_dof_control_interface[dof] == CRuckigPosition && fabs(calc->pd[dof]) > eps) {
scale_vector = calc->pd;
scale_dof = dof;
scale_dof_found = true;
break;
} else if (fabs(inp->current_velocity[dof]) > eps) {
scale_vector = inp->current_velocity;
scale_dof = dof;
scale_dof_found = true;
break;
} else if (fabs(inp->current_acceleration[dof]) > eps) {
scale_vector = inp->current_acceleration;
scale_dof = dof;
scale_dof_found = true;
break;
} else if (fabs(inp->target_velocity[dof]) > eps) {
scale_vector = inp->target_velocity;
scale_dof = dof;
scale_dof_found = true;
break;
} else if (fabs(inp->target_acceleration[dof]) > eps) {
scale_vector = inp->target_acceleration;
scale_dof = dof;
scale_dof_found = true;
break;
}
}
if (!scale_dof_found) {
return false;
}
const double scale = scale_vector[scale_dof];
const double pd_scale = calc->pd[scale_dof] / scale;
const double v0_scale = inp->current_velocity[scale_dof] / scale;
const double vf_scale = inp->target_velocity[scale_dof] / scale;
const double a0_scale = inp->current_acceleration[scale_dof] / scale;
const double af_scale = inp->target_acceleration[scale_dof] / scale;
const double scale_limiting = scale_vector[limiting_dof];
double control_limiting;
if (isinf(inp->max_jerk[limiting_dof])) {
control_limiting = (limiting_direction == DirectionUP)
? inp->max_acceleration[limiting_dof]
: calc->inp_min_acceleration[limiting_dof];
} else {
control_limiting = (limiting_direction == DirectionUP)
? inp->max_jerk[limiting_dof]
: -inp->max_jerk[limiting_dof];
}
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) {
continue;
}
const double current_scale = scale_vector[dof];
if (
(calc->inp_per_dof_control_interface[dof] == CRuckigPosition && fabs(calc->pd[dof] - pd_scale * current_scale) > eps)
|| fabs(inp->current_velocity[dof] - v0_scale * current_scale) > eps
|| fabs(inp->current_acceleration[dof] - a0_scale * current_scale) > eps
|| fabs(inp->target_velocity[dof] - vf_scale * current_scale) > eps
|| fabs(inp->target_acceleration[dof] - af_scale * current_scale) > eps
) {
return false;
}
calc->new_phase_control[dof] = control_limiting * current_scale / scale_limiting;
}
return true;
}
/* Simple insertion sort for index array by values */
static void sort_indices(size_t *idx_arr, const double *values, size_t count) {
for (size_t i = 1; i < count; ++i) {
size_t key = idx_arr[i];
double key_val = values[key];
size_t j = i;
while (j > 0 && values[idx_arr[j - 1]] > key_val) {
idx_arr[j] = idx_arr[j - 1];
--j;
}
idx_arr[j] = key;
}
}
/*
* synchronize: Find a valid synchronization time.
* Returns true if found; sets t_sync, limiting_dof, and updates profiles.
*
* has_t_min: whether t_min is valid
* t_min: minimum duration
* limiting_dof_out: set to the limiting DOF index; has_limiting_dof set to true/false
*/
static bool synchronize(CRuckigCalculator *calc,
bool has_t_min, double t_min,
double *t_sync,
bool *has_limiting_dof, size_t *limiting_dof_out,
CRuckigProfile *profiles,
bool discrete_duration, double delta_time)
{
const size_t dofs = calc->degrees_of_freedom;
/* Fill possible_t_syncs */
bool any_interval = false;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) {
calc->possible_t_syncs[dof] = 0.0;
calc->possible_t_syncs[dofs + dof] = INFINITY;
calc->possible_t_syncs[2 * dofs + dof] = INFINITY;
continue;
}
calc->possible_t_syncs[dof] = calc->blocks[dof].t_min;
calc->possible_t_syncs[dofs + dof] = calc->blocks[dof].a.valid
? calc->blocks[dof].a.right : INFINITY;
calc->possible_t_syncs[2 * dofs + dof] = calc->blocks[dof].b.valid
? calc->blocks[dof].b.right : INFINITY;
any_interval = any_interval || calc->blocks[dof].a.valid || calc->blocks[dof].b.valid;
}
calc->possible_t_syncs[3 * dofs] = has_t_min ? t_min : INFINITY;
any_interval = any_interval || has_t_min;
/* Discrete duration rounding */
if (discrete_duration) {
size_t count = 3 * dofs + 1;
for (size_t i = 0; i < count; ++i) {
if (isinf(calc->possible_t_syncs[i])) continue;
double remainder = fmod(calc->possible_t_syncs[i], delta_time);
if (remainder > eps) {
calc->possible_t_syncs[i] += delta_time - remainder;
}
}
}
/* Initialize and sort indices */
size_t idx_end_count = any_interval ? (3 * dofs + 1) : dofs;
for (size_t i = 0; i < idx_end_count; ++i) {
calc->idx[i] = i;
}
sort_indices(calc->idx, calc->possible_t_syncs, idx_end_count);
/* Start at dofs-1 (skip the dofs-1 smallest t_min values since we need ALL dofs at or past their t_min) */
size_t start_idx = (dofs >= 1) ? (dofs - 1) : 0;
for (size_t iter = start_idx; iter < idx_end_count; ++iter) {
size_t i = calc->idx[iter];
double possible_t_sync = calc->possible_t_syncs[i];
/* Check if any DOF is blocked */
bool is_blocked = false;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) {
continue;
}
if (cruckig_block_is_blocked(&calc->blocks[dof], possible_t_sync)) {
is_blocked = true;
break;
}
}
double t_min_or_zero = has_t_min ? t_min : 0.0;
if (is_blocked || possible_t_sync < t_min_or_zero || isinf(possible_t_sync)) {
continue;
}
*t_sync = possible_t_sync;
if (i == 3 * dofs) {
/* Optional t_min was the winning candidate */
*has_limiting_dof = false;
return true;
}
/* Determine which DOF and which block part */
size_t quot = i / dofs;
size_t rem = i % dofs;
*limiting_dof_out = rem;
*has_limiting_dof = true;
switch (quot) {
case 0:
profiles[rem] = calc->blocks[rem].p_min;
break;
case 1:
profiles[rem] = calc->blocks[rem].a.profile;
break;
case 2:
profiles[rem] = calc->blocks[rem].b.profile;
break;
}
return true;
}
return false;
}
CRUCKIG_HOT
/*
* Find the optimal profile for a single DOF (Step 1).
* Separated to keep large Step1 structs (~3.6KB) off the main function's stack,
* which matters for the kernel's limited stack size.
*/
static bool find_profile_step1(
CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigProfile *p,
size_t dof)
{
switch (calc->inp_per_dof_control_interface[dof]) {
case CRuckigPosition: {
if (!isinf(inp->max_jerk[dof])) {
CRuckigPositionThirdOrderStep1 *step1 = &calc->step1_workspace.pos3_step1;
cruckig_pos3_step1_init(step1,
p->p[0], p->v[0], p->a[0], p->pf, p->vf, p->af,
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
return cruckig_pos3_step1_get_profile(step1, p, &calc->blocks[dof]);
} else if (!isinf(inp->max_acceleration[dof])) {
CRuckigPositionSecondOrderStep1 *step1 = &calc->step1_workspace.pos2_step1;
cruckig_pos2_step1_init(step1,
p->p[0], p->v[0], p->pf, p->vf,
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
return cruckig_pos2_step1_get_profile(step1, p, &calc->blocks[dof]);
} else {
CRuckigPositionFirstOrderStep1 *step1 = &calc->step1_workspace.pos1_step1;
cruckig_pos1_step1_init(step1,
p->p[0], p->pf,
inp->max_velocity[dof], calc->inp_min_velocity[dof]);
return cruckig_pos1_step1_get_profile(step1, p, &calc->blocks[dof]);
}
} break;
case CRuckigVelocity: {
if (!isinf(inp->max_jerk[dof])) {
CRuckigVelocityThirdOrderStep1 *step1 = &calc->step1_workspace.vel3_step1;
cruckig_vel3_step1_init(step1,
p->v[0], p->a[0], p->vf, p->af,
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
return cruckig_vel3_step1_get_profile(step1, p, &calc->blocks[dof]);
} else {
CRuckigVelocitySecondOrderStep1 *step1 = &calc->step1_workspace.vel2_step1;
cruckig_vel2_step1_init(step1,
p->v[0], p->vf,
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
return cruckig_vel2_step1_get_profile(step1, p, &calc->blocks[dof]);
}
} break;
}
return false;
}
CRuckigResult cruckig_calculator_calculate(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted)
{
*was_interrupted = false;
const size_t dofs = calc->degrees_of_freedom;
for (size_t dof = 0; dof < dofs; ++dof) {
CRuckigProfile *p = &traj->profiles[dof];
calc->inp_min_velocity[dof] = inp->min_velocity
? inp->min_velocity[dof] : -inp->max_velocity[dof];
calc->inp_min_acceleration[dof] = inp->min_acceleration
? inp->min_acceleration[dof] : -inp->max_acceleration[dof];
calc->inp_per_dof_control_interface[dof] = inp->per_dof_control_interface
? inp->per_dof_control_interface[dof] : inp->control_interface;
calc->inp_per_dof_synchronization[dof] = inp->per_dof_synchronization
? inp->per_dof_synchronization[dof] : inp->synchronization;
if (!inp->enabled[dof]) {
p->p[7] = inp->current_position[dof];
p->v[7] = inp->current_velocity[dof];
p->a[7] = inp->current_acceleration[dof];
p->t_sum[6] = 0.0;
calc->blocks[dof].t_min = 0.0;
calc->blocks[dof].a.valid = false;
calc->blocks[dof].b.valid = false;
continue;
}
/* Calculate brake (if input exceeds or will exceed limits) */
switch (calc->inp_per_dof_control_interface[dof]) {
case CRuckigPosition: {
if (!isinf(inp->max_jerk[dof])) {
cruckig_brake_get_position_brake_trajectory(&p->brake,
inp->current_velocity[dof], inp->current_acceleration[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else if (!isinf(inp->max_acceleration[dof])) {
cruckig_brake_get_second_order_position_brake_trajectory(&p->brake,
inp->current_velocity[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
}
cruckig_profile_set_boundary(p,
inp->current_position[dof], inp->current_velocity[dof],
inp->current_acceleration[dof],
inp->target_position[dof], inp->target_velocity[dof],
inp->target_acceleration[dof]);
} break;
case CRuckigVelocity: {
if (!isinf(inp->max_jerk[dof])) {
cruckig_brake_get_velocity_brake_trajectory(&p->brake,
inp->current_acceleration[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else {
cruckig_brake_get_second_order_velocity_brake_trajectory(&p->brake);
}
cruckig_profile_set_boundary_for_velocity(p,
inp->current_position[dof], inp->current_velocity[dof],
inp->current_acceleration[dof],
inp->target_velocity[dof], inp->target_acceleration[dof]);
} break;
}
/* Finalize pre-trajectory */
if (!isinf(inp->max_jerk[dof])) {
cruckig_brake_finalize(&p->brake, &p->p[0], &p->v[0], &p->a[0]);
} else if (!isinf(inp->max_acceleration[dof])) {
cruckig_brake_finalize_second_order(&p->brake, &p->p[0], &p->v[0], &p->a[0]);
}
if (!find_profile_step1(calc, inp, p, dof)) {
bool has_zero_limits = (inp->max_acceleration[dof] == 0.0 ||
calc->inp_min_acceleration[dof] == 0.0 ||
inp->max_jerk[dof] == 0.0);
if (has_zero_limits) {
return CRuckigErrorZeroLimits;
} else {
return CRuckigErrorExecutionTimeCalculation;
}
}
traj->independent_min_durations[dof] = calc->blocks[dof].t_min;
}
const bool discrete_duration = (inp->duration_discretization == CRuckigDiscrete);
if (dofs == 1 && !inp->has_minimum_duration && !discrete_duration) {
traj->duration = calc->blocks[0].t_min;
traj->profiles[0] = calc->blocks[0].p_min;
traj->cumulative_times[0] = traj->duration;
return CRuckigWorking;
}
/* Synchronize */
bool has_limiting_dof = false;
size_t limiting_dof = 0;
bool found_synchronization = synchronize(calc,
inp->has_minimum_duration, inp->minimum_duration,
&traj->duration, &has_limiting_dof, &limiting_dof,
traj->profiles, discrete_duration, delta_time);
if (!found_synchronization) {
bool has_zero_limits = false;
for (size_t dof = 0; dof < dofs; ++dof) {
if (inp->max_acceleration[dof] == 0.0 ||
calc->inp_min_acceleration[dof] == 0.0 ||
inp->max_jerk[dof] == 0.0) {
has_zero_limits = true;
break;
}
}
if (has_zero_limits) {
return CRuckigErrorZeroLimits;
} else {
return CRuckigErrorSynchronizationCalculation;
}
}
/* None Synchronization */
for (size_t dof = 0; dof < dofs; ++dof) {
if (inp->enabled[dof] && calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) {
traj->profiles[dof] = calc->blocks[dof].p_min;
if (calc->blocks[dof].t_min > traj->duration) {
traj->duration = calc->blocks[dof].t_min;
has_limiting_dof = true;
limiting_dof = dof;
}
}
}
traj->cumulative_times[0] = traj->duration;
/* Check maximal duration */
if (traj->duration > 7.6e3) {
return CRuckigErrorTrajectoryDuration;
}
if (traj->duration == 0.0) {
/* Copy all profiles for end state */
for (size_t dof = 0; dof < dofs; ++dof) {
traj->profiles[dof] = calc->blocks[dof].p_min;
}
return CRuckigWorking;
}
/* Check if all synchronizations are None */
if (!discrete_duration) {
bool all_none = true;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncNone) {
all_none = false;
break;
}
}
if (all_none) {
return CRuckigWorking;
}
}
/* Phase Synchronization */
if (has_limiting_dof) {
bool any_phase = false;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncPhase) {
any_phase = true;
break;
}
}
if (any_phase) {
const CRuckigProfile *p_limiting = &traj->profiles[limiting_dof];
if (is_input_collinear(calc, inp, p_limiting->direction, limiting_dof)) {
bool found_time_synchronization = true;
for (size_t dof = 0; dof < dofs; ++dof) {
if (!inp->enabled[dof] || dof == limiting_dof ||
calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) {
continue;
}
CRuckigProfile *p = &traj->profiles[dof];
double t_profile = traj->duration - p->brake.duration - p->accel.duration;
/* Copy timing information from limiting DOF */
memcpy(p->t, p_limiting->t, sizeof(p->t));
p->control_signs = p_limiting->control_signs;
switch (calc->inp_per_dof_control_interface[dof]) {
case CRuckigPosition: {
switch (p->control_signs) {
case ControlSignsUDDU: {
if (!isinf(inp->max_jerk[dof])) {
found_time_synchronization &= cruckig_profile_check_with_timing_full(p,
ControlSignsUDDU, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else if (!isinf(inp->max_acceleration[dof])) {
found_time_synchronization &= cruckig_profile_check_for_second_order_with_timing_full(p,
ControlSignsUDDU, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
-calc->new_phase_control[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
} else {
found_time_synchronization &= cruckig_profile_check_for_first_order_with_timing_full(p,
ControlSignsUDDU, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof]);
}
} break;
case ControlSignsUDUD: {
if (!isinf(inp->max_jerk[dof])) {
found_time_synchronization &= cruckig_profile_check_with_timing_full(p,
ControlSignsUDUD, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else {
found_time_synchronization &= cruckig_profile_check_for_second_order_with_timing_full(p,
ControlSignsUDUD, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
-calc->new_phase_control[dof],
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
}
} break;
}
} break;
case CRuckigVelocity: {
switch (p->control_signs) {
case ControlSignsUDDU: {
if (!isinf(inp->max_jerk[dof])) {
found_time_synchronization &= cruckig_profile_check_for_velocity_with_timing_full(p,
ControlSignsUDDU, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else {
found_time_synchronization &= cruckig_profile_check_for_second_order_velocity_with_timing_full(p,
ControlSignsUDDU, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
}
} break;
case ControlSignsUDUD: {
if (!isinf(inp->max_jerk[dof])) {
found_time_synchronization &= cruckig_profile_check_for_velocity_with_timing_full(p,
ControlSignsUDUD, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
} else {
found_time_synchronization &= cruckig_profile_check_for_second_order_velocity_with_timing_full(p,
ControlSignsUDUD, ReachedLimitsNONE,
t_profile, calc->new_phase_control[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
}
} break;
}
} break;
}
p->limits = p_limiting->limits; /* After check method call */
}
if (found_time_synchronization) {
bool all_phase_or_none = true;
for (size_t dof = 0; dof < dofs; ++dof) {
if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase &&
calc->inp_per_dof_synchronization[dof] != CRuckigSyncNone) {
all_phase_or_none = false;
break;
}
}
if (all_phase_or_none) {
return CRuckigWorking;
}
}
}
}
}
/* Time Synchronization (Step 2) */
for (size_t dof = 0; dof < dofs; ++dof) {
bool skip_synchronization = ((has_limiting_dof && dof == limiting_dof) ||
calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) &&
!discrete_duration;
if (!inp->enabled[dof] || skip_synchronization) {
continue;
}
CRuckigProfile *p = &traj->profiles[dof];
double t_profile = traj->duration - p->brake.duration - p->accel.duration;
if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncTimeIfNecessary &&
fabs(inp->target_velocity[dof]) < eps &&
fabs(inp->target_acceleration[dof]) < eps) {
*p = calc->blocks[dof].p_min;
continue;
}
/* Check if the final time corresponds to an extremal profile from step 1 */
if (fabs(t_profile - calc->blocks[dof].t_min) < 2 * eps) {
*p = calc->blocks[dof].p_min;
continue;
} else if (calc->blocks[dof].a.valid && fabs(t_profile - calc->blocks[dof].a.right) < 2 * eps) {
*p = calc->blocks[dof].a.profile;
continue;
} else if (calc->blocks[dof].b.valid && fabs(t_profile - calc->blocks[dof].b.right) < 2 * eps) {
*p = calc->blocks[dof].b.profile;
continue;
}
bool found_time_synchronization = false;
switch (calc->inp_per_dof_control_interface[dof]) {
case CRuckigPosition: {
if (!isinf(inp->max_jerk[dof])) {
CRuckigPositionThirdOrderStep2 step2;
cruckig_pos3_step2_init(&step2,
t_profile, p->p[0], p->v[0], p->a[0], p->pf, p->vf, p->af,
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
found_time_synchronization = cruckig_pos3_step2_get_profile(&step2, p);
} else if (!isinf(inp->max_acceleration[dof])) {
CRuckigPositionSecondOrderStep2 step2;
cruckig_pos2_step2_init(&step2,
t_profile, p->p[0], p->v[0], p->pf, p->vf,
inp->max_velocity[dof], calc->inp_min_velocity[dof],
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
found_time_synchronization = cruckig_pos2_step2_get_profile(&step2, p);
} else {
CRuckigPositionFirstOrderStep2 step2;
cruckig_pos1_step2_init(&step2,
t_profile, p->p[0], p->pf,
inp->max_velocity[dof], calc->inp_min_velocity[dof]);
found_time_synchronization = cruckig_pos1_step2_get_profile(&step2, p);
}
} break;
case CRuckigVelocity: {
if (!isinf(inp->max_jerk[dof])) {
CRuckigVelocityThirdOrderStep2 step2;
cruckig_vel3_step2_init(&step2,
t_profile, p->v[0], p->a[0], p->vf, p->af,
inp->max_acceleration[dof], calc->inp_min_acceleration[dof],
inp->max_jerk[dof]);
found_time_synchronization = cruckig_vel3_step2_get_profile(&step2, p);
} else {
CRuckigVelocitySecondOrderStep2 step2;
cruckig_vel2_step2_init(&step2,
t_profile, p->v[0], p->vf,
inp->max_acceleration[dof], calc->inp_min_acceleration[dof]);
found_time_synchronization = cruckig_vel2_step2_get_profile(&step2, p);
}
} break;
}
if (!found_time_synchronization) {
return CRuckigErrorSynchronizationCalculation;
}
}
return CRuckigWorking;
}
/*
* Multi-segment waypoint calculation.
*
* Strategy: sequential segment planning. For each segment between consecutive
* waypoints, use the existing single-segment planner. The end state of segment i
* becomes the start state of segment i+1. At intermediate waypoints, velocity
* and acceleration pass through continuously (zero target velocity at waypoints
* for robustness, with option to optimize).
*/
CRuckigResult cruckig_calculator_calculate_waypoints(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted)
{
const size_t dofs = calc->degrees_of_freedom;
const size_t nwp = inp->num_intermediate_waypoints;
const size_t nsec = nwp + 1; /* Number of sections */
/* Resize trajectory for multi-section */
if (!cruckig_trajectory_resize(traj, nsec)) {
return CRuckigError;
}
/* Create reusable segment input if needed */
if (!calc->segment_input) {
calc->segment_input = cruckig_input_create(dofs);
if (!calc->segment_input) return CRuckigError;
}
CRuckigInputParameter *seg = calc->segment_input;
/* Build a temporary single-section trajectory for each segment */
CRuckigTrajectory *seg_traj = cruckig_trajectory_create(dofs);
if (!seg_traj) return CRuckigError;
double cumulative_time = 0.0;
CRuckigResult final_result = CRuckigWorking;
for (size_t s = 0; s < nsec; ++s) {
/* Set segment input: copy global settings */
seg->control_interface = CRuckigPosition;
seg->synchronization = inp->synchronization;
seg->duration_discretization = CRuckigContinuous;
seg->has_minimum_duration = false;
/* Per-section minimum duration */
if (inp->per_section_minimum_duration) {
seg->minimum_duration = inp->per_section_minimum_duration[s];
seg->has_minimum_duration = true;
}
/* Set start state */
if (s == 0) {
/* First segment starts from input current state */
memcpy(seg->current_position, inp->current_position, dofs * sizeof(double));
memcpy(seg->current_velocity, inp->current_velocity, dofs * sizeof(double));
memcpy(seg->current_acceleration, inp->current_acceleration, dofs * sizeof(double));
}
/* else: current state was set by previous iteration's end state */
/* Set target state */
if (s < nwp) {
/* Target is the next intermediate waypoint */
const double *wp = inp->intermediate_positions + s * dofs;
memcpy(seg->target_position, wp, dofs * sizeof(double));
/* Zero velocity/acceleration at intermediate waypoints */
memset(seg->target_velocity, 0, dofs * sizeof(double));
memset(seg->target_acceleration, 0, dofs * sizeof(double));
} else {
/* Last segment targets the final position */
memcpy(seg->target_position, inp->target_position, dofs * sizeof(double));
memcpy(seg->target_velocity, inp->target_velocity, dofs * sizeof(double));
memcpy(seg->target_acceleration, inp->target_acceleration, dofs * sizeof(double));
}
/* Set kinematic constraints (per-section or global) */
if (inp->per_section_max_velocity) {
memcpy(seg->max_velocity, inp->per_section_max_velocity + s * dofs, dofs * sizeof(double));
} else {
memcpy(seg->max_velocity, inp->max_velocity, dofs * sizeof(double));
}
if (inp->per_section_max_acceleration) {
memcpy(seg->max_acceleration, inp->per_section_max_acceleration + s * dofs, dofs * sizeof(double));
} else {
memcpy(seg->max_acceleration, inp->max_acceleration, dofs * sizeof(double));
}
if (inp->per_section_max_jerk) {
memcpy(seg->max_jerk, inp->per_section_max_jerk + s * dofs, dofs * sizeof(double));
} else {
memcpy(seg->max_jerk, inp->max_jerk, dofs * sizeof(double));
}
/* Optional min limits */
if (inp->per_section_min_velocity) {
if (!seg->min_velocity) seg->min_velocity = (double*)cruckig_malloc(dofs * sizeof(double));
memcpy(seg->min_velocity, inp->per_section_min_velocity + s * dofs, dofs * sizeof(double));
} else if (inp->min_velocity) {
if (!seg->min_velocity) seg->min_velocity = (double*)cruckig_malloc(dofs * sizeof(double));
memcpy(seg->min_velocity, inp->min_velocity, dofs * sizeof(double));
} else {
cruckig_free(seg->min_velocity);
seg->min_velocity = NULL;
}
if (inp->per_section_min_acceleration) {
if (!seg->min_acceleration) seg->min_acceleration = (double*)cruckig_malloc(dofs * sizeof(double));
memcpy(seg->min_acceleration, inp->per_section_min_acceleration + s * dofs, dofs * sizeof(double));
} else if (inp->min_acceleration) {
if (!seg->min_acceleration) seg->min_acceleration = (double*)cruckig_malloc(dofs * sizeof(double));
memcpy(seg->min_acceleration, inp->min_acceleration, dofs * sizeof(double));
} else {
cruckig_free(seg->min_acceleration);
seg->min_acceleration = NULL;
}
/* Enable all DOFs for segment */
for (size_t d = 0; d < dofs; ++d) seg->enabled[d] = true;
/* Calculate this segment */
bool seg_interrupted = false;
CRuckigResult seg_result = cruckig_calculator_calculate(calc, seg, seg_traj,
delta_time, &seg_interrupted);
if (seg_result != CRuckigWorking) {
cruckig_trajectory_destroy(seg_traj);
*was_interrupted = false;
return seg_result;
}
/* Copy segment profiles into the multi-section trajectory */
double seg_duration = cruckig_trajectory_get_duration(seg_traj);
cumulative_time += seg_duration;
traj->cumulative_times[s] = cumulative_time;
for (size_t d = 0; d < dofs; ++d) {
traj->profiles[s * dofs + d] = seg_traj->profiles[d];
if (s == 0) {
traj->independent_min_durations[d] = seg_traj->independent_min_durations[d];
}
}
/* Set next segment's start state from this segment's end state */
if (s < nsec - 1) {
for (size_t d = 0; d < dofs; ++d) {
const CRuckigProfile *p = &seg_traj->profiles[d];
seg->current_position[d] = p->p[7];
seg->current_velocity[d] = p->v[7];
seg->current_acceleration[d] = p->a[7];
}
}
}
traj->duration = cumulative_time;
cruckig_trajectory_destroy(seg_traj);
/* Position limits check */
if (inp->max_position || inp->min_position ||
inp->per_section_max_position || inp->per_section_min_position)
{
/* Sample trajectory and check bounds */
double *pos = (double*)cruckig_malloc(dofs * sizeof(double));
double *vel = (double*)cruckig_malloc(dofs * sizeof(double));
double *acc = (double*)cruckig_malloc(dofs * sizeof(double));
size_t sec;
bool violated = false;
/* Check at fine time steps */
double dt_check = (delta_time > 0.0) ? delta_time : 0.001;
for (double t = 0.0; t <= cumulative_time && !violated; t += dt_check) {
cruckig_trajectory_at_time(traj, t, pos, vel, acc, NULL, &sec);
for (size_t d = 0; d < dofs; ++d) {
double p_max = INFINITY, p_min = -INFINITY;
if (inp->max_position) p_max = inp->max_position[d];
if (inp->min_position) p_min = inp->min_position[d];
/* Per-section position limits */
if (sec < nsec) {
if (inp->per_section_max_position) {
double sec_max = inp->per_section_max_position[sec * dofs + d];
if (sec_max < p_max) p_max = sec_max;
}
if (inp->per_section_min_position) {
double sec_min = inp->per_section_min_position[sec * dofs + d];
if (sec_min > p_min) p_min = sec_min;
}
}
if (pos[d] > p_max + 1e-8 || pos[d] < p_min - 1e-8) {
violated = true;
break;
}
}
}
/* Also check position extrema */
if (!violated) {
cruckig_trajectory_get_position_extrema(traj);
for (size_t d = 0; d < dofs; ++d) {
double p_max = INFINITY, p_min = -INFINITY;
if (inp->max_position) p_max = inp->max_position[d];
if (inp->min_position) p_min = inp->min_position[d];
if (traj->position_extrema[d].max > p_max + 1e-8 ||
traj->position_extrema[d].min < p_min - 1e-8) {
violated = true;
break;
}
}
}
cruckig_free(pos);
cruckig_free(vel);
cruckig_free(acc);
if (violated) {
final_result = CRuckigErrorPositionalLimits;
}
}
*was_interrupted = false;
return final_result;
}
CRuckigResult cruckig_calculator_continue(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted)
{
/* For now, continue_calculation simply re-runs the full calculation.
* A future optimization could resume from partial state. */
if (inp->num_intermediate_waypoints > 0 && inp->control_interface == CRuckigPosition) {
return cruckig_calculator_calculate_waypoints(calc, inp, traj, delta_time, was_interrupted);
}
return cruckig_calculator_calculate(calc, inp, traj, delta_time, was_interrupted);
}

71
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.h vendored Normal file
View File

@@ -0,0 +1,71 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_CALCULATOR_H
#define CRUCKIG_CALCULATOR_H
#include "cruckig_internal.h"
#include "result.h"
#include "block.h"
#include "input_parameter.h"
#include "trajectory.h"
#include "position.h"
#include "velocity.h"
typedef struct {
size_t degrees_of_freedom;
double *new_phase_control;
double *pd;
double *possible_t_syncs;
size_t *idx;
CRuckigBlock *blocks;
double *inp_min_velocity;
double *inp_min_acceleration;
CRuckigControlInterface *inp_per_dof_control_interface;
CRuckigSynchronization *inp_per_dof_synchronization;
/* Scratch space for waypoint calculation */
CRuckigInputParameter *segment_input; /* Reusable per-segment input */
/* Step1 workspace: kept off the stack to stay within kernel frame limits.
* Only one Step1 type is active at a time, so a union suffices. */
union {
CRuckigPositionThirdOrderStep1 pos3_step1;
CRuckigPositionSecondOrderStep1 pos2_step1;
CRuckigPositionFirstOrderStep1 pos1_step1;
CRuckigVelocityThirdOrderStep1 vel3_step1;
CRuckigVelocitySecondOrderStep1 vel2_step1;
} step1_workspace;
} CRuckigCalculator;
CRuckigCalculator* cruckig_calculator_create(size_t dofs);
void cruckig_calculator_destroy(CRuckigCalculator *calc);
/* Single-segment calculation (existing, backward compatible) */
CRuckigResult cruckig_calculator_calculate(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted);
/* Multi-segment waypoint calculation */
CRuckigResult cruckig_calculator_calculate_waypoints(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted);
/* Continue an interrupted calculation */
CRuckigResult cruckig_calculator_continue(CRuckigCalculator *calc,
const CRuckigInputParameter *inp,
CRuckigTrajectory *traj,
double delta_time,
bool *was_interrupted);
#endif /* CRUCKIG_CALCULATOR_H */

173
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.c vendored Normal file
View File

@@ -0,0 +1,173 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "cruckig.h"
static CRuckig* cruckig_create_internal(size_t dofs, double delta_time, size_t max_waypoints) {
CRuckig *r = (CRuckig*)cruckig_calloc(1, sizeof(CRuckig));
if (!r) return NULL;
r->degrees_of_freedom = dofs;
r->delta_time = delta_time;
r->max_number_of_waypoints = max_waypoints;
r->calculator = cruckig_calculator_create(dofs);
if (!r->calculator) {
cruckig_free(r);
return NULL;
}
r->current_input = cruckig_input_create(dofs);
if (!r->current_input) {
cruckig_calculator_destroy(r->calculator);
cruckig_free(r);
return NULL;
}
r->current_input_initialized = false;
return r;
}
CRuckig* cruckig_create(size_t dofs, double delta_time) {
return cruckig_create_internal(dofs, delta_time, 0);
}
CRuckig* cruckig_create_waypoints(size_t dofs, double delta_time, size_t max_waypoints) {
return cruckig_create_internal(dofs, delta_time, max_waypoints);
}
void cruckig_destroy(CRuckig *r) {
if (!r) return;
cruckig_calculator_destroy(r->calculator);
cruckig_input_destroy(r->current_input);
cruckig_free(r);
}
void cruckig_reset(CRuckig *r) {
if (!r) return;
r->current_input_initialized = false;
}
static inline bool use_waypoints(const CRuckigInputParameter *input) {
return input->num_intermediate_waypoints > 0 &&
input->control_interface == CRuckigPosition;
}
bool cruckig_validate_input(const CRuckig *r, const CRuckigInputParameter *input,
bool check_current_within_limits,
bool check_target_within_limits)
{
if (!r || !input) return false;
if (!cruckig_input_validate(input, check_current_within_limits, check_target_within_limits)) {
return false;
}
if (r->delta_time <= 0.0 && input->duration_discretization != CRuckigContinuous) {
return false;
}
/* Validate waypoint count against max */
if (input->num_intermediate_waypoints > r->max_number_of_waypoints &&
r->max_number_of_waypoints > 0) {
return false;
}
return true;
}
static CRuckigResult dispatch_calculate(CRuckig *r, const CRuckigInputParameter *input,
CRuckigTrajectory *trajectory, bool *was_interrupted)
{
if (use_waypoints(input)) {
/* Ensure trajectory has enough capacity */
size_t nsec = input->num_intermediate_waypoints + 1;
if (!cruckig_trajectory_resize(trajectory, nsec)) {
return CRuckigError;
}
return cruckig_calculator_calculate_waypoints(r->calculator, input, trajectory,
r->delta_time, was_interrupted);
} else {
/* Single-segment: ensure single section */
if (trajectory->num_sections != 1) {
cruckig_trajectory_resize(trajectory, 1);
}
return cruckig_calculator_calculate(r->calculator, input, trajectory,
r->delta_time, was_interrupted);
}
}
CRuckigResult cruckig_calculate(CRuckig *r, const CRuckigInputParameter *input,
CRuckigTrajectory *trajectory)
{
if (!r || !input || !trajectory) return CRuckigError;
if (!cruckig_validate_input(r, input, false, true)) {
return CRuckigErrorInvalidInput;
}
bool was_interrupted = false;
return dispatch_calculate(r, input, trajectory, &was_interrupted);
}
static double get_time_us(void) {
/* Timing measurement for interrupt budget feature.
* Not used by LinuxCNC (only cruckig_update, not cruckig_calculate). */
return 0.0;
}
CRUCKIG_HOT
CRuckigResult cruckig_update(CRuckig *r, const CRuckigInputParameter *input,
CRuckigOutputParameter *output)
{
if (CRUCKIG_UNLIKELY(!r || !input || !output)) return CRuckigError;
double start_us = get_time_us();
output->new_calculation = false;
CRuckigResult result = CRuckigWorking;
if (!r->current_input_initialized || !cruckig_input_is_equal(input, r->current_input)) {
if (!cruckig_validate_input(r, input, false, true)) {
return CRuckigErrorInvalidInput;
}
result = dispatch_calculate(r, input, output->trajectory,
&output->was_calculation_interrupted);
if (result != CRuckigWorking && result != CRuckigErrorPositionalLimits) {
return result;
}
cruckig_input_copy(r->current_input, input);
r->current_input_initialized = true;
output->time = 0.0;
output->new_section = 0;
output->new_calculation = true;
}
size_t old_section = output->new_section;
output->time += r->delta_time;
cruckig_trajectory_at_time(output->trajectory, output->time,
output->new_position, output->new_velocity,
output->new_acceleration, output->new_jerk,
&output->new_section);
output->did_section_change = (output->new_section > old_section);
double stop_us = get_time_us();
output->calculation_duration = stop_us - start_us;
cruckig_output_pass_to_input(output, r->current_input);
if (output->time > cruckig_trajectory_get_duration(output->trajectory)) {
return CRuckigFinished;
}
return result;
}

54
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.h vendored Normal file
View File

@@ -0,0 +1,54 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_CRUCKIG_H
#define CRUCKIG_CRUCKIG_H
#include "cruckig_internal.h"
#include "result.h"
#include "input_parameter.h"
#include "output_parameter.h"
#include "trajectory.h"
#include "calculator.h"
/* Main cruckig instance */
typedef struct {
size_t degrees_of_freedom;
double delta_time;
size_t max_number_of_waypoints;
CRuckigCalculator *calculator;
CRuckigInputParameter *current_input;
bool current_input_initialized;
} CRuckig;
/* Create and destroy (backward compatible: 0 waypoints) */
CRuckig* cruckig_create(size_t dofs, double delta_time);
/* Create with waypoint support */
CRuckig* cruckig_create_waypoints(size_t dofs, double delta_time, size_t max_waypoints);
void cruckig_destroy(CRuckig *r);
/* Reset (force recalculation on next update) */
void cruckig_reset(CRuckig *r);
/* Calculate trajectory (offline, auto-dispatches to waypoint calculator if needed) */
CRuckigResult cruckig_calculate(CRuckig *r, const CRuckigInputParameter *input,
CRuckigTrajectory *trajectory);
/* Update (online, call every delta_time) */
CRuckigResult cruckig_update(CRuckig *r, const CRuckigInputParameter *input,
CRuckigOutputParameter *output);
/* Validate input */
bool cruckig_validate_input(const CRuckig *r, const CRuckigInputParameter *input,
bool check_current_within_limits,
bool check_target_within_limits);
#endif /* CRUCKIG_CRUCKIG_H */

55
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig_internal.h vendored Normal file
View File

@@ -0,0 +1,55 @@
/*
* cruckig_internal.h - Internal header for cruckig
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*
* Provides RTAPI-portable types, memory allocation, math, string
* functions, and compiler hint macros for cruckig internals.
*
* All cruckig headers should include this as their first include.
* C files should NOT include this directly -- they get it through
* their corresponding header.
*/
#ifndef CRUCKIG_CRUCKIG_INTERNAL_H
#define CRUCKIG_CRUCKIG_INTERNAL_H
/* RTAPI provides bool, size_t, math, string, and memory allocation
* portably across userspace and kernel builds. */
#include <rtapi.h>
#include <rtapi_bool.h>
#include <rtapi_math.h>
#include <rtapi_string.h>
#include <rtapi_slab.h>
#include <float.h>
/* INFINITY: not provided by rtapi_math.h in kernel space */
#ifndef INFINITY
#define INFINITY __builtin_inf()
#endif
/* Memory allocation: always use rtapi_slab wrappers */
#define cruckig_malloc(sz) rtapi_kmalloc(sz, RTAPI_GFP_KERNEL)
#define cruckig_calloc(n, sz) rtapi_kzalloc((n) * (sz), RTAPI_GFP_KERNEL)
#define cruckig_realloc(p, sz) rtapi_krealloc(p, sz, RTAPI_GFP_KERNEL)
#define cruckig_free(p) rtapi_kfree(p)
/* Branch prediction hints */
#if defined(__GNUC__) || defined(__clang__)
# define CRUCKIG_LIKELY(x) __builtin_expect(!!(x), 1)
# define CRUCKIG_UNLIKELY(x) __builtin_expect(!!(x), 0)
# define CRUCKIG_FORCE_INLINE static inline __attribute__((always_inline))
# define CRUCKIG_HOT __attribute__((hot))
# define CRUCKIG_RESTRICT __restrict__
# define CRUCKIG_PREFETCH(addr) __builtin_prefetch(addr, 0, 1)
#else
# define CRUCKIG_LIKELY(x) (x)
# define CRUCKIG_UNLIKELY(x) (x)
# define CRUCKIG_FORCE_INLINE static inline
# define CRUCKIG_HOT
# define CRUCKIG_RESTRICT restrict
# define CRUCKIG_PREFETCH(addr) ((void)0)
#endif
#endif /* CRUCKIG_CRUCKIG_INTERNAL_H */

408
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.c vendored Normal file
View File

@@ -0,0 +1,408 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "input_parameter.h"
static double v_at_a_zero(double v0, double a0, double j) {
return v0 + (a0 * a0) / (2.0 * j);
}
CRuckigInputParameter* cruckig_input_create(size_t dofs) {
CRuckigInputParameter *inp = (CRuckigInputParameter*)cruckig_calloc(1, sizeof(CRuckigInputParameter));
if (!inp) return NULL;
inp->degrees_of_freedom = dofs;
inp->control_interface = CRuckigPosition;
inp->synchronization = CRuckigSyncTime;
inp->duration_discretization = CRuckigContinuous;
inp->current_position = (double*)cruckig_calloc(dofs, sizeof(double));
inp->current_velocity = (double*)cruckig_calloc(dofs, sizeof(double));
inp->current_acceleration = (double*)cruckig_calloc(dofs, sizeof(double));
inp->target_position = (double*)cruckig_calloc(dofs, sizeof(double));
inp->target_velocity = (double*)cruckig_calloc(dofs, sizeof(double));
inp->target_acceleration = (double*)cruckig_calloc(dofs, sizeof(double));
inp->max_velocity = (double*)cruckig_calloc(dofs, sizeof(double));
inp->max_acceleration = (double*)cruckig_malloc(dofs * sizeof(double));
inp->max_jerk = (double*)cruckig_malloc(dofs * sizeof(double));
inp->enabled = (bool*)cruckig_malloc(dofs * sizeof(bool));
if (!inp->current_position || !inp->current_velocity || !inp->current_acceleration ||
!inp->target_position || !inp->target_velocity || !inp->target_acceleration ||
!inp->max_velocity || !inp->max_acceleration || !inp->max_jerk || !inp->enabled) {
cruckig_input_destroy(inp);
return NULL;
}
/* Initialize defaults matching C++ */
for (size_t dof = 0; dof < dofs; ++dof) {
inp->max_acceleration[dof] = INFINITY;
inp->max_jerk[dof] = INFINITY;
inp->enabled[dof] = true;
}
inp->min_velocity = NULL;
inp->min_acceleration = NULL;
inp->per_dof_control_interface = NULL;
inp->per_dof_synchronization = NULL;
inp->minimum_duration = -1.0;
inp->has_minimum_duration = false;
/* Pro fields: initialize to defaults */
inp->intermediate_positions = NULL;
inp->num_intermediate_waypoints = 0;
inp->per_section_max_velocity = NULL;
inp->per_section_max_acceleration = NULL;
inp->per_section_max_jerk = NULL;
inp->per_section_min_velocity = NULL;
inp->per_section_min_acceleration = NULL;
inp->per_section_max_position = NULL;
inp->per_section_min_position = NULL;
inp->max_position = NULL;
inp->min_position = NULL;
inp->per_section_minimum_duration = NULL;
inp->interrupt_calculation_duration = 0.0;
return inp;
}
void cruckig_input_destroy(CRuckigInputParameter *inp) {
if (!inp) return;
cruckig_free(inp->current_position);
cruckig_free(inp->current_velocity);
cruckig_free(inp->current_acceleration);
cruckig_free(inp->target_position);
cruckig_free(inp->target_velocity);
cruckig_free(inp->target_acceleration);
cruckig_free(inp->max_velocity);
cruckig_free(inp->max_acceleration);
cruckig_free(inp->max_jerk);
cruckig_free(inp->enabled);
cruckig_free(inp->min_velocity);
cruckig_free(inp->min_acceleration);
cruckig_free(inp->per_dof_control_interface);
cruckig_free(inp->per_dof_synchronization);
/* Pro fields */
cruckig_free(inp->intermediate_positions);
cruckig_free(inp->per_section_max_velocity);
cruckig_free(inp->per_section_max_acceleration);
cruckig_free(inp->per_section_max_jerk);
cruckig_free(inp->per_section_min_velocity);
cruckig_free(inp->per_section_min_acceleration);
cruckig_free(inp->per_section_max_position);
cruckig_free(inp->per_section_min_position);
cruckig_free(inp->max_position);
cruckig_free(inp->min_position);
cruckig_free(inp->per_section_minimum_duration);
cruckig_free(inp);
}
void cruckig_input_set_intermediate_positions(CRuckigInputParameter *inp,
const double *positions,
size_t num_waypoints)
{
if (!inp) return;
const size_t dofs = inp->degrees_of_freedom;
cruckig_free(inp->intermediate_positions);
if (num_waypoints == 0 || !positions) {
inp->intermediate_positions = NULL;
inp->num_intermediate_waypoints = 0;
return;
}
size_t total = num_waypoints * dofs;
inp->intermediate_positions = (double*)cruckig_malloc(total * sizeof(double));
memcpy(inp->intermediate_positions, positions, total * sizeof(double));
inp->num_intermediate_waypoints = num_waypoints;
}
bool cruckig_input_validate(const CRuckigInputParameter *inp,
bool check_current_within_limits,
bool check_target_within_limits)
{
if (!inp) return false;
const size_t dofs = inp->degrees_of_freedom;
/* Waypoint-specific validation */
if (inp->num_intermediate_waypoints > 0) {
/* Waypoints require Position control interface */
if (inp->control_interface != CRuckigPosition) return false;
/* Waypoints incompatible with Discrete discretization */
if (inp->duration_discretization == CRuckigDiscrete) return false;
/* Waypoints incompatible with minimum_duration */
if (inp->has_minimum_duration) return false;
/* Infinite jerk not supported with waypoints */
for (size_t dof = 0; dof < dofs; ++dof) {
if (isinf(inp->max_jerk[dof])) return false;
if (isinf(inp->max_acceleration[dof])) return false;
}
}
for (size_t dof = 0; dof < dofs; ++dof) {
const double jMax = inp->max_jerk[dof];
if (isnan(jMax) || jMax < 0.0) return false;
const double aMax = inp->max_acceleration[dof];
if (isnan(aMax) || aMax < 0.0) return false;
const double aMin = inp->min_acceleration ? inp->min_acceleration[dof] : -aMax;
if (isnan(aMin) || aMin > 0.0) return false;
const double a0 = inp->current_acceleration[dof];
if (isnan(a0)) return false;
const double af = inp->target_acceleration[dof];
if (isnan(af)) return false;
if (check_current_within_limits) {
if (a0 > aMax) return false;
if (a0 < aMin) return false;
}
if (check_target_within_limits) {
if (af > aMax) return false;
if (af < aMin) return false;
}
const double v0 = inp->current_velocity[dof];
if (isnan(v0)) return false;
const double vf = inp->target_velocity[dof];
if (isnan(vf)) return false;
CRuckigControlInterface ci = inp->per_dof_control_interface
? inp->per_dof_control_interface[dof]
: inp->control_interface;
if (ci == CRuckigPosition) {
const double p0 = inp->current_position[dof];
if (isnan(p0)) return false;
const double pf = inp->target_position[dof];
if (isnan(pf)) return false;
const double vMax = inp->max_velocity[dof];
if (isnan(vMax) || vMax < 0.0) return false;
const double vMin = inp->min_velocity ? inp->min_velocity[dof] : -vMax;
if (isnan(vMin) || vMin > 0.0) return false;
if (check_current_within_limits) {
if (v0 > vMax) return false;
if (v0 < vMin) return false;
}
if (check_target_within_limits) {
if (vf > vMax) return false;
if (vf < vMin) return false;
}
if (check_current_within_limits) {
if (a0 > 0 && jMax > 0 && v_at_a_zero(v0, a0, jMax) > vMax)
return false;
if (a0 < 0 && jMax > 0 && v_at_a_zero(v0, a0, -jMax) < vMin)
return false;
}
if (check_target_within_limits) {
if (af < 0 && jMax > 0 && v_at_a_zero(vf, af, jMax) > vMax)
return false;
if (af > 0 && jMax > 0 && v_at_a_zero(vf, af, -jMax) < vMin)
return false;
}
}
}
return true;
}
bool cruckig_input_is_equal(const CRuckigInputParameter *a, const CRuckigInputParameter *b) {
if (!a || !b) return (a == b);
if (a->degrees_of_freedom != b->degrees_of_freedom) return false;
const size_t dofs = a->degrees_of_freedom;
const size_t dsz = dofs * sizeof(double);
if (memcmp(a->current_position, b->current_position, dsz) != 0) return false;
if (memcmp(a->current_velocity, b->current_velocity, dsz) != 0) return false;
if (memcmp(a->current_acceleration, b->current_acceleration, dsz) != 0) return false;
if (memcmp(a->target_position, b->target_position, dsz) != 0) return false;
if (memcmp(a->target_velocity, b->target_velocity, dsz) != 0) return false;
if (memcmp(a->target_acceleration, b->target_acceleration, dsz) != 0) return false;
if (memcmp(a->max_velocity, b->max_velocity, dsz) != 0) return false;
if (memcmp(a->max_acceleration, b->max_acceleration, dsz) != 0) return false;
if (memcmp(a->max_jerk, b->max_jerk, dsz) != 0) return false;
if (memcmp(a->enabled, b->enabled, dofs * sizeof(bool)) != 0) return false;
/* Compare optional min_velocity */
if ((a->min_velocity == NULL) != (b->min_velocity == NULL)) return false;
if (a->min_velocity && memcmp(a->min_velocity, b->min_velocity, dsz) != 0) return false;
/* Compare optional min_acceleration */
if ((a->min_acceleration == NULL) != (b->min_acceleration == NULL)) return false;
if (a->min_acceleration && memcmp(a->min_acceleration, b->min_acceleration, dsz) != 0) return false;
/* Compare optional per_dof_control_interface */
if ((a->per_dof_control_interface == NULL) != (b->per_dof_control_interface == NULL)) return false;
if (a->per_dof_control_interface &&
memcmp(a->per_dof_control_interface, b->per_dof_control_interface,
dofs * sizeof(CRuckigControlInterface)) != 0) return false;
/* Compare optional per_dof_synchronization */
if ((a->per_dof_synchronization == NULL) != (b->per_dof_synchronization == NULL)) return false;
if (a->per_dof_synchronization &&
memcmp(a->per_dof_synchronization, b->per_dof_synchronization,
dofs * sizeof(CRuckigSynchronization)) != 0) return false;
if (a->control_interface != b->control_interface) return false;
if (a->synchronization != b->synchronization) return false;
if (a->duration_discretization != b->duration_discretization) return false;
if (a->has_minimum_duration != b->has_minimum_duration) return false;
if (a->has_minimum_duration && a->minimum_duration != b->minimum_duration) return false;
/* Compare Pro fields */
if (a->num_intermediate_waypoints != b->num_intermediate_waypoints) return false;
if (a->num_intermediate_waypoints > 0) {
size_t wp_sz = a->num_intermediate_waypoints * dofs * sizeof(double);
if (memcmp(a->intermediate_positions, b->intermediate_positions, wp_sz) != 0) return false;
}
/* Compare position limits */
if ((a->max_position == NULL) != (b->max_position == NULL)) return false;
if (a->max_position && memcmp(a->max_position, b->max_position, dsz) != 0) return false;
if ((a->min_position == NULL) != (b->min_position == NULL)) return false;
if (a->min_position && memcmp(a->min_position, b->min_position, dsz) != 0) return false;
/* Compare per-section constraints */
size_t nsec = a->num_intermediate_waypoints + 1;
size_t sec_dsz = nsec * dofs * sizeof(double);
#define CMP_OPT_SEC(field) \
if ((a->field == NULL) != (b->field == NULL)) return false; \
if (a->field && memcmp(a->field, b->field, sec_dsz) != 0) return false;
CMP_OPT_SEC(per_section_max_velocity)
CMP_OPT_SEC(per_section_max_acceleration)
CMP_OPT_SEC(per_section_max_jerk)
CMP_OPT_SEC(per_section_min_velocity)
CMP_OPT_SEC(per_section_min_acceleration)
CMP_OPT_SEC(per_section_max_position)
CMP_OPT_SEC(per_section_min_position)
#undef CMP_OPT_SEC
if ((a->per_section_minimum_duration == NULL) != (b->per_section_minimum_duration == NULL)) return false;
if (a->per_section_minimum_duration &&
memcmp(a->per_section_minimum_duration, b->per_section_minimum_duration,
nsec * sizeof(double)) != 0) return false;
if (a->interrupt_calculation_duration != b->interrupt_calculation_duration) return false;
return true;
}
/* Helper to copy an optional flat array */
static void copy_opt_array(double **dst, const double *src, size_t count) {
if (src) {
size_t sz = count * sizeof(double);
if (!*dst) {
*dst = (double*)cruckig_malloc(sz);
}
memcpy(*dst, src, sz);
} else {
cruckig_free(*dst);
*dst = NULL;
}
}
void cruckig_input_copy(CRuckigInputParameter *dst, const CRuckigInputParameter *src) {
if (!dst || !src) return;
if (dst == src) return;
const size_t dofs = src->degrees_of_freedom;
const size_t dsz = dofs * sizeof(double);
/* dst must already be allocated with same dofs */
dst->degrees_of_freedom = dofs;
dst->control_interface = src->control_interface;
dst->synchronization = src->synchronization;
dst->duration_discretization = src->duration_discretization;
memcpy(dst->current_position, src->current_position, dsz);
memcpy(dst->current_velocity, src->current_velocity, dsz);
memcpy(dst->current_acceleration, src->current_acceleration, dsz);
memcpy(dst->target_position, src->target_position, dsz);
memcpy(dst->target_velocity, src->target_velocity, dsz);
memcpy(dst->target_acceleration, src->target_acceleration, dsz);
memcpy(dst->max_velocity, src->max_velocity, dsz);
memcpy(dst->max_acceleration, src->max_acceleration, dsz);
memcpy(dst->max_jerk, src->max_jerk, dsz);
memcpy(dst->enabled, src->enabled, dofs * sizeof(bool));
copy_opt_array(&dst->min_velocity, src->min_velocity, dofs);
copy_opt_array(&dst->min_acceleration, src->min_acceleration, dofs);
/* Handle optional per_dof_control_interface */
if (src->per_dof_control_interface) {
if (!dst->per_dof_control_interface) {
dst->per_dof_control_interface = (CRuckigControlInterface*)cruckig_malloc(dofs * sizeof(CRuckigControlInterface));
}
memcpy(dst->per_dof_control_interface, src->per_dof_control_interface,
dofs * sizeof(CRuckigControlInterface));
} else {
cruckig_free(dst->per_dof_control_interface);
dst->per_dof_control_interface = NULL;
}
/* Handle optional per_dof_synchronization */
if (src->per_dof_synchronization) {
if (!dst->per_dof_synchronization) {
dst->per_dof_synchronization = (CRuckigSynchronization*)cruckig_malloc(dofs * sizeof(CRuckigSynchronization));
}
memcpy(dst->per_dof_synchronization, src->per_dof_synchronization,
dofs * sizeof(CRuckigSynchronization));
} else {
cruckig_free(dst->per_dof_synchronization);
dst->per_dof_synchronization = NULL;
}
dst->minimum_duration = src->minimum_duration;
dst->has_minimum_duration = src->has_minimum_duration;
/* Copy Pro fields */
if (src->num_intermediate_waypoints > 0 && src->intermediate_positions) {
size_t wp_sz = src->num_intermediate_waypoints * dofs;
copy_opt_array(&dst->intermediate_positions, src->intermediate_positions, wp_sz);
dst->num_intermediate_waypoints = src->num_intermediate_waypoints;
} else {
cruckig_free(dst->intermediate_positions);
dst->intermediate_positions = NULL;
dst->num_intermediate_waypoints = 0;
}
copy_opt_array(&dst->max_position, src->max_position, dofs);
copy_opt_array(&dst->min_position, src->min_position, dofs);
/* Per-section arrays */
size_t nsec = src->num_intermediate_waypoints + 1;
size_t sec_count = nsec * dofs;
copy_opt_array(&dst->per_section_max_velocity, src->per_section_max_velocity, sec_count);
copy_opt_array(&dst->per_section_max_acceleration, src->per_section_max_acceleration, sec_count);
copy_opt_array(&dst->per_section_max_jerk, src->per_section_max_jerk, sec_count);
copy_opt_array(&dst->per_section_min_velocity, src->per_section_min_velocity, sec_count);
copy_opt_array(&dst->per_section_min_acceleration, src->per_section_min_acceleration, sec_count);
copy_opt_array(&dst->per_section_max_position, src->per_section_max_position, sec_count);
copy_opt_array(&dst->per_section_min_position, src->per_section_min_position, sec_count);
if (src->per_section_minimum_duration) {
copy_opt_array(&dst->per_section_minimum_duration, src->per_section_minimum_duration, nsec);
} else {
cruckig_free(dst->per_section_minimum_duration);
dst->per_section_minimum_duration = NULL;
}
dst->interrupt_calculation_duration = src->interrupt_calculation_duration;
}

94
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.h vendored Normal file
View File

@@ -0,0 +1,94 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_INPUT_PARAMETER_H
#define CRUCKIG_INPUT_PARAMETER_H
#include "cruckig_internal.h"
#include "result.h"
typedef struct {
size_t degrees_of_freedom;
CRuckigControlInterface control_interface;
CRuckigSynchronization synchronization;
CRuckigDurationDiscretization duration_discretization;
/* Current state */
double *current_position;
double *current_velocity;
double *current_acceleration;
/* Target state */
double *target_position;
double *target_velocity;
double *target_acceleration;
/* Kinematic constraints */
double *max_velocity;
double *max_acceleration;
double *max_jerk;
/* Optional min limits (NULL = use -max) */
double *min_velocity; /* NULL or array of dofs */
double *min_acceleration; /* NULL or array of dofs */
/* Per-DOF enable flags */
bool *enabled;
/* Optional per-DOF control interface / synchronization (NULL = use global) */
CRuckigControlInterface *per_dof_control_interface; /* NULL or array of dofs */
CRuckigSynchronization *per_dof_synchronization; /* NULL or array of dofs */
/* Optional minimum trajectory duration (-1 = not set) */
double minimum_duration;
bool has_minimum_duration;
/* ---- Pro features ---- */
/* Intermediate waypoints: flat array of num_waypoints * dofs doubles.
* Each waypoint is dofs consecutive doubles. NULL if no waypoints. */
double *intermediate_positions;
size_t num_intermediate_waypoints;
/* Per-section kinematic constraints: flat arrays of (num_waypoints+1) * dofs.
* Section i constraints at offset i*dofs. NULL = use global. */
double *per_section_max_velocity;
double *per_section_max_acceleration;
double *per_section_max_jerk;
double *per_section_min_velocity;
double *per_section_min_acceleration;
/* Per-section position limits: flat arrays of (num_waypoints+1) * dofs. */
double *per_section_max_position;
double *per_section_min_position;
/* Global position limits during trajectory (NULL = no limits) */
double *max_position; /* NULL or array of dofs */
double *min_position; /* NULL or array of dofs */
/* Per-section minimum duration: array of (num_waypoints+1). NULL = no constraint. */
double *per_section_minimum_duration;
/* Calculation interruption budget in microseconds. 0 = no interruption. */
double interrupt_calculation_duration;
} CRuckigInputParameter;
CRuckigInputParameter* cruckig_input_create(size_t dofs);
void cruckig_input_destroy(CRuckigInputParameter *inp);
bool cruckig_input_validate(const CRuckigInputParameter *inp,
bool check_current_within_limits,
bool check_target_within_limits);
bool cruckig_input_is_equal(const CRuckigInputParameter *a, const CRuckigInputParameter *b);
void cruckig_input_copy(CRuckigInputParameter *dst, const CRuckigInputParameter *src);
/* Set intermediate waypoints. Copies the data. positions is num_waypoints * dofs doubles. */
void cruckig_input_set_intermediate_positions(CRuckigInputParameter *inp,
const double *positions,
size_t num_waypoints);
#endif /* CRUCKIG_INPUT_PARAMETER_H */

104
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.c vendored Normal file
View File

@@ -0,0 +1,104 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "output_parameter.h"
CRuckigOutputParameter* cruckig_output_create(size_t dofs) {
CRuckigOutputParameter *out = (CRuckigOutputParameter*)cruckig_calloc(1, sizeof(CRuckigOutputParameter));
if (!out) return NULL;
out->degrees_of_freedom = dofs;
out->trajectory = cruckig_trajectory_create(dofs);
if (!out->trajectory) {
cruckig_free(out);
return NULL;
}
out->new_position = (double*)cruckig_calloc(dofs, sizeof(double));
out->new_velocity = (double*)cruckig_calloc(dofs, sizeof(double));
out->new_acceleration = (double*)cruckig_calloc(dofs, sizeof(double));
out->new_jerk = (double*)cruckig_calloc(dofs, sizeof(double));
if (!out->new_position || !out->new_velocity ||
!out->new_acceleration || !out->new_jerk) {
cruckig_output_destroy(out);
return NULL;
}
out->time = 0.0;
out->new_section = 0;
out->did_section_change = false;
out->new_calculation = false;
out->was_calculation_interrupted = false;
out->calculation_duration = 0.0;
return out;
}
void cruckig_output_destroy(CRuckigOutputParameter *out) {
if (!out) return;
cruckig_trajectory_destroy(out->trajectory);
cruckig_free(out->new_position);
cruckig_free(out->new_velocity);
cruckig_free(out->new_acceleration);
cruckig_free(out->new_jerk);
cruckig_free(out);
}
void cruckig_output_pass_to_input(const CRuckigOutputParameter *out, CRuckigInputParameter *inp) {
if (!out || !inp) return;
const size_t dofs = out->degrees_of_freedom;
const size_t dsz = dofs * sizeof(double);
memcpy(inp->current_position, out->new_position, dsz);
memcpy(inp->current_velocity, out->new_velocity, dsz);
memcpy(inp->current_acceleration, out->new_acceleration, dsz);
/* If section changed and we have intermediate waypoints, remove the first waypoint */
if (out->did_section_change && inp->num_intermediate_waypoints > 0) {
size_t remaining = inp->num_intermediate_waypoints - 1;
if (remaining == 0) {
cruckig_free(inp->intermediate_positions);
inp->intermediate_positions = NULL;
inp->num_intermediate_waypoints = 0;
} else {
/* Shift waypoints forward by one */
memmove(inp->intermediate_positions,
inp->intermediate_positions + dofs,
remaining * dofs * sizeof(double));
inp->num_intermediate_waypoints = remaining;
}
/* Also shift per-section constraints if present */
size_t old_nsec = remaining + 2; /* was num_waypoints+1 sections */
size_t new_nsec = remaining + 1;
#define SHIFT_PER_SEC(field) \
if (inp->field) { \
memmove(inp->field, inp->field + dofs, new_nsec * dofs * sizeof(double)); \
}
SHIFT_PER_SEC(per_section_max_velocity)
SHIFT_PER_SEC(per_section_max_acceleration)
SHIFT_PER_SEC(per_section_max_jerk)
SHIFT_PER_SEC(per_section_min_velocity)
SHIFT_PER_SEC(per_section_min_acceleration)
SHIFT_PER_SEC(per_section_max_position)
SHIFT_PER_SEC(per_section_min_position)
#undef SHIFT_PER_SEC
if (inp->per_section_minimum_duration) {
memmove(inp->per_section_minimum_duration,
inp->per_section_minimum_duration + 1,
new_nsec * sizeof(double));
}
(void)old_nsec;
}
}

37
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.h vendored Normal file
View File

@@ -0,0 +1,37 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_OUTPUT_PARAMETER_H
#define CRUCKIG_OUTPUT_PARAMETER_H
#include "cruckig_internal.h"
#include "trajectory.h"
#include "input_parameter.h"
typedef struct {
size_t degrees_of_freedom;
CRuckigTrajectory *trajectory;
double *new_position;
double *new_velocity;
double *new_acceleration;
double *new_jerk;
double time;
size_t new_section;
bool did_section_change;
bool new_calculation;
bool was_calculation_interrupted;
double calculation_duration; /* microseconds */
} CRuckigOutputParameter;
CRuckigOutputParameter* cruckig_output_create(size_t dofs);
void cruckig_output_destroy(CRuckigOutputParameter *out);
void cruckig_output_pass_to_input(const CRuckigOutputParameter *out, CRuckigInputParameter *inp);
#endif /* CRUCKIG_OUTPUT_PARAMETER_H */

103
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position.h vendored Normal file
View File

@@ -0,0 +1,103 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_POSITION_H
#define CRUCKIG_POSITION_H
#include "cruckig_internal.h"
#include "profile.h"
#include "block.h"
/* ---- Third Order Step 1 ---- */
typedef struct {
double v0, a0, vf, af;
double _vMax, _vMin, _aMax, _aMin, _jMax;
double pd;
double v0_v0, vf_vf;
double a0_a0, a0_p3, a0_p4;
double af_af, af_p3, af_p4;
double jMax_jMax;
CRuckigProfile valid_profiles[6];
} CRuckigPositionThirdOrderStep1;
void cruckig_pos3_step1_init(CRuckigPositionThirdOrderStep1 *s,
double p0, double v0, double a0,
double pf, double vf, double af,
double vMax, double vMin, double aMax, double aMin, double jMax);
bool cruckig_pos3_step1_get_profile(CRuckigPositionThirdOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block);
/* ---- Third Order Step 2 ---- */
typedef struct {
double v0, a0, tf, vf, af;
double _vMax, _vMin, _aMax, _aMin, _jMax;
double pd;
double tf_tf, tf_p3, tf_p4;
double vd, vd_vd;
double ad, ad_ad;
double v0_v0, vf_vf;
double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6;
double af_af, af_p3, af_p4, af_p5, af_p6;
double jMax_jMax;
double g1, g2;
} CRuckigPositionThirdOrderStep2;
void cruckig_pos3_step2_init(CRuckigPositionThirdOrderStep2 *s,
double tf, double p0, double v0, double a0,
double pf, double vf, double af,
double vMax, double vMin, double aMax, double aMin, double jMax);
bool cruckig_pos3_step2_get_profile(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile);
/* ---- Second Order Step 1 ---- */
typedef struct {
double v0, vf;
double _vMax, _vMin, _aMax, _aMin;
double pd;
CRuckigProfile valid_profiles[4];
} CRuckigPositionSecondOrderStep1;
void cruckig_pos2_step1_init(CRuckigPositionSecondOrderStep1 *s,
double p0, double v0, double pf, double vf,
double vMax, double vMin, double aMax, double aMin);
bool cruckig_pos2_step1_get_profile(CRuckigPositionSecondOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block);
/* ---- Second Order Step 2 ---- */
typedef struct {
double v0, tf, vf;
double _vMax, _vMin, _aMax, _aMin;
double pd, vd;
} CRuckigPositionSecondOrderStep2;
void cruckig_pos2_step2_init(CRuckigPositionSecondOrderStep2 *s,
double tf, double p0, double v0, double pf, double vf,
double vMax, double vMin, double aMax, double aMin);
bool cruckig_pos2_step2_get_profile(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile);
/* ---- First Order Step 1 ---- */
typedef struct {
double _vMax, _vMin;
double pd;
} CRuckigPositionFirstOrderStep1;
void cruckig_pos1_step1_init(CRuckigPositionFirstOrderStep1 *s,
double p0, double pf, double vMax, double vMin);
bool cruckig_pos1_step1_get_profile(CRuckigPositionFirstOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block);
/* ---- First Order Step 2 ---- */
typedef struct {
double tf;
double _vMax, _vMin;
double pd;
} CRuckigPositionFirstOrderStep2;
void cruckig_pos1_step2_init(CRuckigPositionFirstOrderStep2 *s,
double tf, double p0, double pf, double vMax, double vMin);
bool cruckig_pos1_step2_get_profile(CRuckigPositionFirstOrderStep2 *s, CRuckigProfile *profile);
#endif /* CRUCKIG_POSITION_H */

41
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step1.c vendored Normal file
View File

@@ -0,0 +1,41 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "position.h"
#include "block.h"
#include "profile.h"
void cruckig_pos1_step1_init(CRuckigPositionFirstOrderStep1 *s,
double p0, double pf, double vMax, double vMin)
{
s->_vMax = vMax;
s->_vMin = vMin;
s->pd = pf - p0;
}
bool cruckig_pos1_step1_get_profile(CRuckigPositionFirstOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block)
{
CRuckigProfile *p = &block->p_min;
cruckig_profile_set_boundary_from_profile(p, input);
const double vf = (s->pd > 0) ? s->_vMax : s->_vMin;
p->t[0] = 0;
p->t[1] = 0;
p->t[2] = 0;
p->t[3] = s->pd / vf;
p->t[4] = 0;
p->t[5] = 0;
p->t[6] = 0;
if (cruckig_profile_check_for_first_order(p, ControlSignsUDDU, ReachedLimitsVEL, vf)) {
block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration;
return true;
}
return false;
}

37
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step2.c vendored Normal file
View File

@@ -0,0 +1,37 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "position.h"
#include "block.h"
#include "profile.h"
#include "roots.h"
void cruckig_pos1_step2_init(CRuckigPositionFirstOrderStep2 *s,
double tf, double p0, double pf, double vMax, double vMin)
{
s->tf = tf;
s->_vMax = vMax;
s->_vMin = vMin;
s->pd = pf - p0;
}
bool cruckig_pos1_step2_get_profile(CRuckigPositionFirstOrderStep2 *s, CRuckigProfile *profile)
{
const double vf = s->pd / s->tf;
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = s->tf;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
return cruckig_profile_check_for_first_order_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE,
s->tf, vf, s->_vMax, s->_vMin);
}

179
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step1.c vendored Normal file
View File

@@ -0,0 +1,179 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "position.h"
#include "block.h"
#include "profile.h"
void cruckig_pos2_step1_init(CRuckigPositionSecondOrderStep1 *s,
double p0, double v0, double pf, double vf,
double vMax, double vMin, double aMax, double aMin)
{
s->v0 = v0;
s->vf = vf;
s->_vMax = vMax;
s->_vMin = vMin;
s->_aMax = aMax;
s->_aMin = aMin;
s->pd = pf - p0;
}
static void time_acc0(CRuckigPositionSecondOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, bool return_after_found)
{
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = (-s->v0 + vMax) / aMax;
profile->t[1] = (aMin * s->v0 * s->v0 - aMax * s->vf * s->vf) / (2 * aMax * aMin * vMax) + vMax * (aMax - aMin) / (2 * aMax * aMin) + s->pd / vMax;
profile->t[2] = (s->vf - vMax) / aMin;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsACC0, aMax, aMin, vMax, vMin)) {
++(*counter);
if (*counter < 4) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
}
(void)return_after_found;
}
static void time_none(CRuckigPositionSecondOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, bool return_after_found)
{
double h1 = (aMax * s->vf * s->vf - aMin * s->v0 * s->v0 - 2 * aMax * aMin * s->pd) / (aMax - aMin);
if (h1 >= 0.0) {
h1 = sqrt(h1);
/* Solution 1 */
{
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = -(s->v0 + h1) / aMax;
profile->t[1] = 0;
profile->t[2] = (s->vf + h1) / aMin;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, aMax, aMin, vMax, vMin)) {
++(*counter);
if (*counter < 4) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
if (return_after_found) {
return;
}
}
}
/* Solution 2 */
{
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = (-s->v0 + h1) / aMax;
profile->t[1] = 0;
profile->t[2] = (s->vf - h1) / aMin;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, aMax, aMin, vMax, vMin)) {
++(*counter);
if (*counter < 4) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
}
}
}
}
static bool time_all_single_step(CRuckigPositionSecondOrderStep1 *s,
CRuckigProfile *profile, double vMax, double vMin)
{
if (fabs(s->vf - s->v0) > DBL_EPSILON) {
return false;
}
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (fabs(s->v0) > DBL_EPSILON) {
profile->t[3] = s->pd / s->v0;
if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, 0.0, vMax, vMin)) {
return true;
}
} else if (fabs(s->pd) < DBL_EPSILON) {
if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, 0.0, vMax, vMin)) {
return true;
}
}
return false;
}
bool cruckig_pos2_step1_get_profile(CRuckigPositionSecondOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block)
{
/* Zero-limits special case */
if (s->_vMax == 0.0 && s->_vMin == 0.0) {
CRuckigProfile *p = &block->p_min;
cruckig_profile_set_boundary_from_profile(p, input);
if (time_all_single_step(s, p, s->_vMax, s->_vMin)) {
block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration;
if (fabs(s->v0) > DBL_EPSILON) {
block->a.valid = true;
block->a.left = block->t_min;
block->a.right = INFINITY;
}
return true;
}
return false;
}
size_t valid_profile_counter = 0;
cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input);
if (fabs(s->vf) < DBL_EPSILON) {
/* There is no blocked interval when vf==0, so return after first found profile */
const double vMax = (s->pd >= 0) ? s->_vMax : s->_vMin;
const double vMin = (s->pd >= 0) ? s->_vMin : s->_vMax;
const double aMax = (s->pd >= 0) ? s->_aMax : s->_aMin;
const double aMin = (s->pd >= 0) ? s->_aMin : s->_aMax;
time_none(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, true);
if (valid_profile_counter > 0) { goto return_block; }
time_none(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, true);
} else {
time_none(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, false);
time_none(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, false);
time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, false);
time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, false);
}
return_block:
return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 4);
}

146
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step2.c vendored Normal file
View File

@@ -0,0 +1,146 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "position.h"
#include "block.h"
#include "profile.h"
#include "roots.h"
void cruckig_pos2_step2_init(CRuckigPositionSecondOrderStep2 *s,
double tf, double p0, double v0, double pf, double vf,
double vMax, double vMin, double aMax, double aMin)
{
s->v0 = v0;
s->tf = tf;
s->vf = vf;
s->_vMax = vMax;
s->_vMin = vMin;
s->_aMax = aMax;
s->_aMin = aMin;
s->pd = pf - p0;
s->vd = vf - v0;
}
static bool time_acc0(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile,
double vMax, double vMin, double aMax, double aMin)
{
/* UD Solution 1/2 */
{
const double h1 = sqrt((2 * aMax * (s->pd - s->tf * s->vf) - 2 * aMin * (s->pd - s->tf * s->v0) + s->vd * s->vd) / (aMax * aMin) + s->tf * s->tf);
profile->t[0] = (aMax * s->vd - aMax * aMin * (s->tf - h1)) / (aMax * (aMax - aMin));
profile->t[1] = h1;
profile->t[2] = s->tf - (profile->t[0] + h1);
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UU Solution */
{
const double h1 = (-s->vd + aMax * s->tf);
profile->t[0] = -s->vd * s->vd / (2 * aMax * h1) + (s->pd - s->v0 * s->tf) / h1;
profile->t[1] = -s->vd / aMax + s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = s->tf - (profile->t[0] + profile->t[1]);
if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UU Solution - 2 step */
{
profile->t[0] = 0;
profile->t[1] = -s->vd / aMax + s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = s->vd / aMax;
if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) {
profile->pf = profile->p[7];
return true;
}
}
return false;
}
static bool time_none(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile,
double vMax, double vMin, double aMax, double aMin)
{
if (fabs(s->v0) < DBL_EPSILON && fabs(s->vf) < DBL_EPSILON && fabs(s->pd) < DBL_EPSILON) {
profile->t[0] = 0;
profile->t[1] = s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, aMax, aMin, vMax, vMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UD Solution 1/2 */
{
const double h1 = 2 * (s->vf * s->tf - s->pd);
profile->t[0] = h1 / s->vd;
profile->t[1] = s->tf - profile->t[0];
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
const double af = s->vd * s->vd / h1;
if ((aMin - 1e-12 < af) && (af < aMax + 1e-12) &&
cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, af, -af, vMax, vMin)) {
profile->pf = profile->p[7];
return true;
}
}
return false;
}
static bool check_all(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile,
double vMax, double vMin, double aMax, double aMin)
{
return time_acc0(s, profile, vMax, vMin, aMax, aMin) ||
time_none(s, profile, vMax, vMin, aMax, aMin);
}
bool cruckig_pos2_step2_get_profile(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile)
{
/* Test all cases to get ones that match */
if (s->pd > 0) {
return check_all(s, profile, s->_vMax, s->_vMin, s->_aMax, s->_aMin) ||
check_all(s, profile, s->_vMin, s->_vMax, s->_aMin, s->_aMax);
}
return check_all(s, profile, s->_vMin, s->_vMax, s->_aMin, s->_aMax) ||
check_all(s, profile, s->_vMax, s->_vMin, s->_aMax, s->_aMin);
}

705
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step1.c vendored Normal file
View File

@@ -0,0 +1,705 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "position.h"
#include "block.h"
#include "profile.h"
#include "roots.h"
void cruckig_pos3_step1_init(CRuckigPositionThirdOrderStep1 *s,
double p0, double v0, double a0,
double pf, double vf, double af,
double vMax, double vMin, double aMax, double aMin, double jMax)
{
s->v0 = v0;
s->a0 = a0;
s->vf = vf;
s->af = af;
s->_vMax = vMax;
s->_vMin = vMin;
s->_aMax = aMax;
s->_aMin = aMin;
s->_jMax = jMax;
s->pd = pf - p0;
s->v0_v0 = v0 * v0;
s->vf_vf = vf * vf;
s->a0_a0 = a0 * a0;
s->af_af = af * af;
s->a0_p3 = a0 * s->a0_a0;
s->a0_p4 = s->a0_a0 * s->a0_a0;
s->af_p3 = af * s->af_af;
s->af_p4 = s->af_af * s->af_af;
s->jMax_jMax = jMax * jMax;
}
/* Helper: add_profile equivalent - increment counter, copy boundary to next */
static inline void add_profile(CRuckigProfile *valid_profiles, size_t *counter, size_t max_profiles)
{
const size_t prev = *counter;
++(*counter);
if (*counter < max_profiles) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], &valid_profiles[prev]);
}
}
static void time_all_vel(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax,
bool return_after_found)
{
CRuckigProfile *profile = &valid_profiles[*counter];
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3;
const double a0_p4 = s->a0_p4, af_p4 = s->af_p4;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
(void)return_after_found;
/* ACC0_ACC1_VEL */
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = (a0_a0 / 2 - aMax * aMax - jMax * (v0 - vMax)) / (aMax * jMax);
profile->t[2] = aMax / jMax;
profile->t[3] = (3 * (a0_p4 * aMin - af_p4 * aMax) + 8 * aMax * aMin * (af_p3 - a0_p3 + 3 * jMax * (a0 * v0 - af * vf)) + 6 * a0_a0 * aMin * (aMax * aMax - 2 * jMax * v0) - 6 * af_af * aMax * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (aMax * aMin * (aMax * (v0 + vMax) - aMin * (vf + vMax) - 2 * jMax * pd) + (aMin - aMax) * jMax * vMax * vMax + jMax * (aMax * vf_vf - aMin * v0_v0))) / (24 * aMax * aMin * jMax_jMax * vMax);
profile->t[4] = -aMin / jMax;
profile->t[5] = -(af_af / 2 - aMin * aMin - jMax * (vf - vMax)) / (aMin * jMax);
profile->t[6] = profile->t[4] + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
/* ACC1_VEL */
{
const double t_acc0 = sqrt(a0_a0 / (2 * jMax_jMax) + (vMax - v0) / jMax);
profile->t[0] = t_acc0 - a0 / jMax;
profile->t[1] = 0;
profile->t[2] = t_acc0;
profile->t[3] = -(3 * af_p4 - 8 * aMin * (af_p3 - a0_p3) - 24 * aMin * jMax * (a0 * v0 - af * vf) + 6 * af_af * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (2 * aMin * jMax * pd + aMin * aMin * (vf + vMax) + jMax * (vMax * vMax - vf_vf) + aMin * t_acc0 * (a0_a0 - 2 * jMax * (v0 + vMax)))) / (24 * aMin * jMax_jMax * vMax);
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* ACC0_VEL */
{
const double t_acc1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax);
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = (a0_a0 / 2 - aMax * aMax - jMax * (v0 - vMax)) / (aMax * jMax);
profile->t[2] = aMax / jMax;
profile->t[3] = (3 * a0_p4 + 8 * aMax * (af_p3 - a0_p3) + 24 * aMax * jMax * (a0 * v0 - af * vf) + 6 * a0_a0 * (aMax * aMax - 2 * jMax * v0) - 12 * jMax * (-2 * aMax * jMax * pd + aMax * aMax * (v0 + vMax) + jMax * (vMax * vMax - v0_v0) + aMax * t_acc1 * (-af_af + 2 * (vf + vMax) * jMax))) / (24 * aMax * jMax_jMax * vMax);
profile->t[4] = t_acc1;
profile->t[5] = 0;
profile->t[6] = t_acc1 + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_VEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* VEL */
{
const double t_acc0 = sqrt(a0_a0 / (2 * jMax_jMax) + (vMax - v0) / jMax);
const double t_acc1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax);
/* Solution 3/4 */
profile->t[0] = t_acc0 - a0 / jMax;
profile->t[1] = 0;
profile->t[2] = t_acc0;
profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * t_acc1 + a0_a0 * t_acc0) / 2) / (jMax * vMax) - (v0 / vMax + 1.0) * t_acc0 - (vf / vMax + 1.0) * t_acc1 + pd / vMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
}
}
}
static void time_acc0_acc1(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax,
bool return_after_found)
{
CRuckigProfile *profile = &valid_profiles[*counter];
const double a0 = s->a0, af = s->af;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3;
const double a0_p4 = s->a0_p4, af_p4 = s->af_p4;
const double v0 = s->v0, vf = s->vf;
const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
double h1 = (3 * (af_p4 * aMax - a0_p4 * aMin) + aMax * aMin * (8 * (a0_p3 - af_p3) + 3 * aMax * aMin * (aMax - aMin) + 6 * aMin * af_af - 6 * aMax * a0_a0) + 12 * jMax * (aMax * aMin * ((aMax - 2 * a0) * v0 - (aMin - 2 * af) * vf) + aMin * a0_a0 * v0 - aMax * af_af * vf)) / (3 * (aMax - aMin) * jMax_jMax) + 4 * (aMax * vf_vf - aMin * v0_v0 - 2 * aMin * aMax * pd) / (aMax - aMin);
if (h1 >= 0) {
h1 = sqrt(h1) / 2;
const double h2 = a0_a0 / (2 * aMax * jMax) + (aMin - 2 * aMax) / (2 * jMax) - v0 / aMax;
const double h3 = -af_af / (2 * aMin * jMax) - (aMax - 2 * aMin) / (2 * jMax) + vf / aMin;
/* UDDU: Solution 2 */
if (h2 > h1 / aMax && h3 > -h1 / aMin) {
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = h2 - h1 / aMax;
profile->t[2] = aMax / jMax;
profile->t[3] = 0;
profile->t[4] = -aMin / jMax;
profile->t[5] = h3 + h1 / aMin;
profile->t[6] = profile->t[4] + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, true, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
if (return_after_found) {
return;
}
}
}
/* UDDU: Solution 1 */
profile = &valid_profiles[*counter];
if (h2 > -h1 / aMax && h3 > h1 / aMin) {
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = h2 + h1 / aMax;
profile->t[2] = aMax / jMax;
profile->t[3] = 0;
profile->t[4] = -aMin / jMax;
profile->t[5] = h3 - h1 / aMin;
profile->t[6] = profile->t[4] + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, true, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
}
}
}
}
static void time_all_none_acc0_acc1(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax,
bool return_after_found)
{
CRuckigProfile *profile = &valid_profiles[*counter];
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3;
const double a0_p4 = s->a0_p4, af_p4 = s->af_p4;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
/* NONE UDDU / UDUD Strategy */
const double h2_none = (a0_a0 - af_af) / (2 * jMax) + (vf - v0);
const double h2_h2 = h2_none * h2_none;
const double t_min_none = (a0 - af) / jMax;
const double t_max_none = (aMax - aMin) / jMax;
double polynom_none[4];
polynom_none[0] = 0;
polynom_none[1] = -2 * (a0_a0 + af_af - 2 * jMax * (v0 + vf)) / jMax_jMax;
polynom_none[2] = 4 * (a0_p3 - af_p3 + 3 * jMax * (af * vf - a0 * v0)) / (3 * jMax * jMax_jMax) - 4 * pd / jMax;
polynom_none[3] = -h2_h2 / jMax_jMax;
/* ACC0 */
const double h3_acc0 = (a0_a0 - af_af) / (2 * aMax * jMax) + (vf - v0) / aMax;
const double t_min_acc0 = (aMax - af) / jMax;
const double t_max_acc0 = (aMax - aMin) / jMax;
const double h0_acc0 = 3 * (af_p4 - a0_p4) + 8 * (a0_p3 - af_p3) * aMax + 24 * aMax * jMax * (af * vf - a0 * v0) - 6 * a0_a0 * (aMax * aMax - 2 * jMax * v0) + 6 * af_af * (aMax * aMax - 2 * jMax * vf) + 12 * jMax * (jMax * (vf_vf - v0_v0 - 2 * aMax * pd) - aMax * aMax * (vf - v0));
const double h2_acc0 = -af_af + aMax * aMax + 2 * jMax * vf;
double polynom_acc0[4];
polynom_acc0[0] = -2 * aMax / jMax;
polynom_acc0[1] = h2_acc0 / jMax_jMax;
polynom_acc0[2] = 0;
polynom_acc0[3] = h0_acc0 / (12 * jMax_jMax * jMax_jMax);
/* ACC1 */
const double h3_acc1 = -(a0_a0 + af_af) / (2 * jMax * aMin) + aMin / jMax + (vf - v0) / aMin;
const double t_min_acc1 = (aMin - a0) / jMax;
const double t_max_acc1 = (aMax - a0) / jMax;
const double h0_acc1 = (a0_p4 - af_p4) / 4 + 2 * (af_p3 - a0_p3) * aMin / 3 + (a0_a0 - af_af) * aMin * aMin / 2 + jMax * (af_af * vf + a0_a0 * v0 + 2 * aMin * (jMax * pd - a0 * v0 - af * vf) + aMin * aMin * (v0 + vf) + jMax * (v0_v0 - vf_vf));
const double h2_acc1 = a0_a0 - a0 * aMin + 2 * jMax * v0;
double polynom_acc1[4];
polynom_acc1[0] = 2 * (2 * a0 - aMin) / jMax;
polynom_acc1[1] = (5 * a0_a0 + aMin * (aMin - 6 * a0) + 2 * jMax * v0) / jMax_jMax;
polynom_acc1[2] = 2 * (a0 - aMin) * h2_acc1 / (jMax_jMax * jMax);
polynom_acc1[3] = h0_acc1 / (jMax_jMax * jMax_jMax);
CRuckigRootSet roots_none = cruckig_roots_solve_quart_monic(polynom_none[0], polynom_none[1], polynom_none[2], polynom_none[3]);
CRuckigRootSet roots_acc0 = cruckig_roots_solve_quart_monic(polynom_acc0[0], polynom_acc0[1], polynom_acc0[2], polynom_acc0[3]);
CRuckigRootSet roots_acc1 = cruckig_roots_solve_quart_monic(polynom_acc1[0], polynom_acc1[1], polynom_acc1[2], polynom_acc1[3]);
cruckig_root_set_sort(&roots_none);
cruckig_root_set_sort(&roots_acc0);
cruckig_root_set_sort(&roots_acc1);
for (size_t i = 0; i < roots_none.size; ++i) {
double t = roots_none.data[i];
if (t < t_min_none || t > t_max_none) {
continue;
}
/* Single Newton-step (regarding pd) */
if (t > DBL_EPSILON) {
const double h1 = jMax * t * t;
const double orig = -h2_h2 / (4 * jMax * t) + h2_none * (af / jMax + t) + (4 * a0_p3 + 2 * af_p3 - 6 * a0_a0 * (af + 2 * jMax * t) + 12 * (af - a0) * jMax * v0 + 3 * jMax_jMax * (-4 * pd + (h1 + 8 * v0) * t)) / (12 * jMax_jMax);
const double deriv = h2_none + 2 * v0 - a0_a0 / jMax + h2_h2 / (4 * h1) + (3 * h1) / 4;
t -= orig / deriv;
}
const double h0 = h2_none / (2 * jMax * t);
profile = &valid_profiles[*counter];
profile->t[0] = h0 + t / 2 - a0 / jMax;
profile->t[1] = 0;
profile->t[2] = t;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = -h0 + t / 2 + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
if (return_after_found) {
return;
}
}
}
for (size_t i = 0; i < roots_acc0.size; ++i) {
double t = roots_acc0.data[i];
if (t < t_min_acc0 || t > t_max_acc0) {
continue;
}
/* Single Newton step (regarding pd) */
if (t > DBL_EPSILON) {
const double h1 = jMax * t;
const double orig = h0_acc0 / (12 * jMax_jMax * t) + t * (h2_acc0 + h1 * (h1 - 2 * aMax));
const double deriv = 2 * (h2_acc0 + h1 * (2 * h1 - 3 * aMax));
t -= orig / deriv;
}
profile = &valid_profiles[*counter];
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = h3_acc0 - 2 * t + jMax / aMax * t * t;
profile->t[2] = t;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = (af - aMax) / jMax + t;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
if (return_after_found) {
return;
}
}
}
for (size_t i = 0; i < roots_acc1.size; ++i) {
double t = roots_acc1.data[i];
if (t < t_min_acc1 || t > t_max_acc1) {
continue;
}
/* Double Newton step (regarding pd) */
if (t > DBL_EPSILON) {
const double h5 = a0_p3 + 2 * jMax * a0 * v0;
double h1 = jMax * t;
double orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax;
double deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1));
{
double correction = orig / deriv;
if (correction > t) correction = t;
t -= correction;
}
h1 = jMax * t;
orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax;
if (fabs(orig) > 1e-9) {
deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1));
t -= orig / deriv;
h1 = jMax * t;
orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax;
if (fabs(orig) > 1e-9) {
deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1));
t -= orig / deriv;
}
}
}
profile = &valid_profiles[*counter];
profile->t[0] = t;
profile->t[1] = 0;
profile->t[2] = (a0 - aMin) / jMax + t;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = h3_acc1 - (2 * a0 + jMax * t) * t / aMin;
profile->t[6] = (af - aMin) / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1, true, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
if (return_after_found) {
return;
}
}
}
}
static void time_acc1_vel_two_step(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax)
{
CRuckigProfile *profile = &valid_profiles[*counter];
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double vf_vf = s->vf_vf;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3, af_p4 = s->af_p4;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = a0 / jMax;
profile->t[3] = -(3 * af_p4 - 8 * aMin * (af_p3 - a0_p3) - 24 * aMin * jMax * (a0 * v0 - af * vf) + 6 * af_af * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (2 * aMin * jMax * pd + aMin * aMin * (vf + vMax) + jMax * (vMax * vMax - vf_vf) + aMin * a0 * (a0_a0 - 2 * jMax * (v0 + vMax)) / jMax)) / (24 * aMin * jMax_jMax * vMax);
profile->t[4] = -aMin / jMax;
profile->t[5] = -(af_af / 2 - aMin * aMin + jMax * (vMax - vf)) / (aMin * jMax);
profile->t[6] = profile->t[4] + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
}
}
static void time_acc0_two_step(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax)
{
CRuckigProfile *profile = &valid_profiles[*counter];
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3;
const double a0_p4 = s->a0_p4, af_p4 = s->af_p4;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
/* Two step */
{
profile->t[0] = 0;
profile->t[1] = (af_af - a0_a0 + 2 * jMax * (vf - v0)) / (2 * a0 * jMax);
profile->t[2] = (a0 - af) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* Three step - Removed pf */
{
profile = &valid_profiles[*counter];
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = (a0_a0 + af_af - 2 * aMax * aMax + 2 * jMax * (vf - v0)) / (2 * aMax * jMax);
profile->t[2] = (-af + aMax) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* Three step - Removed aMax */
{
profile = &valid_profiles[*counter];
const double h0 = 3 * (af_af - a0_a0 + 2 * jMax * (v0 + vf));
const double h2 = a0_p3 + 2 * af_p3 + 6 * jMax_jMax * pd + 6 * (af - a0) * jMax * vf - 3 * a0 * af_af;
const double h1_sq = 2 * (2 * h2 * h2 + h0 * (a0_p4 - 6 * a0_a0 * (af_af + 2 * jMax * vf) + 8 * a0 * (af_p3 + 3 * jMax_jMax * pd + 3 * af * jMax * vf) - 3 * (af_p4 + 4 * af_af * jMax * vf + 4 * jMax_jMax * (vf_vf - v0_v0))));
const double h1 = sqrt(h1_sq) * fabs(jMax) / jMax;
profile->t[0] = (4 * af_p3 + 2 * a0_p3 - 6 * a0 * af_af + 12 * jMax_jMax * pd + 12 * (af - a0) * jMax * vf + h1) / (2 * jMax * h0);
profile->t[1] = -h1 / (jMax * h0);
profile->t[2] = (-4 * a0_p3 - 2 * af_p3 + 6 * a0_a0 * af + 12 * jMax_jMax * pd - 12 * (af - a0) * jMax * v0 + h1) / (2 * jMax * h0);
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* Three step - t=(aMax - aMin)/jMax */
{
profile = &valid_profiles[*counter];
const double t = (aMax - aMin) / jMax;
profile->t[0] = (-a0 + aMax) / jMax;
profile->t[1] = (a0_a0 - af_af) / (2 * aMax * jMax) + (vf - v0 + jMax * t * t) / aMax - 2 * t;
profile->t[2] = t;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = (af - aMin) / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
}
static void time_vel_two_step(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax)
{
CRuckigProfile *profile;
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double af_af = s->af_af;
const double a0_p3 = s->a0_p3, af_p3 = s->af_p3;
const double jMax_jMax = s->jMax_jMax;
const double pd = s->pd;
const double h1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax);
/* Four step - Solution 3/4 */
{
profile = &valid_profiles[*counter];
profile->t[0] = -a0 / jMax;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * h1) / 2) / (jMax * vMax) - (vf / vMax + 1.0) * h1 + pd / vMax;
profile->t[4] = h1;
profile->t[5] = 0;
profile->t[6] = h1 + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* Four step */
{
profile = &valid_profiles[*counter];
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = a0 / jMax;
profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * h1 + a0_p3 / jMax) / 2) / (jMax * vMax) - (v0 / vMax + 1.0) * a0 / jMax - (vf / vMax + 1.0) * h1 + pd / vMax;
profile->t[4] = h1;
profile->t[5] = 0;
profile->t[6] = h1 + af / jMax;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
}
static void time_none_two_step(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double vMax, double vMin, double aMax, double aMin, double jMax)
{
CRuckigProfile *profile;
const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af;
const double a0_a0 = s->a0_a0, af_af = s->af_af;
/* Two step */
{
profile = &valid_profiles[*counter];
const double h0 = sqrt((a0_a0 + af_af) / 2 + jMax * (vf - v0)) * fabs(jMax) / jMax;
profile->t[0] = (h0 - a0) / jMax;
profile->t[1] = 0;
profile->t[2] = (h0 - af) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
/* Single step */
{
profile = &valid_profiles[*counter];
profile->t[0] = (af - a0) / jMax;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) {
add_profile(valid_profiles, counter, 6);
return;
}
}
}
static bool time_all_single_step(CRuckigPositionThirdOrderStep1 *s,
CRuckigProfile *profile, double vMax, double vMin, double aMax, double aMin)
{
const double v0 = s->v0, a0 = s->a0, af = s->af;
const double v0_v0 = s->v0_v0;
const double pd = s->pd;
if (fabs(af - a0) > DBL_EPSILON) {
return false;
}
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (fabs(a0) > DBL_EPSILON) {
const double q = sqrt(2 * a0 * pd + v0_v0);
/* Solution 1 */
profile->t[3] = (-v0 + q) / a0;
if (profile->t[3] >= 0.0 && cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) {
return true;
}
/* Solution 2 */
profile->t[3] = -(v0 + q) / a0;
if (profile->t[3] >= 0.0 && cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) {
return true;
}
} else if (fabs(v0) > DBL_EPSILON) {
profile->t[3] = pd / v0;
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) {
return true;
}
} else if (fabs(pd) < DBL_EPSILON) {
if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) {
return true;
}
}
return false;
}
CRUCKIG_HOT
bool cruckig_pos3_step1_get_profile(CRuckigPositionThirdOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block)
{
/* Zero-limits special case */
if (s->_jMax == 0.0 || s->_aMax == 0.0 || s->_aMin == 0.0) {
CRuckigProfile *p = &block->p_min;
cruckig_profile_set_boundary_from_profile(p, input);
if (time_all_single_step(s, p, s->_vMax, s->_vMin, s->_aMax, s->_aMin)) {
block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration;
if (fabs(s->v0) > DBL_EPSILON || fabs(s->a0) > DBL_EPSILON) {
block->a.valid = true;
block->a.left = block->t_min;
block->a.right = INFINITY;
}
return true;
}
return false;
}
size_t valid_profile_counter = 0;
cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input);
if (fabs(s->vf) < DBL_EPSILON && fabs(s->af) < DBL_EPSILON) {
const double vMax = (s->pd >= 0) ? s->_vMax : s->_vMin;
const double vMin = (s->pd >= 0) ? s->_vMin : s->_vMax;
const double aMax = (s->pd >= 0) ? s->_aMax : s->_aMin;
const double aMin = (s->pd >= 0) ? s->_aMin : s->_aMax;
const double jMax = (s->pd >= 0) ? s->_jMax : -s->_jMax;
if (fabs(s->v0) < DBL_EPSILON && fabs(s->a0) < DBL_EPSILON && fabs(s->pd) < DBL_EPSILON) {
time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true);
} else {
/* There is no blocked interval when vf==0 && af==0, so return after first found profile */
time_all_vel(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_all_vel(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true);
}
} else {
time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false);
time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false);
time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false);
time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false);
time_all_vel(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false);
time_all_vel(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false);
}
if (valid_profile_counter == 0) {
time_none_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_none_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_acc1_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax);
if (valid_profile_counter > 0) { goto return_block; }
time_acc1_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax);
}
return_block:
return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 6);
}

1370
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step2.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

539
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.c vendored Normal file
View File

@@ -0,0 +1,539 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "profile.h"
#include "roots.h"
#include "utils.h"
void cruckig_profile_init(CRuckigProfile *p) {
memset(p->t, 0, sizeof(p->t));
memset(p->t_sum, 0, sizeof(p->t_sum));
memset(p->j, 0, sizeof(p->j));
memset(p->a, 0, sizeof(p->a));
memset(p->v, 0, sizeof(p->v));
memset(p->p, 0, sizeof(p->p));
cruckig_brake_init(&p->brake);
cruckig_brake_init(&p->accel);
p->pf = 0.0;
p->vf = 0.0;
p->af = 0.0;
p->limits = ReachedLimitsNONE;
p->direction = DirectionUP;
p->control_signs = ControlSignsUDDU;
}
void cruckig_profile_set_boundary(CRuckigProfile *p, double p0, double v0, double a0,
double pf, double vf, double af) {
p->a[0] = a0;
p->v[0] = v0;
p->p[0] = p0;
p->af = af;
p->vf = vf;
p->pf = pf;
}
void cruckig_profile_set_boundary_from_profile(CRuckigProfile *p, const CRuckigProfile *src) {
p->a[0] = src->a[0];
p->v[0] = src->v[0];
p->p[0] = src->p[0];
p->af = src->af;
p->vf = src->vf;
p->pf = src->pf;
p->brake = src->brake;
p->accel = src->accel;
}
void cruckig_profile_set_boundary_for_velocity(CRuckigProfile *p, double p0, double v0, double a0,
double vf, double af) {
p->a[0] = a0;
p->v[0] = v0;
p->p[0] = p0;
p->af = af;
p->vf = vf;
}
/* Third-order position check */
CRUCKIG_HOT
bool cruckig_profile_check(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
bool set_limits, double jf, double vMax, double vMin, double aMax, double aMin) {
if (CRUCKIG_UNLIKELY(p->t[0] < 0)) {
return false;
}
p->t_sum[0] = p->t[0];
for (size_t i = 0; i < 6; ++i) {
if (CRUCKIG_UNLIKELY(p->t[i + 1] < 0)) {
return false;
}
p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1];
}
if (lim == ReachedLimitsACC0_ACC1_VEL || lim == ReachedLimitsACC0_VEL || lim == ReachedLimitsACC1_VEL || lim == ReachedLimitsVEL) {
if (CRUCKIG_UNLIKELY(p->t[3] < DBL_EPSILON)) {
return false;
}
}
if (lim == ReachedLimitsACC0 || lim == ReachedLimitsACC0_ACC1) {
if (CRUCKIG_UNLIKELY(p->t[1] < DBL_EPSILON)) {
return false;
}
}
if (lim == ReachedLimitsACC1 || lim == ReachedLimitsACC0_ACC1) {
if (CRUCKIG_UNLIKELY(p->t[5] < DBL_EPSILON)) {
return false;
}
}
if (CRUCKIG_UNLIKELY(p->t_sum[6] > PROFILE_T_MAX)) {
return false;
}
if (cs == ControlSignsUDDU) {
p->j[0] = (p->t[0] > 0 ? jf : 0);
p->j[1] = 0;
p->j[2] = (p->t[2] > 0 ? -jf : 0);
p->j[3] = 0;
p->j[4] = (p->t[4] > 0 ? -jf : 0);
p->j[5] = 0;
p->j[6] = (p->t[6] > 0 ? jf : 0);
} else {
p->j[0] = (p->t[0] > 0 ? jf : 0);
p->j[1] = 0;
p->j[2] = (p->t[2] > 0 ? -jf : 0);
p->j[3] = 0;
p->j[4] = (p->t[4] > 0 ? jf : 0);
p->j[5] = 0;
p->j[6] = (p->t[6] > 0 ? -jf : 0);
}
p->direction = (vMax > 0) ? DirectionUP : DirectionDOWN;
const double vUppLim = (p->direction == DirectionUP ? vMax : vMin) + PROFILE_V_EPS;
const double vLowLim = (p->direction == DirectionUP ? vMin : vMax) - PROFILE_V_EPS;
for (size_t i = 0; i < 7; ++i) {
p->a[i + 1] = p->a[i] + p->t[i] * p->j[i];
p->v[i + 1] = p->v[i] + p->t[i] * (p->a[i] + p->t[i] * p->j[i] / 2);
p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * (p->a[i] / 2 + p->t[i] * p->j[i] / 6));
if (lim == ReachedLimitsACC0_ACC1_VEL || lim == ReachedLimitsACC0_ACC1 || lim == ReachedLimitsACC0_VEL || lim == ReachedLimitsACC1_VEL || lim == ReachedLimitsVEL) {
if (i == 2) {
p->a[3] = 0.0;
}
}
if (set_limits) {
if (lim == ReachedLimitsACC1) {
if (i == 2) {
p->a[3] = aMin;
}
}
if (lim == ReachedLimitsACC0_ACC1) {
if (i == 0) {
p->a[1] = aMax;
}
if (i == 4) {
p->a[5] = aMin;
}
}
}
if (i > 1 && p->a[i + 1] * p->a[i] < -DBL_EPSILON) {
const double v_a_zero = p->v[i] - (p->a[i] * p->a[i]) / (2 * p->j[i]);
if (v_a_zero > vUppLim || v_a_zero < vLowLim) {
return false;
}
}
}
p->control_signs = cs;
p->limits = lim;
const double aUppLim = (p->direction == DirectionUP ? aMax : aMin) + PROFILE_A_EPS;
const double aLowLim = (p->direction == DirectionUP ? aMin : aMax) - PROFILE_A_EPS;
return fabs(p->p[7] - p->pf) < PROFILE_P_PREC && fabs(p->v[7] - p->vf) < PROFILE_V_PREC && fabs(p->a[7] - p->af) < PROFILE_A_PREC
&& p->a[1] >= aLowLim && p->a[3] >= aLowLim && p->a[5] >= aLowLim
&& p->a[1] <= aUppLim && p->a[3] <= aUppLim && p->a[5] <= aUppLim
&& p->v[3] <= vUppLim && p->v[4] <= vUppLim && p->v[5] <= vUppLim && p->v[6] <= vUppLim
&& p->v[3] >= vLowLim && p->v[4] >= vLowLim && p->v[5] >= vLowLim && p->v[6] >= vLowLim;
}
bool cruckig_profile_check_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double vMax, double vMin, double aMax, double aMin) {
(void)tf;
/* Time doesn't need to be checked as every profile has a: tf - ... equation */
return cruckig_profile_check(p, cs, lim, false, jf, vMax, vMin, aMax, aMin);
}
bool cruckig_profile_check_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double vMax, double vMin, double aMax, double aMin, double jMax) {
return (fabs(jf) < fabs(jMax) + PROFILE_J_EPS) && cruckig_profile_check_with_timing(p, cs, lim, tf, jf, vMax, vMin, aMax, aMin);
}
/* Third-order velocity check */
CRUCKIG_HOT
bool cruckig_profile_check_for_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double jf, double aMax, double aMin) {
if (CRUCKIG_UNLIKELY(p->t[0] < 0)) {
return false;
}
p->t_sum[0] = p->t[0];
for (size_t i = 0; i < 6; ++i) {
if (p->t[i + 1] < 0) {
return false;
}
p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1];
}
if (lim == ReachedLimitsACC0) {
if (p->t[1] < DBL_EPSILON) {
return false;
}
}
if (p->t_sum[6] > PROFILE_T_MAX) {
return false;
}
if (cs == ControlSignsUDDU) {
p->j[0] = (p->t[0] > 0 ? jf : 0);
p->j[1] = 0;
p->j[2] = (p->t[2] > 0 ? -jf : 0);
p->j[3] = 0;
p->j[4] = (p->t[4] > 0 ? -jf : 0);
p->j[5] = 0;
p->j[6] = (p->t[6] > 0 ? jf : 0);
} else {
p->j[0] = (p->t[0] > 0 ? jf : 0);
p->j[1] = 0;
p->j[2] = (p->t[2] > 0 ? -jf : 0);
p->j[3] = 0;
p->j[4] = (p->t[4] > 0 ? jf : 0);
p->j[5] = 0;
p->j[6] = (p->t[6] > 0 ? -jf : 0);
}
for (size_t i = 0; i < 7; ++i) {
p->a[i + 1] = p->a[i] + p->t[i] * p->j[i];
p->v[i + 1] = p->v[i] + p->t[i] * (p->a[i] + p->t[i] * p->j[i] / 2);
p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * (p->a[i] / 2 + p->t[i] * p->j[i] / 6));
}
p->control_signs = cs;
p->limits = lim;
p->direction = (aMax > 0) ? DirectionUP : DirectionDOWN;
const double aUppLim = (p->direction == DirectionUP ? aMax : aMin) + PROFILE_A_EPS;
const double aLowLim = (p->direction == DirectionUP ? aMin : aMax) - PROFILE_A_EPS;
return fabs(p->v[7] - p->vf) < PROFILE_V_PREC && fabs(p->a[7] - p->af) < PROFILE_A_PREC
&& p->a[1] >= aLowLim && p->a[3] >= aLowLim && p->a[5] >= aLowLim
&& p->a[1] <= aUppLim && p->a[3] <= aUppLim && p->a[5] <= aUppLim;
}
bool cruckig_profile_check_for_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double aMax, double aMin) {
(void)tf;
return cruckig_profile_check_for_velocity(p, cs, lim, jf, aMax, aMin);
}
bool cruckig_profile_check_for_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double aMax, double aMin, double jMax) {
return (fabs(jf) < fabs(jMax) + PROFILE_J_EPS) && cruckig_profile_check_for_velocity_with_timing(p, cs, lim, tf, jf, aMax, aMin);
}
/* Second-order position check */
bool cruckig_profile_check_for_second_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double aUp, double aDown, double vMax, double vMin) {
if (p->t[0] < 0) {
return false;
}
p->t_sum[0] = p->t[0];
for (size_t i = 0; i < 6; ++i) {
if (p->t[i + 1] < 0) {
return false;
}
p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1];
}
if (p->t_sum[6] > PROFILE_T_MAX) {
return false;
}
p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0;
p->j[4] = 0; p->j[5] = 0; p->j[6] = 0;
if (cs == ControlSignsUDDU) {
p->a[0] = (p->t[0] > 0 ? aUp : 0);
p->a[1] = 0;
p->a[2] = (p->t[2] > 0 ? aDown : 0);
p->a[3] = 0;
p->a[4] = (p->t[4] > 0 ? aDown : 0);
p->a[5] = 0;
p->a[6] = (p->t[6] > 0 ? aUp : 0);
p->a[7] = p->af;
} else {
p->a[0] = (p->t[0] > 0 ? aUp : 0);
p->a[1] = 0;
p->a[2] = (p->t[2] > 0 ? aDown : 0);
p->a[3] = 0;
p->a[4] = (p->t[4] > 0 ? aUp : 0);
p->a[5] = 0;
p->a[6] = (p->t[6] > 0 ? aDown : 0);
p->a[7] = p->af;
}
p->direction = (vMax > 0) ? DirectionUP : DirectionDOWN;
const double vUppLim = (p->direction == DirectionUP ? vMax : vMin) + PROFILE_V_EPS;
const double vLowLim = (p->direction == DirectionUP ? vMin : vMax) - PROFILE_V_EPS;
for (size_t i = 0; i < 7; ++i) {
p->v[i + 1] = p->v[i] + p->t[i] * p->a[i];
p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2);
}
p->control_signs = cs;
p->limits = lim;
return fabs(p->p[7] - p->pf) < PROFILE_P_PREC && fabs(p->v[7] - p->vf) < PROFILE_V_PREC
&& p->v[2] <= vUppLim && p->v[3] <= vUppLim && p->v[4] <= vUppLim && p->v[5] <= vUppLim && p->v[6] <= vUppLim
&& p->v[2] >= vLowLim && p->v[3] >= vLowLim && p->v[4] >= vLowLim && p->v[5] >= vLowLim && p->v[6] >= vLowLim;
}
bool cruckig_profile_check_for_second_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aDown, double vMax, double vMin) {
(void)tf;
return cruckig_profile_check_for_second_order(p, cs, lim, aUp, aDown, vMax, vMin);
}
bool cruckig_profile_check_for_second_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aDown, double vMax, double vMin,
double aMax, double aMin) {
return (aMin - PROFILE_A_EPS < aUp) && (aUp < aMax + PROFILE_A_EPS) && (aMin - PROFILE_A_EPS < aDown) && (aDown < aMax + PROFILE_A_EPS)
&& cruckig_profile_check_for_second_order_with_timing(p, cs, lim, tf, aUp, aDown, vMax, vMin);
}
/* Second-order velocity check */
bool cruckig_profile_check_for_second_order_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double aUp) {
/* ReachedLimits::ACC0 */
if (p->t[1] < 0.0) {
return false;
}
p->t_sum[0] = 0;
p->t_sum[1] = p->t[1];
p->t_sum[2] = p->t[1];
p->t_sum[3] = p->t[1];
p->t_sum[4] = p->t[1];
p->t_sum[5] = p->t[1];
p->t_sum[6] = p->t[1];
if (p->t_sum[6] > PROFILE_T_MAX) {
return false;
}
p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0;
p->j[4] = 0; p->j[5] = 0; p->j[6] = 0;
p->a[0] = 0;
p->a[1] = (p->t[1] > 0) ? aUp : 0;
p->a[2] = 0; p->a[3] = 0; p->a[4] = 0; p->a[5] = 0; p->a[6] = 0;
p->a[7] = p->af;
for (size_t i = 0; i < 7; ++i) {
p->v[i + 1] = p->v[i] + p->t[i] * p->a[i];
p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2);
}
p->control_signs = cs;
p->limits = lim;
p->direction = (aUp > 0) ? DirectionUP : DirectionDOWN;
return fabs(p->v[7] - p->vf) < PROFILE_V_PREC;
}
bool cruckig_profile_check_for_second_order_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp) {
(void)tf;
return cruckig_profile_check_for_second_order_velocity(p, cs, lim, aUp);
}
bool cruckig_profile_check_for_second_order_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aMax, double aMin) {
return (aMin - PROFILE_A_EPS < aUp) && (aUp < aMax + PROFILE_A_EPS)
&& cruckig_profile_check_for_second_order_velocity_with_timing(p, cs, lim, tf, aUp);
}
/* First-order position check */
bool cruckig_profile_check_for_first_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double vUp) {
/* ReachedLimits::VEL */
if (p->t[3] < 0.0) {
return false;
}
p->t_sum[0] = 0; p->t_sum[1] = 0; p->t_sum[2] = 0;
p->t_sum[3] = p->t[3];
p->t_sum[4] = p->t[3]; p->t_sum[5] = p->t[3]; p->t_sum[6] = p->t[3];
if (p->t_sum[6] > PROFILE_T_MAX) {
return false;
}
p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0;
p->j[4] = 0; p->j[5] = 0; p->j[6] = 0;
p->a[0] = 0; p->a[1] = 0; p->a[2] = 0; p->a[3] = 0;
p->a[4] = 0; p->a[5] = 0; p->a[6] = 0; p->a[7] = p->af;
p->v[0] = 0; p->v[1] = 0; p->v[2] = 0;
p->v[3] = (p->t[3] > 0 ? vUp : 0);
p->v[4] = 0; p->v[5] = 0; p->v[6] = 0; p->v[7] = p->vf;
for (size_t i = 0; i < 7; ++i) {
p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2);
}
p->control_signs = cs;
p->limits = lim;
p->direction = (vUp > 0) ? DirectionUP : DirectionDOWN;
return fabs(p->p[7] - p->pf) < PROFILE_P_PREC;
}
bool cruckig_profile_check_for_first_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double vUp) {
(void)tf;
return cruckig_profile_check_for_first_order(p, cs, lim, vUp);
}
bool cruckig_profile_check_for_first_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double vUp, double vMax, double vMin) {
return (vMin - PROFILE_V_EPS < vUp) && (vUp < vMax + PROFILE_V_EPS)
&& cruckig_profile_check_for_first_order_with_timing(p, cs, lim, tf, vUp);
}
/* Position extrema helpers */
static void check_position_extremum(double t_ext, double t_sum_val, double t_seg, double pos, double vel, double acc, double jrk, CRuckigBound *ext) {
if (0 < t_ext && t_ext < t_seg) {
double p_ext, v_ext, a_ext;
cruckig_integrate(t_ext, pos, vel, acc, jrk, &p_ext, &v_ext, &a_ext);
(void)v_ext;
if (a_ext > 0 && p_ext < ext->min) {
ext->min = p_ext;
ext->t_min = t_sum_val + t_ext;
} else if (a_ext < 0 && p_ext > ext->max) {
ext->max = p_ext;
ext->t_max = t_sum_val + t_ext;
}
}
}
static void check_step_for_position_extremum(double t_sum_val, double t_seg, double pos, double vel, double acc, double jrk, CRuckigBound *ext) {
if (pos < ext->min) {
ext->min = pos;
ext->t_min = t_sum_val;
}
if (pos > ext->max) {
ext->max = pos;
ext->t_max = t_sum_val;
}
if (jrk != 0) {
const double D = acc * acc - 2 * jrk * vel;
if (fabs(D) < DBL_EPSILON) {
check_position_extremum(-acc / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext);
} else if (D > 0.0) {
const double D_sqrt = sqrt(D);
check_position_extremum((-acc - D_sqrt) / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext);
check_position_extremum((-acc + D_sqrt) / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext);
}
}
}
CRuckigBound cruckig_profile_get_position_extrema(const CRuckigProfile *p) {
CRuckigBound extrema;
extrema.min = INFINITY;
extrema.max = -INFINITY;
extrema.t_min = 0.0;
extrema.t_max = 0.0;
if (p->brake.duration > 0.0) {
if (p->brake.t[0] > 0.0) {
check_step_for_position_extremum(0.0, p->brake.t[0], p->brake.p[0], p->brake.v[0], p->brake.a[0], p->brake.j[0], &extrema);
if (p->brake.t[1] > 0.0) {
check_step_for_position_extremum(p->brake.t[0], p->brake.t[1], p->brake.p[1], p->brake.v[1], p->brake.a[1], p->brake.j[1], &extrema);
}
}
}
double t_current_sum = 0.0;
for (size_t i = 0; i < 7; ++i) {
if (i > 0) {
t_current_sum = p->t_sum[i - 1];
}
check_step_for_position_extremum(t_current_sum + p->brake.duration, p->t[i], p->p[i], p->v[i], p->a[i], p->j[i], &extrema);
}
if (p->pf < extrema.min) {
extrema.min = p->pf;
extrema.t_min = p->t_sum[6] + p->brake.duration;
}
if (p->pf > extrema.max) {
extrema.max = p->pf;
extrema.t_max = p->t_sum[6] + p->brake.duration;
}
return extrema;
}
bool cruckig_profile_get_first_state_at_position(const CRuckigProfile *p, double pt, double *time, double time_after) {
double t_cum = 0.0;
for (size_t i = 0; i < 7; ++i) {
if (p->t[i] == 0.0) {
continue;
}
if (fabs(p->p[i] - pt) < DBL_EPSILON && t_cum >= time_after) {
*time = t_cum;
return true;
}
CRuckigRootSet cubic_roots = cruckig_roots_solve_cubic(p->j[i] / 6, p->a[i] / 2, p->v[i], p->p[i] - pt);
cruckig_root_set_sort(&cubic_roots);
for (size_t r = 0; r < cubic_roots.size; ++r) {
double _t = cubic_roots.data[r];
if (0 < _t && time_after - t_cum <= _t && _t <= p->t[i]) {
*time = _t + t_cum;
return true;
}
}
t_cum += p->t[i];
}
if ((p->t[6] > 0.0 || p->t_sum[6] == 0.0) && fabs(p->pf - pt) < 1e-9 && p->t_sum[6] >= time_after) {
*time = p->t_sum[6];
return true;
}
return false;
}

126
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.h vendored Normal file
View File

@@ -0,0 +1,126 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_PROFILE_H
#define CRUCKIG_PROFILE_H
#include "cruckig_internal.h"
#include "brake.h"
/* Constants */
#define PROFILE_V_EPS 1e-12
#define PROFILE_A_EPS 1e-12
#define PROFILE_J_EPS 1e-12
#define PROFILE_P_PREC 1e-8
#define PROFILE_V_PREC 1e-8
#define PROFILE_A_PREC 1e-10
#define PROFILE_T_PREC 1e-12
#define PROFILE_T_MAX 1e12
typedef enum {
ReachedLimitsACC0_ACC1_VEL = 0,
ReachedLimitsVEL,
ReachedLimitsACC0,
ReachedLimitsACC1,
ReachedLimitsACC0_ACC1,
ReachedLimitsACC0_VEL,
ReachedLimitsACC1_VEL,
ReachedLimitsNONE
} CRuckigReachedLimits;
typedef enum {
DirectionUP = 0,
DirectionDOWN
} CRuckigDirection;
typedef enum {
ControlSignsUDDU = 0,
ControlSignsUDUD
} CRuckigControlSigns;
/* Position extrema info */
typedef struct {
double min, max;
double t_min, t_max;
} CRuckigBound;
/* Single-DOF kinematic profile */
typedef struct {
double t[7];
double t_sum[7];
double j[7];
double a[8];
double v[8];
double p[8];
CRuckigBrakeProfile brake;
CRuckigBrakeProfile accel;
double pf, vf, af;
CRuckigReachedLimits limits;
CRuckigDirection direction;
CRuckigControlSigns control_signs;
} CRuckigProfile;
void cruckig_profile_init(CRuckigProfile *p);
/* Set boundary conditions */
void cruckig_profile_set_boundary(CRuckigProfile *p, double p0, double v0, double a0,
double pf, double vf, double af);
void cruckig_profile_set_boundary_from_profile(CRuckigProfile *p, const CRuckigProfile *src);
void cruckig_profile_set_boundary_for_velocity(CRuckigProfile *p, double p0, double v0, double a0,
double vf, double af);
/* Third-order position check */
bool cruckig_profile_check(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
bool set_limits, double jf, double vMax, double vMin, double aMax, double aMin);
bool cruckig_profile_check_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double vMax, double vMin, double aMax, double aMin);
bool cruckig_profile_check_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double vMax, double vMin, double aMax, double aMin, double jMax);
/* Third-order velocity check */
bool cruckig_profile_check_for_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double jf, double aMax, double aMin);
bool cruckig_profile_check_for_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double aMax, double aMin);
bool cruckig_profile_check_for_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double jf, double aMax, double aMin, double jMax);
/* Second-order position check */
bool cruckig_profile_check_for_second_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double aUp, double aDown, double vMax, double vMin);
bool cruckig_profile_check_for_second_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aDown, double vMax, double vMin);
bool cruckig_profile_check_for_second_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aDown, double vMax, double vMin,
double aMax, double aMin);
/* Second-order velocity check */
bool cruckig_profile_check_for_second_order_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double aUp);
bool cruckig_profile_check_for_second_order_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp);
bool cruckig_profile_check_for_second_order_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double aUp, double aMax, double aMin);
/* First-order position check */
bool cruckig_profile_check_for_first_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double vUp);
bool cruckig_profile_check_for_first_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double vUp);
bool cruckig_profile_check_for_first_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim,
double tf, double vUp, double vMax, double vMin);
/* Position extrema */
CRuckigBound cruckig_profile_get_position_extrema(const CRuckigProfile *p);
/* First time at position */
bool cruckig_profile_get_first_state_at_position(const CRuckigProfile *p, double pt, double *time, double time_after);
#endif /* CRUCKIG_PROFILE_H */

40
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/result.h vendored Normal file
View File

@@ -0,0 +1,40 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_RESULT_H
#define CRUCKIG_RESULT_H
typedef enum {
CRuckigWorking = 0,
CRuckigFinished = 1,
CRuckigError = -1,
CRuckigErrorInvalidInput = -100,
CRuckigErrorTrajectoryDuration = -101,
CRuckigErrorPositionalLimits = -102,
CRuckigErrorZeroLimits = -104,
CRuckigErrorExecutionTimeCalculation = -110,
CRuckigErrorSynchronizationCalculation = -111
} CRuckigResult;
typedef enum {
CRuckigPosition = 0,
CRuckigVelocity = 1
} CRuckigControlInterface;
typedef enum {
CRuckigSyncTime = 0,
CRuckigSyncTimeIfNecessary = 1,
CRuckigSyncPhase = 2,
CRuckigSyncNone = 3
} CRuckigSynchronization;
typedef enum {
CRuckigContinuous = 0,
CRuckigDiscrete = 1
} CRuckigDurationDiscretization;
#endif /* CRUCKIG_RESULT_H */

408
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.c vendored Normal file
View File

@@ -0,0 +1,408 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "roots.h"
/*
* cruckig_cbrt() - Cube root, used for cubic/quartic polynomial solving.
* Optimized implementation from musl libc / FreeBSD libmsun.
* Polynomial approximation to 23 bits + one Newton step to 53 bits.
* Error < 0.667 ulps.
*
* Copyright (c) 1993 Sun Microsystems, Inc. All rights reserved.
* Developed at SunPro, a Sun Microsystems, Inc. business.
* Copyright (c) 2005-2020 Rich Felker, et al. (musl libc)
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*/
double cruckig_cbrt(double x) {
static const unsigned B1 = 715094163;
static const unsigned B2 = 696219795;
static const double P0 = 1.87595182427177009643;
static const double P1 = -1.88497979543377169875;
static const double P2 = 1.621429720105354466140;
static const double P3 = -0.758397934778766047437;
static const double P4 = 0.145996192886612446982;
union { double f; unsigned long long i; } u = {x};
double r, s, t, w;
unsigned hx = u.i >> 32 & 0x7fffffff;
if (hx >= 0x7ff00000)
return x + x;
if (hx < 0x00100000) {
u.f = x * 0x1p54;
hx = u.i >> 32 & 0x7fffffff;
if (hx == 0) return x;
hx = hx / 3 + B2;
} else {
hx = hx / 3 + B1;
}
u.i &= 1ULL << 63;
u.i |= (unsigned long long)hx << 32;
t = u.f;
r = (t * t) * (t / x);
t = t * ((P0 + r * (P1 + r * P2)) + ((r * r) * r) * (P3 + r * P4));
u.f = t;
u.i = (u.i + 0x80000000) & 0xffffffffc0000000ULL;
t = u.f;
s = t * t;
r = x / s;
w = t + t;
r = (r - t) / (w + r);
t = t + t * r;
return t;
}
void cruckig_root_set_sort(CRuckigRootSet *s) {
/* Insertion sort for small arrays (max 4 elements) */
for (size_t i = 1; i < s->size; ++i) {
double key = s->data[i];
size_t j = i;
while (j > 0 && s->data[j - 1] > key) {
s->data[j] = s->data[j - 1];
--j;
}
s->data[j] = key;
}
}
CRUCKIG_HOT
CRuckigRootSet cruckig_roots_solve_cubic(double a, double b, double c, double d) {
CRuckigRootSet roots;
cruckig_root_set_init(&roots);
if (fabs(d) < DBL_EPSILON) {
/* First solution is x = 0 */
cruckig_root_set_insert(&roots, 0.0);
/* Converting to a quadratic equation */
d = c;
c = b;
b = a;
a = 0.0;
}
if (fabs(a) < DBL_EPSILON) {
if (fabs(b) < DBL_EPSILON) {
/* Linear equation */
if (fabs(c) > DBL_EPSILON) {
cruckig_root_set_insert(&roots, -d / c);
}
} else {
/* Quadratic equation */
const double discriminant = c * c - 4 * b * d;
if (discriminant >= 0) {
const double inv2b = 1.0 / (2 * b);
const double y = sqrt(discriminant);
cruckig_root_set_insert(&roots, (-c + y) * inv2b);
cruckig_root_set_insert(&roots, (-c - y) * inv2b);
}
}
} else {
/* Cubic equation */
const double inva = 1.0 / a;
const double invaa = inva * inva;
const double bb = b * b;
const double bover3a = b * inva / 3;
const double p = (a * c - bb / 3) * invaa;
const double halfq = (2 * bb * b - 9 * a * b * c + 27 * a * a * d) / 54 * invaa * inva;
const double yy = p * p * p / 27 + halfq * halfq;
const double cos120 = -0.50;
const double sin120 = 0.866025403784438646764;
if (yy > DBL_EPSILON) {
/* Sqrt is positive: one real solution */
const double y = sqrt(yy);
const double uuu = -halfq + y;
const double vvv = -halfq - y;
const double www = fabs(uuu) > fabs(vvv) ? uuu : vvv;
const double w = cruckig_cbrt(www);
cruckig_root_set_insert(&roots, w - p / (3 * w) - bover3a);
} else if (yy < -DBL_EPSILON) {
/* Sqrt is negative: three real solutions */
const double x = -halfq;
const double y = sqrt(-yy);
double theta;
double r;
/* Convert to polar form */
if (fabs(x) > DBL_EPSILON) {
theta = (x > 0.0) ? atan(y / x) : (atan(y / x) + M_PI);
r = sqrt(x * x - yy);
} else {
/* Vertical line */
theta = M_PI / 2;
r = y;
}
/* Calculate cube root */
theta /= 3;
r = 2 * cruckig_cbrt(r);
/* Convert to complex coordinate */
const double ux = cos(theta) * r;
const double uyi = sin(theta) * r;
cruckig_root_set_insert(&roots, ux - bover3a);
cruckig_root_set_insert(&roots, ux * cos120 - uyi * sin120 - bover3a);
cruckig_root_set_insert(&roots, ux * cos120 + uyi * sin120 - bover3a);
} else {
/* Sqrt is zero: two real solutions */
const double www = -halfq;
const double w = 2 * cruckig_cbrt(www);
cruckig_root_set_insert(&roots, w - bover3a);
cruckig_root_set_insert(&roots, w * cos120 - bover3a);
}
}
return roots;
}
int cruckig_roots_solve_resolvent(double x[3], double a, double b, double c) {
const double cos120 = -0.50;
const double sin120 = 0.866025403784438646764;
a /= 3;
const double a2 = a * a;
double q = a2 - b / 3;
const double r = (a * (2 * a2 - b) + c) / 2;
const double r2 = r * r;
const double q3 = q * q * q;
if (r2 < q3) {
const double qsqrt = sqrt(q);
double t_val = r / (q * qsqrt);
if (t_val < -1.0) t_val = -1.0;
if (t_val > 1.0) t_val = 1.0;
q = -2 * qsqrt;
const double theta = acos(t_val) / 3;
const double ux = cos(theta) * q;
const double uyi = sin(theta) * q;
x[0] = ux - a;
x[1] = ux * cos120 - uyi * sin120 - a;
x[2] = ux * cos120 + uyi * sin120 - a;
return 3;
} else {
double A = -cruckig_cbrt(fabs(r) + sqrt(r2 - q3));
if (r < 0.0) {
A = -A;
}
const double B = (0.0 == A ? 0.0 : q / A);
x[0] = (A + B) - a;
x[1] = -(A + B) / 2 - a;
x[2] = sqrt(3.0) * (A - B) / 2;
if (fabs(x[2]) < DBL_EPSILON) {
x[2] = x[1];
return 2;
}
return 1;
}
}
CRUCKIG_HOT
CRuckigRootSet cruckig_roots_solve_quart_monic(double a, double b, double c, double d) {
CRuckigRootSet roots;
cruckig_root_set_init(&roots);
if (fabs(d) < DBL_EPSILON) {
if (fabs(c) < DBL_EPSILON) {
cruckig_root_set_insert(&roots, 0.0);
const double D = a * a - 4 * b;
if (fabs(D) < DBL_EPSILON) {
cruckig_root_set_insert(&roots, -a / 2);
} else if (D > 0.0) {
const double sqrtD = sqrt(D);
cruckig_root_set_insert(&roots, (-a - sqrtD) / 2);
cruckig_root_set_insert(&roots, (-a + sqrtD) / 2);
}
return roots;
}
if (fabs(a) < DBL_EPSILON && fabs(b) < DBL_EPSILON) {
cruckig_root_set_insert(&roots, 0.0);
cruckig_root_set_insert(&roots, -cruckig_cbrt(c));
return roots;
}
}
const double a3 = -b;
const double b3 = a * c - 4 * d;
const double c3 = -a * a * d - c * c + 4 * b * d;
double x3[3];
const int number_zeroes = cruckig_roots_solve_resolvent(x3, a3, b3, c3);
double y = x3[0];
/* Choosing Y with maximal absolute value */
if (number_zeroes != 1) {
if (fabs(x3[1]) > fabs(y)) {
y = x3[1];
}
if (fabs(x3[2]) > fabs(y)) {
y = x3[2];
}
}
double q1, q2, p1, p2;
double D;
D = y * y - 4 * d;
if (fabs(D) < DBL_EPSILON) {
q1 = q2 = y / 2;
D = a * a - 4 * (b - y);
if (fabs(D) < DBL_EPSILON) {
p1 = p2 = a / 2;
} else {
const double sqrtD = sqrt(D);
p1 = (a + sqrtD) / 2;
p2 = (a - sqrtD) / 2;
}
} else {
const double sqrtD = sqrt(D);
q1 = (y + sqrtD) / 2;
q2 = (y - sqrtD) / 2;
p1 = (a * q1 - c) / (q1 - q2);
p2 = (c - a * q2) / (q1 - q2);
}
{
const double eps = 16 * DBL_EPSILON;
D = p1 * p1 - 4 * q1;
if (fabs(D) < eps) {
cruckig_root_set_insert(&roots, -p1 / 2);
} else if (D > 0.0) {
const double sqrtD = sqrt(D);
cruckig_root_set_insert(&roots, (-p1 - sqrtD) / 2);
cruckig_root_set_insert(&roots, (-p1 + sqrtD) / 2);
}
D = p2 * p2 - 4 * q2;
if (fabs(D) < eps) {
cruckig_root_set_insert(&roots, -p2 / 2);
} else if (D > 0.0) {
const double sqrtD = sqrt(D);
cruckig_root_set_insert(&roots, (-p2 - sqrtD) / 2);
cruckig_root_set_insert(&roots, (-p2 + sqrtD) / 2);
}
}
return roots;
}
double cruckig_roots_poly_eval(const double *p, size_t n, double x) {
if (n == 0) {
return 0.0;
}
double retVal = 0.0;
if (fabs(x) < DBL_EPSILON) {
retVal = p[n - 1];
} else if (x == 1.0) {
for (int i = (int)n - 1; i >= 0; i--) {
retVal += p[i];
}
} else {
double xn = 1.0;
for (int i = (int)n - 1; i >= 0; i--) {
retVal += p[i] * xn;
xn *= x;
}
}
return retVal;
}
void cruckig_roots_poly_derivative(const double *coeffs, size_t n, double *deriv) {
for (size_t i = 0; i < n - 1; ++i) {
deriv[i] = (double)(n - 1 - i) * coeffs[i];
}
}
double cruckig_roots_shrink_interval(const double *p, size_t n, double l, double h) {
const size_t maxIts = 128;
const double tolerance = 1e-14;
const double fl = cruckig_roots_poly_eval(p, n, l);
const double fh = cruckig_roots_poly_eval(p, n, h);
if (fl == 0.0) {
return l;
}
if (fh == 0.0) {
return h;
}
if (fl > 0.0) {
/* swap l and h */
double tmp = l;
l = h;
h = tmp;
}
double rts = (l + h) / 2;
double dxold = fabs(h - l);
double dx = dxold;
/* Compute derivative coefficients (n-1 elements) */
double deriv[16]; /* max polynomial degree supported */
cruckig_roots_poly_derivative(p, n, deriv);
size_t dn = n - 1;
double f = cruckig_roots_poly_eval(p, n, rts);
double df = cruckig_roots_poly_eval(deriv, dn, rts);
double temp;
for (size_t j = 0; j < maxIts; j++) {
if ((((rts - h) * df - f) * ((rts - l) * df - f) > 0.0) || (fabs(2 * f) > fabs(dxold * df))) {
dxold = dx;
dx = (h - l) / 2;
rts = l + dx;
if (l == rts) {
break;
}
} else {
dxold = dx;
dx = f / df;
temp = rts;
rts -= dx;
if (temp == rts) {
break;
}
}
if (fabs(dx) < tolerance) {
break;
}
f = cruckig_roots_poly_eval(p, n, rts);
df = cruckig_roots_poly_eval(deriv, dn, rts);
if (f < 0.0) {
l = rts;
} else {
h = rts;
}
}
return rts;
}

57
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.h vendored Normal file
View File

@@ -0,0 +1,57 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_ROOTS_H
#define CRUCKIG_ROOTS_H
#include "cruckig_internal.h"
/* A set of positive double roots, stored on the stack */
typedef struct {
double data[4];
size_t size;
} CRuckigRootSet;
CRUCKIG_FORCE_INLINE void cruckig_root_set_init(CRuckigRootSet *s) {
s->size = 0;
}
CRUCKIG_FORCE_INLINE void cruckig_root_set_insert(CRuckigRootSet *s, double value) {
if (value >= 0.0) {
s->data[s->size] = value;
s->size++;
}
}
/*
* Cube root, portable replacement for cbrt() (not available in kernel).
* Optimized implementation from musl libc / FreeBSD libmsun.
*/
double cruckig_cbrt(double x);
/* Sort the root set (simple insertion sort for small N) */
void cruckig_root_set_sort(CRuckigRootSet *s);
/* Solve a*x^3 + b*x^2 + c*x + d = 0, returning positive roots */
CRuckigRootSet cruckig_roots_solve_cubic(double a, double b, double c, double d);
/* Solve resolvent equation, returns number of zeros */
int cruckig_roots_solve_resolvent(double x[3], double a, double b, double c);
/* Solve monic quartic x^4 + a*x^3 + b*x^2 + c*x + d = 0 */
CRuckigRootSet cruckig_roots_solve_quart_monic(double a, double b, double c, double d);
/* Evaluate polynomial of order N at x. Coefficients in descending order: p[0]*x^(N-1) + ... + p[N-1] */
double cruckig_roots_poly_eval(const double *p, size_t n, double x);
/* Calculate derivative coefficients */
void cruckig_roots_poly_derivative(const double *coeffs, size_t n, double *deriv);
/* Safe Newton method: find root in [l, h] where p(l)*p(h) < 0 */
double cruckig_roots_shrink_interval(const double *p, size_t n, double l, double h);
#endif /* CRUCKIG_ROOTS_H */

315
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.c vendored Normal file
View File

@@ -0,0 +1,315 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "trajectory.h"
#include "utils.h"
CRuckigTrajectory* cruckig_trajectory_create(size_t dofs) {
CRuckigTrajectory *traj = (CRuckigTrajectory*)cruckig_calloc(1, sizeof(CRuckigTrajectory));
if (!traj) return NULL;
traj->degrees_of_freedom = dofs;
traj->num_sections = 1;
traj->section_capacity = 1;
traj->duration = 0.0;
traj->profiles = (CRuckigProfile*)cruckig_calloc(dofs, sizeof(CRuckigProfile));
traj->cumulative_times = (double*)cruckig_calloc(1, sizeof(double));
traj->independent_min_durations = (double*)cruckig_calloc(dofs, sizeof(double));
traj->position_extrema = (CRuckigBound*)cruckig_calloc(dofs, sizeof(CRuckigBound));
if (!traj->profiles || !traj->cumulative_times ||
!traj->independent_min_durations || !traj->position_extrema) {
cruckig_trajectory_destroy(traj);
return NULL;
}
for (size_t dof = 0; dof < dofs; ++dof) {
cruckig_profile_init(&traj->profiles[dof]);
}
return traj;
}
void cruckig_trajectory_destroy(CRuckigTrajectory *traj) {
if (!traj) return;
cruckig_free(traj->profiles);
cruckig_free(traj->cumulative_times);
cruckig_free(traj->independent_min_durations);
cruckig_free(traj->position_extrema);
cruckig_free(traj);
}
bool cruckig_trajectory_resize(CRuckigTrajectory *traj, size_t num_sections) {
if (!traj || num_sections == 0) return false;
const size_t dofs = traj->degrees_of_freedom;
if (num_sections > traj->section_capacity) {
CRuckigProfile *new_profiles = (CRuckigProfile*)cruckig_realloc(
traj->profiles, num_sections * dofs * sizeof(CRuckigProfile));
double *new_times = (double*)cruckig_realloc(
traj->cumulative_times, num_sections * sizeof(double));
if (!new_profiles || !new_times) {
/* Restore on failure */
if (new_profiles) traj->profiles = new_profiles;
if (new_times) traj->cumulative_times = new_times;
return false;
}
traj->profiles = new_profiles;
traj->cumulative_times = new_times;
traj->section_capacity = num_sections;
/* Initialize new profiles */
for (size_t s = traj->num_sections; s < num_sections; ++s) {
for (size_t d = 0; d < dofs; ++d) {
cruckig_profile_init(&traj->profiles[s * dofs + d]);
}
traj->cumulative_times[s] = 0.0;
}
}
traj->num_sections = num_sections;
return true;
}
/*
* state_to_integrate_from: Determine the integration base state at a given time.
* Supports multi-section trajectories via binary search on cumulative_times.
*/
static void state_to_integrate_from(const CRuckigTrajectory *traj, double time,
size_t *new_section,
double *t_out, double *p_out, double *v_out,
double *a_out, double *j_out)
{
const size_t dofs = traj->degrees_of_freedom;
const size_t nsec = traj->num_sections;
if (time >= traj->duration) {
/* Past the end of trajectory */
*new_section = nsec;
size_t last = nsec - 1;
for (size_t dof = 0; dof < dofs; ++dof) {
const CRuckigProfile *prof = &traj->profiles[last * dofs + dof];
double t_pre = prof->brake.duration;
double t_diff = time - (traj->duration - (t_pre + prof->t_sum[6]) + t_pre + prof->t_sum[6]);
/* Simplify: time past the end of last section's profile */
double section_start = (last > 0) ? traj->cumulative_times[last - 1] : 0.0;
t_diff = time - section_start - t_pre - prof->t_sum[6];
t_out[dof] = t_diff;
p_out[dof] = prof->p[7];
v_out[dof] = prof->v[7];
a_out[dof] = prof->a[7];
j_out[dof] = 0.0;
}
return;
}
/* Binary search to find current section */
size_t section = 0;
if (nsec > 1) {
size_t lo = 0, hi = nsec;
while (lo < hi) {
size_t mid = lo + (hi - lo) / 2;
if (traj->cumulative_times[mid] <= time) {
lo = mid + 1;
} else {
hi = mid;
}
}
section = lo;
if (section >= nsec) section = nsec - 1;
}
*new_section = section;
/* Time offset within this section */
double section_start = (section > 0) ? traj->cumulative_times[section - 1] : 0.0;
double t_diff = time - section_start;
for (size_t dof = 0; dof < dofs; ++dof) {
const CRuckigProfile *prof = &traj->profiles[section * dofs + dof];
double t_diff_dof = t_diff;
/* Brake pre-trajectory (only in first section, or in each section for waypoints) */
if (prof->brake.duration > 0.0) {
if (t_diff_dof < prof->brake.duration) {
size_t index = (t_diff_dof < prof->brake.t[0]) ? 0 : 1;
if (index > 0) {
t_diff_dof -= prof->brake.t[index - 1];
}
t_out[dof] = t_diff_dof;
p_out[dof] = prof->brake.p[index];
v_out[dof] = prof->brake.v[index];
a_out[dof] = prof->brake.a[index];
j_out[dof] = prof->brake.j[index];
continue;
} else {
t_diff_dof -= prof->brake.duration;
}
}
/* Non-time synchronization: past the end of this DOF's profile */
if (t_diff_dof >= prof->t_sum[6]) {
t_out[dof] = t_diff_dof - prof->t_sum[6];
p_out[dof] = prof->p[7];
v_out[dof] = prof->v[7];
a_out[dof] = prof->a[7];
j_out[dof] = 0.0;
continue;
}
/* Binary search in t_sum[0..6] */
size_t index_dof = 0;
{
size_t lo = 0, hi = 7;
while (lo < hi) {
size_t mid = lo + (hi - lo) / 2;
if (prof->t_sum[mid] <= t_diff_dof) {
lo = mid + 1;
} else {
hi = mid;
}
}
index_dof = lo;
}
if (index_dof > 0) {
t_diff_dof -= prof->t_sum[index_dof - 1];
}
t_out[dof] = t_diff_dof;
p_out[dof] = prof->p[index_dof];
v_out[dof] = prof->v[index_dof];
a_out[dof] = prof->a[index_dof];
j_out[dof] = prof->j[index_dof];
}
}
CRUCKIG_HOT
void cruckig_trajectory_at_time(const CRuckigTrajectory *traj, double time,
double * CRUCKIG_RESTRICT new_position,
double * CRUCKIG_RESTRICT new_velocity,
double * CRUCKIG_RESTRICT new_acceleration,
double * CRUCKIG_RESTRICT new_jerk,
size_t *new_section)
{
const size_t dofs = traj->degrees_of_freedom;
/* Implementation limit: max 16 DOF (stack-allocated work arrays) */
double t_buf[16], p_buf[16], v_buf[16], a_buf[16], j_buf[16];
const size_t ndofs = (dofs > 16) ? 16 : dofs;
state_to_integrate_from(traj, time, new_section, t_buf, p_buf, v_buf, a_buf, j_buf);
for (size_t dof = 0; dof < ndofs; ++dof) {
double p_out, v_out, a_out;
cruckig_integrate(t_buf[dof], p_buf[dof], v_buf[dof], a_buf[dof], j_buf[dof],
&p_out, &v_out, &a_out);
new_position[dof] = p_out;
new_velocity[dof] = v_out;
new_acceleration[dof] = a_out;
if (new_jerk) {
new_jerk[dof] = j_buf[dof];
}
}
}
void cruckig_trajectory_at_time_simple(const CRuckigTrajectory *traj, double time,
double *new_position, double *new_velocity,
double *new_acceleration)
{
size_t new_section;
cruckig_trajectory_at_time(traj, time, new_position, new_velocity,
new_acceleration, NULL, &new_section);
}
double cruckig_trajectory_get_duration(const CRuckigTrajectory *traj) {
return traj->duration;
}
size_t cruckig_trajectory_get_intermediate_durations(const CRuckigTrajectory *traj,
double *out_durations)
{
for (size_t s = 0; s < traj->num_sections; ++s) {
out_durations[s] = traj->cumulative_times[s];
}
return traj->num_sections;
}
void cruckig_trajectory_get_position_extrema(CRuckigTrajectory *traj) {
const size_t dofs = traj->degrees_of_freedom;
for (size_t dof = 0; dof < dofs; ++dof) {
/* Initialize from first section */
CRuckigBound bound = cruckig_profile_get_position_extrema(&traj->profiles[dof]);
/* Merge across all sections */
for (size_t s = 1; s < traj->num_sections; ++s) {
double section_start = traj->cumulative_times[s - 1];
CRuckigBound sb = cruckig_profile_get_position_extrema(
&traj->profiles[s * dofs + dof]);
if (sb.min < bound.min) {
bound.min = sb.min;
bound.t_min = sb.t_min + section_start;
}
if (sb.max > bound.max) {
bound.max = sb.max;
bound.t_max = sb.t_max + section_start;
}
}
traj->position_extrema[dof] = bound;
}
}
bool cruckig_trajectory_get_first_time_at_position(const CRuckigTrajectory *traj,
size_t dof, double position,
double *time, double time_after)
{
if (dof >= traj->degrees_of_freedom) return false;
const size_t dofs = traj->degrees_of_freedom;
/* Search through all sections */
for (size_t s = 0; s < traj->num_sections; ++s) {
double section_start = (s > 0) ? traj->cumulative_times[s - 1] : 0.0;
double adjusted_time_after = time_after - section_start;
if (adjusted_time_after < 0.0) adjusted_time_after = 0.0;
if (cruckig_profile_get_first_state_at_position(
&traj->profiles[s * dofs + dof], position, time, adjusted_time_after)) {
*time += section_start;
return true;
}
}
return false;
}
void cruckig_trajectory_get_independent_min_durations(const CRuckigTrajectory *traj,
double *out_durations)
{
for (size_t dof = 0; dof < traj->degrees_of_freedom; ++dof) {
out_durations[dof] = traj->independent_min_durations[dof];
}
}
const CRuckigProfile* cruckig_trajectory_get_profile(const CRuckigTrajectory *traj, size_t dof)
{
if (dof >= traj->degrees_of_freedom) return NULL;
return &traj->profiles[dof];
}
const CRuckigProfile* cruckig_trajectory_get_section_profile(const CRuckigTrajectory *traj,
size_t section, size_t dof)
{
if (section >= traj->num_sections || dof >= traj->degrees_of_freedom) return NULL;
return &traj->profiles[section * traj->degrees_of_freedom + dof];
}

71
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.h vendored Normal file
View File

@@ -0,0 +1,71 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_TRAJECTORY_H
#define CRUCKIG_TRAJECTORY_H
#include "cruckig_internal.h"
#include "profile.h"
typedef struct {
size_t degrees_of_freedom;
/* Multi-section support: profiles[section * dofs + dof] */
CRuckigProfile *profiles; /* Array of num_sections * dofs profiles */
size_t num_sections; /* Number of sections (1 for state-to-state) */
size_t section_capacity; /* Allocated capacity for sections */
double duration;
double *cumulative_times; /* Array of num_sections cumulative durations */
double *independent_min_durations; /* Array of dofs */
CRuckigBound *position_extrema; /* Array of dofs */
} CRuckigTrajectory;
/* Create trajectory for single-section (backward compatible) */
CRuckigTrajectory* cruckig_trajectory_create(size_t dofs);
void cruckig_trajectory_destroy(CRuckigTrajectory *traj);
/* Resize trajectory for multi-section (num_sections = max_waypoints + 1) */
bool cruckig_trajectory_resize(CRuckigTrajectory *traj, size_t num_sections);
/* Query trajectory state at time */
void cruckig_trajectory_at_time(const CRuckigTrajectory *traj, double time,
double *new_position, double *new_velocity,
double *new_acceleration, double *new_jerk,
size_t *new_section);
/* Simplified version without jerk/section */
void cruckig_trajectory_at_time_simple(const CRuckigTrajectory *traj, double time,
double *new_position, double *new_velocity,
double *new_acceleration);
double cruckig_trajectory_get_duration(const CRuckigTrajectory *traj);
/* Get intermediate durations (cumulative times array). Returns num_sections. */
size_t cruckig_trajectory_get_intermediate_durations(const CRuckigTrajectory *traj,
double *out_durations);
/* Get position extrema for all DOFs */
void cruckig_trajectory_get_position_extrema(CRuckigTrajectory *traj);
/* Get first time at position for a DOF. Returns true if found. */
bool cruckig_trajectory_get_first_time_at_position(const CRuckigTrajectory *traj,
size_t dof, double position,
double *time, double time_after);
/* Get independent minimum durations (one per DOF). Caller provides array of dofs. */
void cruckig_trajectory_get_independent_min_durations(const CRuckigTrajectory *traj,
double *out_durations);
/* Get the underlying profile for a specific DOF in a section (read-only). */
const CRuckigProfile* cruckig_trajectory_get_profile(const CRuckigTrajectory *traj, size_t dof);
/* Get profile for specific section and DOF. */
const CRuckigProfile* cruckig_trajectory_get_section_profile(const CRuckigTrajectory *traj,
size_t section, size_t dof);
#endif /* CRUCKIG_TRAJECTORY_H */

26
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/utils.h vendored Normal file
View File

@@ -0,0 +1,26 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_UTILS_H
#define CRUCKIG_UTILS_H
#include "cruckig_internal.h"
CRUCKIG_FORCE_INLINE void cruckig_integrate(double t, double p0, double v0, double a0, double j,
double * CRUCKIG_RESTRICT p_out,
double * CRUCKIG_RESTRICT v_out,
double * CRUCKIG_RESTRICT a_out) {
*p_out = p0 + t * (v0 + t * (a0 / 2.0 + t * j / 6.0));
*v_out = v0 + t * (a0 + t * j / 2.0);
*a_out = a0 + t * j;
}
CRUCKIG_FORCE_INLINE double cruckig_pow2(double v) {
return v * v;
}
#endif /* CRUCKIG_UTILS_H */

63
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity.h vendored Normal file
View File

@@ -0,0 +1,63 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#ifndef CRUCKIG_VELOCITY_H
#define CRUCKIG_VELOCITY_H
#include "cruckig_internal.h"
#include "profile.h"
#include "block.h"
/* ---- Third Order Step 1 ---- */
typedef struct {
double a0, af;
double _aMax, _aMin, _jMax;
double vd;
CRuckigProfile valid_profiles[3];
} CRuckigVelocityThirdOrderStep1;
void cruckig_vel3_step1_init(CRuckigVelocityThirdOrderStep1 *s,
double v0, double a0, double vf, double af,
double aMax, double aMin, double jMax);
bool cruckig_vel3_step1_get_profile(CRuckigVelocityThirdOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block);
/* ---- Third Order Step 2 ---- */
typedef struct {
double a0, tf, af;
double _aMax, _aMin, _jMax;
double vd, ad;
} CRuckigVelocityThirdOrderStep2;
void cruckig_vel3_step2_init(CRuckigVelocityThirdOrderStep2 *s,
double tf, double v0, double a0, double vf, double af,
double aMax, double aMin, double jMax);
bool cruckig_vel3_step2_get_profile(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile);
/* ---- Second Order Step 1 ---- */
typedef struct {
double _aMax, _aMin;
double vd;
} CRuckigVelocitySecondOrderStep1;
void cruckig_vel2_step1_init(CRuckigVelocitySecondOrderStep1 *s,
double v0, double vf, double aMax, double aMin);
bool cruckig_vel2_step1_get_profile(CRuckigVelocitySecondOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block);
/* ---- Second Order Step 2 ---- */
typedef struct {
double tf;
double _aMax, _aMin;
double vd;
} CRuckigVelocitySecondOrderStep2;
void cruckig_vel2_step2_init(CRuckigVelocitySecondOrderStep2 *s,
double tf, double v0, double vf, double aMax, double aMin);
bool cruckig_vel2_step2_get_profile(CRuckigVelocitySecondOrderStep2 *s, CRuckigProfile *profile);
#endif /* CRUCKIG_VELOCITY_H */

40
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step1.c vendored Normal file
View File

@@ -0,0 +1,40 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "velocity.h"
#include "block.h"
#include "profile.h"
void cruckig_vel2_step1_init(CRuckigVelocitySecondOrderStep1 *s,
double v0, double vf, double aMax, double aMin)
{
s->_aMax = aMax;
s->_aMin = aMin;
s->vd = vf - v0;
}
bool cruckig_vel2_step1_get_profile(CRuckigVelocitySecondOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block)
{
CRuckigProfile *p = &block->p_min;
cruckig_profile_set_boundary_from_profile(p, input);
const double af = (s->vd > 0) ? s->_aMax : s->_aMin;
p->t[0] = 0;
p->t[1] = s->vd / af;
p->t[2] = 0;
p->t[3] = 0;
p->t[4] = 0;
p->t[5] = 0;
p->t[6] = 0;
if (cruckig_profile_check_for_second_order_velocity(p, ControlSignsUDDU, ReachedLimitsACC0, af)) {
block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration;
return true;
}
return false;
}

39
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step2.c vendored Normal file
View File

@@ -0,0 +1,39 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "velocity.h"
#include "block.h"
#include "profile.h"
void cruckig_vel2_step2_init(CRuckigVelocitySecondOrderStep2 *s,
double tf, double v0, double vf, double aMax, double aMin)
{
s->tf = tf;
s->_aMax = aMax;
s->_aMin = aMin;
s->vd = vf - v0;
}
bool cruckig_vel2_step2_get_profile(CRuckigVelocitySecondOrderStep2 *s, CRuckigProfile *profile)
{
const double af = s->vd / s->tf;
profile->t[0] = 0;
profile->t[1] = s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_second_order_velocity_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, af, s->_aMax, s->_aMin)) {
profile->pf = profile->p[7];
return true;
}
return false;
}

187
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step1.c vendored Normal file
View File

@@ -0,0 +1,187 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "velocity.h"
#include "block.h"
#include "profile.h"
/* ---- Internal helper functions ---- */
static void time_acc0(const CRuckigVelocityThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double aMax, double aMin, double jMax, bool return_after_found)
{
(void)return_after_found;
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = (-s->a0 + aMax) / jMax;
profile->t[1] = (s->a0 * s->a0 + s->af * s->af) / (2 * aMax * jMax) - aMax / jMax + s->vd / aMax;
profile->t[2] = (-s->af + aMax) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsACC0, jMax, aMax, aMin)) {
(*counter)++;
if (*counter < 3) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
}
}
static void time_none(const CRuckigVelocityThirdOrderStep1 *s,
CRuckigProfile *valid_profiles, size_t *counter,
double aMax, double aMin, double jMax, bool return_after_found)
{
double h1 = (s->a0 * s->a0 + s->af * s->af) / 2 + jMax * s->vd;
if (h1 >= 0.0) {
h1 = sqrt(h1);
/* Solution 1 */
{
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = -(s->a0 + h1) / jMax;
profile->t[1] = 0;
profile->t[2] = -(s->af + h1) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, jMax, aMax, aMin)) {
(*counter)++;
if (*counter < 3) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
if (return_after_found) {
return;
}
}
}
/* Solution 2 */
{
CRuckigProfile *profile = &valid_profiles[*counter];
profile->t[0] = (-s->a0 + h1) / jMax;
profile->t[1] = 0;
profile->t[2] = (-s->af + h1) / jMax;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, jMax, aMax, aMin)) {
(*counter)++;
if (*counter < 3) {
cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile);
}
}
}
}
}
static bool time_all_single_step(const CRuckigVelocityThirdOrderStep1 *s,
CRuckigProfile *profile,
double aMax, double aMin, double jMax)
{
(void)jMax;
if (fabs(s->af - s->a0) > DBL_EPSILON) {
return false;
}
profile->t[0] = 0;
profile->t[1] = 0;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (fabs(s->a0) > DBL_EPSILON) {
profile->t[3] = s->vd / s->a0;
if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, aMax, aMin)) {
return true;
}
} else if (fabs(s->vd) < DBL_EPSILON) {
if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, aMax, aMin)) {
return true;
}
}
return false;
}
/* ---- Public interface ---- */
void cruckig_vel3_step1_init(CRuckigVelocityThirdOrderStep1 *s,
double v0, double a0, double vf, double af,
double aMax, double aMin, double jMax)
{
s->a0 = a0;
s->af = af;
s->_aMax = aMax;
s->_aMin = aMin;
s->_jMax = jMax;
s->vd = vf - v0;
}
bool cruckig_vel3_step1_get_profile(CRuckigVelocityThirdOrderStep1 *s,
const CRuckigProfile *input, CRuckigBlock *block)
{
/* Zero-limits special case */
if (s->_jMax == 0.0) {
CRuckigProfile *p = &block->p_min;
cruckig_profile_set_boundary_from_profile(p, input);
if (time_all_single_step(s, p, s->_aMax, s->_aMin, s->_jMax)) {
block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration;
if (fabs(s->a0) > DBL_EPSILON) {
block->a.valid = true;
block->a.left = block->t_min;
block->a.right = INFINITY;
}
return true;
}
return false;
}
size_t valid_profile_counter = 0;
cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input);
if (fabs(s->af) < DBL_EPSILON) {
/* There is no blocked interval when af==0, so return after first found profile */
const double aMax = (s->vd >= 0) ? s->_aMax : s->_aMin;
const double aMin = (s->vd >= 0) ? s->_aMin : s->_aMax;
const double jMax = (s->vd >= 0) ? s->_jMax : -s->_jMax;
time_none(s, s->valid_profiles, &valid_profile_counter, aMax, aMin, jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0(s, s->valid_profiles, &valid_profile_counter, aMax, aMin, jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_none(s, s->valid_profiles, &valid_profile_counter, aMin, aMax, -jMax, true);
if (valid_profile_counter > 0) { goto return_block; }
time_acc0(s, s->valid_profiles, &valid_profile_counter, aMin, aMax, -jMax, true);
} else {
time_none(s, s->valid_profiles, &valid_profile_counter, s->_aMax, s->_aMin, s->_jMax, false);
time_none(s, s->valid_profiles, &valid_profile_counter, s->_aMin, s->_aMax, -s->_jMax, false);
time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_aMax, s->_aMin, s->_jMax, false);
time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_aMin, s->_aMax, -s->_jMax, false);
}
return_block:
return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 3);
}

146
wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step2.c vendored Normal file
View File

@@ -0,0 +1,146 @@
/*
* cruckig - Pure C99 port of the Ruckig trajectory generation library
* Copyright (c) 2025 Yang Yang <mika-net@outlook.com>
* Copyright (c) 2021 Lars Berscheid (original C++ Ruckig)
*
* License: MIT, see the LICENSE file in this directory.
*/
#include "velocity.h"
#include "block.h"
#include "profile.h"
/* ---- Internal helper functions ---- */
static bool time_acc0(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile,
double aMax, double aMin, double jMax)
{
/* UD Solution 1/2 */
{
const double h1 = sqrt((-s->ad * s->ad + 2 * jMax * ((s->a0 + s->af) * s->tf - 2 * s->vd)) / (jMax * jMax) + s->tf * s->tf);
profile->t[0] = s->ad / (2 * jMax) + (s->tf - h1) / 2;
profile->t[1] = h1;
profile->t[2] = s->tf - (profile->t[0] + h1);
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UU Solution */
{
const double h1 = (-s->ad + jMax * s->tf);
profile->t[0] = -s->ad * s->ad / (2 * jMax * h1) + (s->vd - s->a0 * s->tf) / h1;
profile->t[1] = -s->ad / jMax + s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = s->tf - (profile->t[0] + profile->t[1]);
if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UU Solution - 2 step */
{
profile->t[0] = 0;
profile->t[1] = -s->ad / jMax + s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = s->ad / jMax;
if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) {
profile->pf = profile->p[7];
return true;
}
}
return false;
}
static bool time_none(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile,
double aMax, double aMin, double jMax)
{
if (fabs(s->a0) < DBL_EPSILON && fabs(s->af) < DBL_EPSILON && fabs(s->vd) < DBL_EPSILON) {
profile->t[0] = 0;
profile->t[1] = s->tf;
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, jMax, aMax, aMin)) {
profile->pf = profile->p[7];
return true;
}
}
/* UD Solution 1/2 */
{
const double h1 = 2 * (s->af * s->tf - s->vd);
profile->t[0] = h1 / s->ad;
profile->t[1] = s->tf - profile->t[0];
profile->t[2] = 0;
profile->t[3] = 0;
profile->t[4] = 0;
profile->t[5] = 0;
profile->t[6] = 0;
const double jf = s->ad * s->ad / h1;
if (fabs(jf) < fabs(jMax) + 1e-12 && cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, jf, aMax, aMin)) {
profile->pf = profile->p[7];
return true;
}
}
return false;
}
static bool check_all(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile,
double aMax, double aMin, double jMax)
{
return time_acc0(s, profile, aMax, aMin, jMax) || time_none(s, profile, aMax, aMin, jMax);
}
/* ---- Public interface ---- */
void cruckig_vel3_step2_init(CRuckigVelocityThirdOrderStep2 *s,
double tf, double v0, double a0, double vf, double af,
double aMax, double aMin, double jMax)
{
s->a0 = a0;
s->tf = tf;
s->af = af;
s->_aMax = aMax;
s->_aMin = aMin;
s->_jMax = jMax;
s->vd = vf - v0;
s->ad = af - a0;
}
bool cruckig_vel3_step2_get_profile(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile)
{
/* Test all cases to get ones that match */
/* However we should guess which one is correct and try them first... */
if (s->vd > 0) {
return check_all(s, profile, s->_aMax, s->_aMin, s->_jMax) || check_all(s, profile, s->_aMin, s->_aMax, -s->_jMax);
}
return check_all(s, profile, s->_aMin, s->_aMax, -s->_jMax) || check_all(s, profile, s->_aMax, s->_aMin, s->_jMax);
}

680
wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.c vendored Normal file
View File

@@ -0,0 +1,680 @@
/********************************************************************
* Description: ruckig_wrapper.c
* Cruckig (pure C) trajectory planning library wrapper implementation
*
* This file provides a C wrapper around the Cruckig C library
* for S-curve trajectory planning in LinuxCNC.
* Replaces the C++ Ruckig implementation to enable RTAI kernel builds.
*
* License: GPL Version 2
* System: Linux
* Original Author: 杨阳 (mika-net@outlook.com)
* Cruckig port: LinuxCNC contributors
*
* Copyright (c) 2024-2026 All rights reserved.
********************************************************************/
#include "ruckig_wrapper.h"
#include <rtapi.h>
#include <rtapi_math.h>
#include <rtapi_slab.h>
/* LinuxCNC precision constants (consistent with tp_types.h) */
#ifndef TP_POS_EPSILON
#define TP_POS_EPSILON 1e-12
#endif
#ifndef TP_VEL_EPSILON
#define TP_VEL_EPSILON 1e-8
#endif
/* Cruckig C headers */
#include "cruckig/cruckig.h"
/* Internal implementation struct */
struct RuckigPlannerImpl {
CRuckig *otg; /* cruckig planner instance */
CRuckigInputParameter *input; /* input parameters */
CRuckigTrajectory *trajectory; /* trajectory result */
double cycle_time; /* cycle time */
int planned; /* whether planning has been done */
double start_time; /* trajectory start time */
double target_pos; /* target position (used for precision correction) */
double target_vel; /* target velocity (used for precision correction) */
double target_acc; /* target acceleration (used for precision correction) */
int use_position_control; /* 1=position control, 0=velocity control */
double last_actual_acc; /* previous actual acceleration (for jerk calculation) */
int is_first_cycle; /* first cycle after replanning */
int enable_logging; /* 1=enabled, 0=disabled */
};
/* Helper macro: conditionally output log based on planner's logging setting */
#define RUCKIG_LOG_IF_ENABLED(planner, level, fmt, ...) \
do { \
if (planner) { \
struct RuckigPlannerImpl *_impl = (struct RuckigPlannerImpl *)planner; \
if (_impl->enable_logging) { \
rtapi_print_msg(level, fmt, ##__VA_ARGS__); \
} \
} else { \
rtapi_print_msg(level, fmt, ##__VA_ARGS__); \
} \
} while (0)
RuckigPlanner ruckig_create(double cycle_time) {
if (cycle_time <= 0.0) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: invalid cycle_time %f\n", cycle_time);
return NULL;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)rtapi_kmalloc(sizeof(struct RuckigPlannerImpl), RTAPI_GFP_KERNEL);
if (!impl) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: memory allocation failed\n");
return NULL;
}
impl->otg = cruckig_create(1, cycle_time);
impl->input = cruckig_input_create(1);
impl->trajectory = cruckig_trajectory_create(1);
if (!impl->otg || !impl->input || !impl->trajectory) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: cruckig allocation failed\n");
if (impl->otg) cruckig_destroy(impl->otg);
if (impl->input) cruckig_input_destroy(impl->input);
if (impl->trajectory) cruckig_trajectory_destroy(impl->trajectory);
rtapi_kfree(impl);
return NULL;
}
impl->cycle_time = cycle_time;
impl->planned = 0;
impl->start_time = 0.0;
impl->target_pos = 0.0;
impl->target_vel = 0.0;
impl->target_acc = 0.0;
impl->use_position_control = 0;
impl->last_actual_acc = 0.0;
impl->is_first_cycle = 0;
impl->enable_logging = 1;
return (RuckigPlanner)impl;
}
void ruckig_destroy(RuckigPlanner planner) {
if (planner) {
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (impl->otg) cruckig_destroy(impl->otg);
if (impl->input) cruckig_input_destroy(impl->input);
if (impl->trajectory) cruckig_trajectory_destroy(impl->trajectory);
rtapi_kfree(impl);
}
}
/* Helper: copy trajectory state for backup/restore on planning failure.
* We cannot just memcpy the CRuckigTrajectory because it contains owned pointers.
* Instead we save/restore the profile data and scalar fields. */
struct TrajectoryBackup {
CRuckigProfile profile; /* single-DOF single-section profile copy */
double duration;
double cumulative_time;
double independent_min_duration;
CRuckigBound position_extremum;
};
static void backup_trajectory(const CRuckigTrajectory *traj, struct TrajectoryBackup *bk) {
bk->duration = traj->duration;
if (traj->profiles)
bk->profile = traj->profiles[0]; /* 1 DOF, 1 section */
if (traj->cumulative_times)
bk->cumulative_time = traj->cumulative_times[0];
if (traj->independent_min_durations)
bk->independent_min_duration = traj->independent_min_durations[0];
if (traj->position_extrema)
bk->position_extremum = traj->position_extrema[0];
}
static void restore_trajectory(CRuckigTrajectory *traj, const struct TrajectoryBackup *bk) {
traj->duration = bk->duration;
if (traj->profiles)
traj->profiles[0] = bk->profile;
if (traj->cumulative_times)
traj->cumulative_times[0] = bk->cumulative_time;
if (traj->independent_min_durations)
traj->independent_min_durations[0] = bk->independent_min_duration;
if (traj->position_extrema)
traj->position_extrema[0] = bk->position_extremum;
}
/* Helper: handle cruckig result codes, return 0 on success, -1 or -2 on failure.
* On failure with a previous plan, restores the backup. */
static int handle_result(CRuckigResult result, RuckigPlanner planner,
const char *func_name,
int had_previous_plan,
const struct TrajectoryBackup *bk,
double bk_target_pos, double bk_target_vel,
int bk_use_position_control, double bk_last_actual_acc) {
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (result == CRuckigWorking || result == CRuckigFinished) {
if (result == CRuckigFinished) {
double duration = cruckig_trajectory_get_duration(impl->trajectory);
if (duration < 0.001) {
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO,
"%s: already at target (duration=%f)\n", func_name, duration);
} else {
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO,
"%s: trajectory finished (duration=%f)\n", func_name, duration);
}
}
return 0; /* success */
}
/* Planning failed: restore previous trajectory if it exists */
if (had_previous_plan) {
restore_trajectory(impl->trajectory, bk);
impl->target_pos = bk_target_pos;
impl->target_vel = bk_target_vel;
impl->use_position_control = bk_use_position_control;
impl->last_actual_acc = bk_last_actual_acc;
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO,
"%s: planning failed, restored previous trajectory\n", func_name);
}
/* Log error */
switch (result) {
case CRuckigErrorInvalidInput:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: invalid input parameters\n", func_name);
break;
case CRuckigErrorTrajectoryDuration:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: trajectory duration exceeds numerical limits\n", func_name);
break;
case CRuckigErrorPositionalLimits:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: positional limits exceeded\n", func_name);
break;
case CRuckigErrorZeroLimits:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: zero limits conflict\n", func_name);
break;
case CRuckigErrorExecutionTimeCalculation:
return -2;
case CRuckigErrorSynchronizationCalculation:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: synchronization calculation error\n", func_name);
break;
case CRuckigError:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: general error\n", func_name);
break;
default:
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"%s: unknown error result %d\n", func_name, (int)result);
break;
}
return -1;
}
int ruckig_plan_position(RuckigPlanner planner,
double current_pos,
double current_vel,
double current_acc,
double target_pos,
double target_vel,
double target_acc,
double min_vel,
double max_vel,
double max_acc,
double max_jerk) {
if (!planner) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
/* Parameter validation */
if (max_vel <= 0.0 || max_acc <= 0.0 || max_jerk <= 0.0) {
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"ruckig_plan_position: invalid limits (v=%f, a=%f, j=%f)\n",
max_vel, max_acc, max_jerk);
return -1;
}
/* Set input parameters (position control mode) */
impl->input->control_interface = CRuckigPosition;
impl->input->synchronization = CRuckigSyncTime;
impl->input->current_position[0] = current_pos;
impl->input->current_velocity[0] = current_vel;
impl->input->current_acceleration[0] = current_acc;
impl->input->target_position[0] = target_pos;
impl->input->target_velocity[0] = target_vel;
impl->input->target_acceleration[0] = target_acc;
impl->input->max_velocity[0] = max_vel;
impl->input->max_acceleration[0] = max_acc;
impl->input->max_jerk[0] = max_jerk;
/* Set min_velocity: cruckig uses NULL for default (-max), or a pointer for explicit */
if (impl->input->min_velocity == NULL) {
impl->input->min_velocity = (double *)rtapi_kmalloc(sizeof(double), RTAPI_GFP_KERNEL);
if (!impl->input->min_velocity) return -1;
}
impl->input->min_velocity[0] = min_vel;
/* Backup trajectory on failure */
int had_previous_plan = impl->planned;
struct TrajectoryBackup bk;
double bk_target_pos = 0.0, bk_target_vel = 0.0, bk_last_actual_acc = 0.0;
int bk_use_position_control = 0;
if (had_previous_plan) {
backup_trajectory(impl->trajectory, &bk);
bk_target_pos = impl->target_pos;
bk_target_vel = impl->target_vel;
bk_use_position_control = impl->use_position_control;
bk_last_actual_acc = impl->last_actual_acc;
}
/* Execute planning */
CRuckigResult result = cruckig_calculate(impl->otg, impl->input, impl->trajectory);
int rc = handle_result(result, planner, "ruckig_plan_position",
had_previous_plan, &bk,
bk_target_pos, bk_target_vel,
bk_use_position_control, bk_last_actual_acc);
if (rc != 0) return rc;
/* Update state on success */
int was_planned = impl->planned;
if (!was_planned) {
impl->last_actual_acc = current_acc;
}
impl->planned = 1;
impl->start_time = 0.0;
impl->target_pos = target_pos;
impl->target_vel = target_vel;
impl->target_acc = target_acc;
impl->use_position_control = 1;
impl->is_first_cycle = 1;
return 0;
}
int ruckig_plan_velocity(RuckigPlanner planner,
double current_vel,
double current_acc,
double target_vel,
double target_acc,
double min_vel,
double max_acc,
double max_jerk) {
if (!planner) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
/* Parameter validation */
if (max_acc <= 0.0 || max_jerk <= 0.0) {
RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR,
"ruckig_plan_velocity: invalid limits (a=%f, j=%f)\n",
max_acc, max_jerk);
return -1;
}
/* Set input parameters (velocity control mode) */
impl->input->control_interface = CRuckigVelocity;
impl->input->synchronization = CRuckigSyncNone;
impl->input->current_position[0] = 0.0;
impl->input->current_velocity[0] = current_vel;
impl->input->current_acceleration[0] = current_acc;
impl->input->target_position[0] = 0.0;
impl->input->target_velocity[0] = target_vel;
impl->input->target_acceleration[0] = target_acc;
impl->input->max_velocity[0] = INFINITY;
impl->input->max_acceleration[0] = max_acc;
impl->input->max_jerk[0] = max_jerk;
/* Set min_velocity */
if (impl->input->min_velocity == NULL) {
impl->input->min_velocity = (double *)rtapi_kmalloc(sizeof(double), RTAPI_GFP_KERNEL);
if (!impl->input->min_velocity) return -1;
}
impl->input->min_velocity[0] = min_vel;
/* Backup trajectory on failure */
int had_previous_plan = impl->planned;
struct TrajectoryBackup bk;
double bk_target_pos = 0.0, bk_target_vel = 0.0, bk_last_actual_acc = 0.0;
int bk_use_position_control = 0;
if (had_previous_plan) {
backup_trajectory(impl->trajectory, &bk);
bk_target_pos = impl->target_pos;
bk_target_vel = impl->target_vel;
bk_use_position_control = impl->use_position_control;
bk_last_actual_acc = impl->last_actual_acc;
}
/* Execute planning */
CRuckigResult result = cruckig_calculate(impl->otg, impl->input, impl->trajectory);
int rc = handle_result(result, planner, "ruckig_plan_velocity",
had_previous_plan, &bk,
bk_target_pos, bk_target_vel,
bk_use_position_control, bk_last_actual_acc);
if (rc != 0) return rc;
/* Update state on success */
int was_planned = impl->planned;
if (!was_planned) {
impl->last_actual_acc = current_acc;
}
impl->planned = 1;
impl->start_time = 0.0;
impl->target_pos = 0.0;
impl->target_vel = target_vel;
impl->target_acc = target_acc;
impl->use_position_control = 0;
impl->is_first_cycle = 1;
return 0;
}
int ruckig_at_time(RuckigPlanner planner,
double time,
double *pos,
double *vel,
double *acc,
double *jerk) {
if (!planner || !pos || !vel || !acc || !jerk) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_at_time: trajectory not planned\n");
return -1;
}
double duration = cruckig_trajectory_get_duration(impl->trajectory);
/* Clamp time */
double query_time = time;
if (time < 0.0) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_at_time: time %f is negative\n", time);
return -1;
}
if (time > duration) {
query_time = duration;
}
/* Get state at specified time */
double new_pos, new_vel, new_acc, new_jerk_unused;
size_t new_section;
cruckig_trajectory_at_time(impl->trajectory, query_time,
&new_pos, &new_vel, &new_acc, &new_jerk_unused,
&new_section);
*pos = new_pos;
*vel = new_vel;
*acc = new_acc;
/* Precision correction: ensure position and velocity exactly match target values */
if (impl->use_position_control) {
const double TIME_THRESHOLD = fmax(duration * 0.1, impl->cycle_time * 10.0);
const double POS_ERROR_THRESHOLD = 1e-6;
if (time >= duration - TIME_THRESHOLD || time >= duration) {
double pos_error = fabs(*pos - impl->target_pos);
if (pos_error < POS_ERROR_THRESHOLD) {
*pos = impl->target_pos;
}
if (time >= duration) {
*vel = impl->target_vel;
}
/* During trajectory: let S-curve complete naturally */
}
} else {
/* Velocity control mode: only correct at trajectory end */
if (time >= duration) {
*vel = impl->target_vel;
*acc = impl->target_acc;
}
}
/* Calculate jerk */
if (time > duration) {
if (impl->use_position_control) {
double pos_error = fabs(*pos - impl->target_pos);
double vel_error = fabs(*vel - impl->target_vel);
double acc_threshold = 1e-6;
int acc_near_zero = (fabs(*acc) < acc_threshold);
if (pos_error < TP_POS_EPSILON * 100.0 && vel_error < TP_VEL_EPSILON * 10.0 && acc_near_zero) {
*jerk = 0.0;
*acc = 0.0;
}
} else {
double vel_error = fabs(*vel - impl->target_vel);
double acc_threshold = 1e-6;
int acc_near_zero = (fabs(*acc) < acc_threshold);
if (vel_error < TP_VEL_EPSILON * 10.0 && acc_near_zero) {
*jerk = 0.0;
*acc = 0.0;
}
}
} else if (query_time > impl->cycle_time) {
/* Compute jerk from acceleration difference */
double prev_pos, prev_vel, prev_acc_val, prev_jerk_unused;
size_t prev_section;
double prev_time = query_time - impl->cycle_time;
if (prev_time < 0.0) prev_time = 0.0;
cruckig_trajectory_at_time(impl->trajectory, prev_time,
&prev_pos, &prev_vel, &prev_acc_val, &prev_jerk_unused,
&prev_section);
*jerk = (new_acc - prev_acc_val) / impl->cycle_time;
} else {
/* First cycle after replanning */
if (impl->is_first_cycle) {
double base_acc = impl->last_actual_acc;
*jerk = (new_acc - base_acc) / impl->cycle_time;
impl->is_first_cycle = 0;
} else {
/* Use initial acceleration from planning time */
*jerk = (query_time > 0.0) ?
(new_acc - impl->input->current_acceleration[0]) / query_time : 0.0;
}
}
/* Save current acceleration for jerk calculation in next cycle */
impl->last_actual_acc = *acc;
return 0;
}
int ruckig_next_cycle(RuckigPlanner planner,
double current_time,
double cycle_time,
double *pos,
double *vel,
double *acc,
double *jerk) {
if (!planner) {
return -1;
}
double next_time = current_time + cycle_time;
return ruckig_at_time(planner, next_time, pos, vel, acc, jerk);
}
double ruckig_get_duration(RuckigPlanner planner) {
if (!planner) {
return -1.0;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
return -1.0;
}
return cruckig_trajectory_get_duration(impl->trajectory);
}
int ruckig_is_finished(RuckigPlanner planner, double current_time) {
if (!planner) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
return -1;
}
double duration = ruckig_get_duration(planner);
if (duration < 0.0) {
return -1;
}
return (current_time >= duration) ? 1 : 0;
}
void ruckig_reset(RuckigPlanner planner) {
if (!planner) {
return;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
/* Reset all state fields */
impl->planned = 0;
impl->start_time = 0.0;
impl->target_pos = 0.0;
impl->target_vel = 0.0;
impl->target_acc = 0.0;
impl->use_position_control = 0;
impl->last_actual_acc = 0.0;
impl->is_first_cycle = 0;
/* Note: do not reset enable_logging, preserve user setting */
/* Reset cruckig objects */
cruckig_reset(impl->otg);
}
void ruckig_set_logging(RuckigPlanner planner, int enable) {
if (!planner) {
return;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
impl->enable_logging = (enable != 0) ? 1 : 0;
}
int ruckig_get_decelerate_phases(RuckigPlanner planner, double *t1, double *t2) {
if (!planner) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_decelerate_phases: trajectory not planned\n");
return -1;
}
/* Get Profile (1 DOF, section 0) */
const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0);
if (!profile) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_decelerate_phases: no profile available\n");
return -1;
}
/* Deceleration phases: t[4]=T1 (jerk), t[5]=T2 (constant accel) */
if (t1 != NULL) {
*t1 = (profile->t[4] > 0.0) ? profile->t[4] : 0.0;
}
if (t2 != NULL) {
*t2 = (profile->t[5] > 0.0) ? profile->t[5] : 0.0;
}
return 0;
}
int ruckig_get_peak_velocity(RuckigPlanner planner, double *peak_vel) {
if (!planner || !peak_vel) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_peak_velocity: trajectory not planned\n");
return -1;
}
const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0);
if (!profile) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_peak_velocity: no profile available\n");
return -1;
}
/* Peak velocity is the maximum of v[0] through v[7] */
double max_v = 0.0;
size_t i;
for (i = 0; i < 8; i++) {
if (profile->v[i] > max_v) {
max_v = profile->v[i];
}
}
*peak_vel = max_v;
return 0;
}
int ruckig_get_start_velocity(RuckigPlanner planner, double *start_vel) {
if (!planner || !start_vel) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_start_velocity: trajectory not planned\n");
return -1;
}
const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0);
if (!profile) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_start_velocity: no profile available\n");
return -1;
}
*start_vel = profile->v[0];
return 0;
}
int ruckig_get_time_at_position(RuckigPlanner planner, double position, double time_after, double *time) {
if (!planner || time == NULL) {
return -1;
}
struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner;
if (!impl->planned) {
rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_time_at_position: trajectory not planned\n");
return -1;
}
double result_time;
if (cruckig_trajectory_get_first_time_at_position(impl->trajectory, 0, position,
&result_time, time_after)) {
*time = result_time;
return 0;
} else {
return -1;
}
}

221
wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.h vendored Normal file
View File

@@ -0,0 +1,221 @@
/********************************************************************
* Description: ruckig_wrapper.h
* Ruckig trajectory planning library wrapper for LinuxCNC
*
* This wrapper provides a C interface to Ruckig C++ library
* for S-curve trajectory planning.
*
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2024 All rights reserved.
********************************************************************/
#ifndef RUCKIG_WRAPPER_H
#define RUCKIG_WRAPPER_H
#include <rtapi.h>
#ifdef __cplusplus
extern "C" {
#endif
/**
* Ruckig trajectory planner handle (opaque pointer)
*/
typedef void* RuckigPlanner;
/**
* Create a Ruckig trajectory planner.
*
* @param cycle_time cycle time in seconds
* @return planner handle, or NULL on failure
*/
RuckigPlanner ruckig_create(double cycle_time);
/**
* Destroy a Ruckig trajectory planner.
*
* @param planner planner handle
*/
void ruckig_destroy(RuckigPlanner planner);
/**
* Plan an S-curve trajectory in position control mode.
*
* Given the initial and target states, plan a complete S-curve trajectory.
*
* @param planner planner handle
* @param current_pos current position
* @param current_vel current velocity
* @param current_acc current acceleration
* @param target_pos target position
* @param target_vel target velocity (usually 0)
* @param target_acc target acceleration (usually 0)
* @param min_vel minimum velocity limit (set to 0 for unidirectional motion)
* @param max_vel maximum velocity limit
* @param max_acc maximum acceleration limit
* @param max_jerk maximum jerk limit
* @return 0 on success, -1 on failure (insufficient distance or invalid params)
*/
int ruckig_plan_position(RuckigPlanner planner,
double current_pos,
double current_vel,
double current_acc,
double target_pos,
double target_vel,
double target_acc,
double min_vel,
double max_vel,
double max_acc,
double max_jerk);
/**
* Plan an S-curve trajectory in velocity control mode (for stop/pause).
*
* Uses velocity control mode, ignoring target position.
* Suitable for stop or pause scenarios where deceleration may span segments.
*
* @param planner planner handle
* @param current_vel current velocity
* @param current_acc current acceleration
* @param target_vel target velocity (0 for stop)
* @param target_acc target acceleration (usually 0)
* @param min_vel minimum velocity limit (set to 0 for unidirectional motion)
* @param max_acc maximum acceleration limit
* @param max_jerk maximum jerk limit
* @return 0 on success, -1 on failure (invalid params)
*/
int ruckig_plan_velocity(RuckigPlanner planner,
double current_vel,
double current_acc,
double target_vel,
double target_acc,
double min_vel,
double max_acc,
double max_jerk);
/**
* Get the motion state at a specified time.
*
* @param planner planner handle
* @param time time in seconds (from trajectory start)
* @param pos [out] position
* @param vel [out] velocity
* @param acc [out] acceleration
* @param jerk [out] jerk
* @return 0 on success, -1 on failure (time out of range)
*/
int ruckig_at_time(RuckigPlanner planner,
double time,
double *pos,
double *vel,
double *acc,
double *jerk);
/**
* Get the motion state at the next cycle.
*
* Computes the state at (current_time + cycle_time).
*
* @param planner planner handle
* @param current_time current time in seconds (from trajectory start)
* @param cycle_time cycle time in seconds
* @param pos [out] position
* @param vel [out] velocity
* @param acc [out] acceleration
* @param jerk [out] jerk
* @return 0 on success, -1 on failure (time out of range or not planned)
*/
int ruckig_next_cycle(RuckigPlanner planner,
double current_time,
double cycle_time,
double *pos,
double *vel,
double *acc,
double *jerk);
/**
* Get total trajectory duration.
*
* @param planner planner handle
* @return total time in seconds, or -1.0 on failure
*/
double ruckig_get_duration(RuckigPlanner planner);
/**
* Check if the trajectory has completed.
*
* @param planner planner handle
* @param current_time current time in seconds
* @return 1 if finished, 0 if not, -1 on error
*/
int ruckig_is_finished(RuckigPlanner planner, double current_time);
/**
* Reset the planner state.
*
* Clears previous planning results, preparing for new planning.
*
* @param planner planner handle
*/
void ruckig_reset(RuckigPlanner planner);
/**
* Enable or disable log output.
*
* Controls whether the planner outputs error and warning messages.
* For velocity planning scenarios (e.g. sp_scurve.c), logging can be
* disabled to avoid unnecessary warnings.
*
* @param planner planner handle
* @param enable 1=enable logging, 0=disable logging
*/
void ruckig_set_logging(RuckigPlanner planner, int enable);
/**
* Get the deceleration phase durations (T1 and T2) from the Ruckig profile.
*
* T1: time for acceleration to change from 0 to -amax (jerk phase)
* T2: time at constant -amax acceleration (constant accel phase)
*
* @param planner planner handle (must have completed planning)
* @param t1 [out] T1 time (jerk phase), NULL if not needed
* @param t2 [out] T2 time (constant accel phase), NULL if not needed
* @return 0 on success, -1 on failure (not planned or cannot retrieve)
*/
int ruckig_get_decelerate_phases(RuckigPlanner planner, double *t1, double *t2);
/**
* Get the peak velocity of the trajectory.
*
* @param planner planner handle (must have completed planning)
* @param peak_vel [out] peak velocity
* @return 0 on success, -1 on failure (not planned or cannot retrieve)
*/
int ruckig_get_peak_velocity(RuckigPlanner planner, double *peak_vel);
/**
* Get the start velocity of the trajectory.
*
* @param planner planner handle (must have completed planning)
* @param start_vel [out] start velocity
* @return 0 on success, -1 on failure (not planned or cannot retrieve)
*/
int ruckig_get_start_velocity(RuckigPlanner planner, double *start_vel);
/**
* Get the time at which the trajectory first reaches a given position.
*
* @param planner planner handle (must have completed planning)
* @param position target position
* @param time_after start query time (optional, default 0.0)
* @param time [out] time at which position is reached
* @return 0 on success, -1 on failure (not planned, position unreachable)
*/
int ruckig_get_time_at_position(RuckigPlanner planner, double position, double time_after, double *time);
#ifdef __cplusplus
}
#endif
#endif /* RUCKIG_WRAPPER_H */

653
wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.c vendored Normal file
View File

@@ -0,0 +1,653 @@
/*!
********************************************************************
* Description: sp_scurve.c
*\brief Ruckig-based S-curve trajectory planning with legacy helpers
*
*\author Derived from a work by Yang Yang
*
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#include <rtapi.h>
#include <rtapi_math.h>
#include "sp_scurve.h"
#include "tp_types.h"
#include "ruckig_wrapper.h"
#ifndef __KERNEL__
#include <stdio.h>
#include <string.h>
#endif
/* ========== Cached Ruckig planner ==========
* Use a static variable to cache the planner, avoiding creation and
* destruction on every call.
*/
static RuckigPlanner cached_planner = NULL;
static double cached_cycle_time = 0.0; /* cycle time used by the current planner */
/**
* @brief Initialize the S-curve planner (call at program entry).
*
* @param cycle_time cycle time in seconds
* @return 0 on success, -1 on failure
*/
int sp_scurve_init(double cycle_time) {
/* Parameter validation */
if (cycle_time <= 0.0) {
rtapi_print_msg(RTAPI_MSG_ERR, "sp_scurve_init: invalid cycle_time=%f\n", cycle_time);
return -1;
}
/* If planner already exists with the same cycle time, nothing to do */
if (cached_planner != NULL && fabs(cached_cycle_time - cycle_time) < 1e-12) {
return 0;
}
/* If planner exists but cycle time changed, destroy the old one first */
if (cached_planner != NULL) {
rtapi_print_msg(RTAPI_MSG_INFO, "sp_scurve_init: cycle time changed from %f to %f, recreating planner\n",
cached_cycle_time, cycle_time);
ruckig_destroy(cached_planner);
cached_planner = NULL;
}
/* Create new planner */
cached_planner = ruckig_create(cycle_time);
if (cached_planner == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, "sp_scurve_init: ruckig_create() failed with cycle_time=%f\n", cycle_time);
return -1;
}
/* Disable log output (used for velocity planning — avoids unnecessary warnings) */
ruckig_set_logging(cached_planner, 0);
cached_cycle_time = cycle_time;
rtapi_print_msg(RTAPI_MSG_INFO, "sp_scurve_init: planner created with cycle_time=%f (logging disabled)\n", cycle_time);
return 0;
}
/**
* @brief Clean up the S-curve planner (call at program exit).
*/
void sp_scurve_cleanup(void) {
if (cached_planner != NULL) {
ruckig_destroy(cached_planner);
cached_planner = NULL;
cached_cycle_time = 0.0;
}
}
/**
* @brief Get the cached Ruckig planner.
*
* Note: sp_scurve_init() must be called before using this.
*
* @return RuckigPlanner handle, or NULL if not initialized
*/
static RuckigPlanner get_cached_planner(void) {
/* If planner is not initialized, return NULL.
* Callers should check the return value and handle the error. */
return cached_planner;
}
/* ================================================================
* Ruckig-based S-curve functions
* ================================================================ */
/**
* @brief Compute the S-curve peak velocity from rest to end-speed
* (using Ruckig planning).
*
* Given total distance and end velocity, plan a complete trajectory
* from (0, 0, 0) to (distance, Ve, 0), then read the peak velocity
* directly from the profile — no iteration required.
*
* @param distance total distance
* @param Ve end velocity
* @param maxA maximum acceleration
* @param maxJ maximum jerk
* @param req_v [out] computed peak velocity
* @return 1 on success, -1 on failure
*/
int findSCurveVSpeedWithEndSpeed(double distance, double Ve,
double maxA, double maxJ, double* req_v) {
/* Parameter validation */
if (distance <= 0 || maxA <= 0 || maxJ <= 0) {
*req_v = fabs(Ve);
return -1;
}
/* When Ve is approximately zero, use the symmetric function */
if (fabs(Ve) <= TP_VEL_EPSILON) {
return findSCurveVSpeed(distance, maxA, maxJ, req_v);
}
/* Use the cached planner */
RuckigPlanner planner = get_cached_planner();
if (!planner) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: planner not initialized, call sp_scurve_init() first\n");
*req_v = fabs(Ve);
return -1;
}
/* Reset planner state */
ruckig_reset(planner);
/* Plan a complete trajectory from (0, 0, 0) to (distance, Ve, 0).
* Ruckig will automatically find the peak velocity that satisfies
* the distance and end-velocity constraints. */
int result = ruckig_plan_position(planner,
0.0, /* start position */
0.0, /* start velocity */
0.0, /* start acceleration */
distance, /* target position */
Ve, /* target velocity */
0.0, /* target acceleration */
0.0, /* min velocity (unidirectional) */
sqrt(maxA * distance + Ve * Ve) * 2.0, /* max velocity (conservative, ensures no limiting) */
maxA, /* max acceleration */
maxJ); /* max jerk */
if (result != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: ruckig_plan_position failed (result=%d)\n", result);
*req_v = fabs(Ve);
return -1;
}
/* Read the peak velocity directly from the profile */
double peak_vel = 0.0;
result = ruckig_get_peak_velocity(planner, &peak_vel);
if (result != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: ruckig_get_peak_velocity failed\n");
*req_v = fabs(Ve);
return -1;
}
*req_v = peak_vel;
return 1;
}
/**
* @brief Compute the maximum start speed that can decelerate to Ve within
* a given distance (jerk-constrained).
*
* Find the largest Vs such that a trajectory exists from (0, Vs, 0) to
* (distance, Ve, 0) under (maxA, maxJ) constraints.
*
* Method: use the constant-acceleration upper bound
* Vs_estimate = sqrt(Ve^2 + 2*maxA*distance)
* as an initial guess and pass it to Ruckig. If planning succeeds,
* Vs_estimate is feasible. If it fails, the jerk constraint requires
* more distance — return a guaranteed-feasible upper bound instead.
*
* On failure, instead of returning 0.9*Vs_estimate (which may still
* exceed the jerk-feasible value), return the 0->0 S-curve peak for
* the same distance. That value is always jerk-feasible and prevents
* downstream planning failures. On success the same peak is used as
* an upper-bound clamp.
*
* @param distance total distance
* @param Ve end velocity
* @param maxA maximum acceleration
* @param maxJ maximum jerk
* @param req_v [out] computed maximum start speed
* @return 1 on success, -1 on failure
*/
int findSCurveMaxStartSpeed(double distance, double Ve,
double maxA, double maxJ, double* req_v) {
if (distance <= 0 || maxA <= 0 || maxJ <= 0) {
*req_v = fabs(Ve);
return -1;
}
if (fabs(Ve) <= TP_VEL_EPSILON) {
return findSCurveVSpeed(distance, maxA, maxJ, req_v);
}
/* 0->0 S-curve peak for this distance — reliable jerk-constrained upper bound,
* used as fallback on failure and as a clamp on success. */
double v_0_to_0_peak = 0.0;
if (findSCurveVSpeed(distance, maxA, maxJ, &v_0_to_0_peak) != 1) {
/* findSCurveVSpeed failed: use triangular upper bound to avoid unbounded result */
v_0_to_0_peak = sqrt(maxA * distance);
}
RuckigPlanner planner = get_cached_planner();
if (!planner) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveMaxStartSpeed: planner not initialized, call sp_scurve_init() first\n");
*req_v = fmin(fabs(Ve) * 2.0, v_0_to_0_peak);
return -1;
}
ruckig_reset(planner);
double Vs_estimate = sqrt(Ve * Ve + 2.0 * maxA * distance);
if (Vs_estimate < fabs(Ve)) {
Vs_estimate = fabs(Ve) * 2.0;
}
int result = ruckig_plan_position(planner,
0.0,
Vs_estimate,
0.0,
distance,
Ve,
0.0,
0.0,
Vs_estimate * 2.0,
maxA,
maxJ);
if (result == 0) {
double duration = ruckig_get_duration(planner);
if (duration > 0.0) {
double actual_pos, actual_vel, actual_acc, actual_jerk;
int query_result = ruckig_at_time(planner, duration,
&actual_pos, &actual_vel,
&actual_acc, &actual_jerk);
if (query_result == 0) {
double pos_error = fabs(actual_pos - distance);
if (pos_error < 1e-6) {
double start_vel = 0.0;
if (ruckig_get_start_velocity(planner, &start_vel) == 0) {
*req_v = fmin(start_vel, v_0_to_0_peak);
return 1;
}
}
}
}
*req_v = fmin(Vs_estimate, v_0_to_0_peak);
return 1;
}
/* Planning failed: jerk constraint makes Vs_estimate infeasible.
* Return the guaranteed-feasible 0->0 peak to avoid downstream failures. */
*req_v = fmax(fabs(Ve), v_0_to_0_peak);
return 1;
}
/**
* @brief Compute the rest-to-rest S-curve peak velocity (using Ruckig planning).
*
* Given a total distance, plan a complete trajectory from (0, 0, 0) to
* (distance, 0, 0), then read the peak velocity directly from the
* profile — no iteration required.
*
* @param distence total distance (rest to rest)
* @param maxA maximum acceleration
* @param maxJ maximum jerk
* @param req_v [out] computed peak velocity
* @return 1 on success, -1 on failure
*/
int findSCurveVSpeed(double distence, double maxA, double maxJ, double* req_v){
/* Parameter validation */
if (distence <= 0 || maxA <= 0 || maxJ <= 0) {
*req_v = 0.0;
return -1;
}
/* Use the cached planner */
RuckigPlanner planner = get_cached_planner();
if (!planner) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: planner not initialized, call sp_scurve_init() first\n");
*req_v = 0.0;
return -1;
}
/* Reset planner state */
ruckig_reset(planner);
/* Plan a complete trajectory from (0, 0, 0) to (distance, 0, 0) */
int result = ruckig_plan_position(planner,
0.0, /* start position */
0.0, /* start velocity */
0.0, /* start acceleration */
distence, /* target position */
0.0, /* target velocity */
0.0, /* target acceleration */
0.0, /* min velocity (unidirectional) */
sqrt(maxA * distence) * 2.0, /* max velocity (conservative, ensures no limiting) */
maxA, /* max acceleration */
maxJ); /* max jerk */
if (result != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_plan_position failed (result=%d)\n", result);
*req_v = 0.0;
return -1;
}
/* Read the peak velocity directly from the profile */
double peak_vel = 0.0;
result = ruckig_get_peak_velocity(planner, &peak_vel);
if (result != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_get_peak_velocity failed\n");
*req_v = 0.0;
return -1;
}
*req_v = peak_vel;
return 1;
}
/**
* @brief Compute S-curve deceleration time parameters using analytical formulas
* (real-time optimized version).
*
* S-curve deceleration consists of three phases:
* T1: jerk ramp-up phase (j = -jerk), acceleration goes from 0 to -amax
* T2: constant deceleration phase (j = 0), acceleration stays at -amax
* T1: jerk ramp-down phase (j = +jerk), acceleration goes from -amax to 0
*
* ========== Velocity-time curve ==========
*
* velocity v
* ^
* V |------\
* | \
* | \____
* | \
* | \
* +---------------\----> time t
* 0 T1 T1+T2 2T1+T2
*
* ========== Analytical formula derivation ==========
*
* For S-curve deceleration:
* - T1 = amax / jerk (time for acceleration to go from 0 to -amax)
* - Phase 1 velocity loss: dv1 = 0.5 * jerk * T1^2 = 0.5 * amax^2 / jerk
* - Phase 3 velocity loss: dv3 = 0.5 * jerk * T1^2 = 0.5 * amax^2 / jerk (same as phase 1)
* - Phase 2 velocity loss: dv2 = amax * T2
* - Total velocity loss: v = dv1 + dv2 + dv3 = amax^2 / jerk + amax * T2
* - Therefore: T2 = (v - amax^2 / jerk) / amax
*
* Special case (triangular profile):
* - If v < amax^2 / jerk, the velocity is too small for a full S-curve
* (no constant deceleration phase)
* - For triangular profile: v = jerk * T1^2, so T1 = sqrt(v / jerk), T2 = 0
*
* ========== Optimization notes ==========
*
* This function uses analytical formulas for direct computation, avoiding
* frequent trajectory planning — suitable for real-time system calls.
* Compared to using Ruckig, performance is significantly better and results
* are fully consistent.
*
* @param v initial velocity (absolute value is taken)
* @param amax maximum acceleration
* @param jerk maximum jerk
* @param t1 [out, optional] jerk phase time T1
* @param t2 [out, optional] constant deceleration phase time T2
* @return total deceleration time = 2*T1 + T2
*/
double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2){
v = fabs(v);
/* Parameter validation */
if (v < TP_VEL_EPSILON) {
if (t1 != NULL) *t1 = 0.0;
if (t2 != NULL) *t2 = 0.0;
return 0.0;
}
if (amax <= 0.0 || jerk <= 0.0) {
if (t1 != NULL) *t1 = 0.0;
if (t2 != NULL) *t2 = 0.0;
return 0.0;
}
/* Compute T1 (jerk phase time) */
double T1 = amax / jerk;
/* Total velocity loss from phase 1 and phase 3:
* dv1 + dv3 = 2 * (0.5 * amax^2 / jerk) = amax^2 / jerk */
double v_loss_jerk_phases = amax * amax / jerk;
double T2 = 0.0;
/* Determine whether this is a full S-curve or a triangular profile */
if (v >= v_loss_jerk_phases) {
/* Full S-curve: constant deceleration phase exists */
T2 = (v - v_loss_jerk_phases) / amax;
if (T2 < 0.0) {
T2 = 0.0; /* guard against numerical error */
}
} else {
/* Triangular profile: no constant deceleration phase, recompute T1.
* v = jerk * T1^2, so T1 = sqrt(v / jerk) */
T1 = sqrt(v / jerk);
T2 = 0.0;
}
/* Output results */
if (t1 != NULL) *t1 = T1;
if (t2 != NULL) *t2 = T2;
/* Total time: 2*T1 + T2
* (T1 to ramp accel to -amax, T2 at constant -amax, T1 to ramp back to 0) */
return T1 * 2.0 + T2;
}
/**
* @brief Compute the maximum speed reachable from rest in time T using
* an S-curve profile (via Ruckig planning).
*
* Given maximum acceleration amax, maximum jerk, and time T, compute the
* maximum velocity achievable from rest using an S-curve acceleration
* profile within time T.
*
* Algorithm: use Ruckig position-control mode to plan toward a sufficiently
* large target position (ensuring the target is not reached within time T),
* then sample the velocity at time T.
*
* @param amax maximum acceleration
* @param jerk maximum jerk
* @param T time in seconds
* @return maximum velocity at time T, or 0.0 on failure
*/
double calcSCurveSpeedWithT(double amax, double jerk, double T) {
/* Parameter validation */
if (amax <= 0.0 || jerk <= 0.0 || T <= 0.0) {
return 0.0;
}
/* Use the cached planner */
RuckigPlanner planner = get_cached_planner();
if (!planner) {
rtapi_print_msg(RTAPI_MSG_ERR, "calcSCurveSpeedWithT: planner not initialized, call sp_scurve_init() first\n");
return 0.0;
}
/* Reset planner state */
ruckig_reset(planner);
/* Estimate a target position large enough that the trajectory will not
* reach it within time T. Use the trapezoidal formula as a conservative
* estimate: s = 0.5 * amax * T^2. Double it for safety. */
double target_pos = 0.5 * amax * T * T * 2.0;
/* Set a max velocity large enough to not be the limiting factor */
double max_vel = amax * T * 2.0; /* conservative estimate */
int result = ruckig_plan_position(planner,
0.0, /* start position */
0.0, /* start velocity */
0.0, /* start acceleration */
target_pos, /* target position (large enough) */
max_vel, /* target velocity (large, not limiting) */
0.0, /* target acceleration */
0.0, /* min velocity (unidirectional) */
max_vel * 2.0, /* max velocity (ensures no limiting) */
amax, /* max acceleration */
jerk); /* max jerk */
if (result != 0) {
/* Planning failed — use conservative fallback estimate.
* For an S-curve the velocity upper bound at time T is amax*T
* (trapezoidal), but the S-curve value is smaller. */
return fmin(amax * T, sqrt(amax * amax * T / jerk));
}
/* Sample velocity at time T */
double pos, vel, acc, jerk_val;
result = ruckig_at_time(planner, T, &pos, &vel, &acc, &jerk_val);
if (result != 0) {
/* Sampling failed — use conservative fallback */
return fmin(amax * T, sqrt(amax * amax * T / jerk));
}
return vel;
}
/* ================================================================
* Legacy functions kept for simple_tp.c compatibility
* ================================================================ */
/* PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
* VT = V0 + A0 * T + J * T^2 / 2
* AT = A0 + J * T
*/
double nextAccel(double t, double targetV, double v, double a, double maxA,
double maxJ) {
double max_da, tiny_da, vel_err, acc_req;
max_da = delta_accel(t, maxJ);
tiny_da = max_da * t * 0.001;
vel_err = targetV - v;
if (vel_err > tiny_da){
acc_req = -max_da +
sqrt(2.0 * maxJ * vel_err + max_da * max_da);
}else if (vel_err < -tiny_da){
acc_req = max_da -
sqrt(-2.0 * maxJ * vel_err + max_da * max_da);
}else{
/* within 'tiny_da' of desired velocity, no need to move */
acc_req = 0.0;
}
/* limit acceleration request */
if (acc_req > maxA){
acc_req = maxA;
}else if (acc_req < -maxA){
acc_req = -maxA;
}
/* ramp acceleration toward request at jerk limit */
if (acc_req > a + max_da){
return a + max_da;
}else if (acc_req < a - max_da){
return a - max_da;
}else{
return acc_req;
}
}
/* PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6
* VT = V0 + A0 * T + J * T^2 / 2
* AT = A0 + J * T
*/
double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j) {
/* Compute next acceleration */
double nextA = nextAccel(t, targetV, v, a, maxA, maxJ);
/* Compute next velocity using trapezoidal rule:
* VT - V0 = (A0 + AT) * T / 2 */
double deltaV = (a + nextA) * t / 2.0;
if ((deltaV < 0 && targetV < v && v + deltaV < targetV) ||
(0 < deltaV && v < targetV && targetV < v + deltaV)) {
/* Would overshoot target velocity — clamp */
nextA = 2.0 * (targetV - v) / t - a;
if(nextA >= maxA){
nextA = maxA;
targetV = (a + nextA) * t / 2.0;
}
v = targetV;
} else {
v += deltaV;
}
/* Compute jerk = delta accel / time */
*req_j = (nextA - a) / t;
if(*req_j > maxJ){
*req_j = maxJ;
nextA = a + maxJ * t;
} else if (*req_j < -maxJ) {
*req_j = -maxJ;
nextA = a - maxJ * t;
}
*req_a = nextA;
*req_v = v;
return v;
}
double stoppingDist(double v, double a, double maxA, double maxJ) {
/* Already stopped */
if (fabs(v) < 0.0001) return 0;
/* Handle negative velocity */
if (v < 0) {
v = -v;
a = -a;
}
double d = 0;
/* Compute distance and velocity change to bring acceleration to 0 */
if (0 < a) {
double t = a / maxJ;
d += sc_distance(t, v, a, -maxJ);
v += delta_velocity(t, a, -maxJ);
a = 0;
}
/* Compute maximum deceleration.
*
* At target velocity, both velocity and acceleration are 0.
* VT = 0 + 0*T1 + J*T1^2/2, and because Amax = J*T1:
* VT = Amax^2 / (2*J)
* From the other side: VT = v + (a + Amax)*T2/2
* Combining: Amax^2 = v*J + 0.5*a*a
*/
double maxDeccel = -sqrt(v * maxJ + 0.5 * a * a);
if (maxDeccel < -maxA) maxDeccel = -maxA;
/* Compute distance and velocity change to reach max deceleration */
if (maxDeccel < a) {
double t = (a - maxDeccel) / maxJ;
d += sc_distance(t, v, a, -maxJ);
v += delta_velocity(t, a, -maxJ);
a = maxDeccel;
}
/* Velocity remaining when entering final jerk phase:
* VT = Amax^2 / (2*J) */
double deltaV = 0.5 * a * a / maxJ;
/* Constant deceleration phase (if needed) */
if (deltaV < v) {
double t = (v - deltaV) / -a;
d += sc_distance(t, v, a, 0);
v += delta_velocity(t, a, 0);
}
/* Distance to zero velocity (final jerk phase) */
d += sc_distance(-a / maxJ, v, a, maxJ);
return d;
}
/* S-curve displacement: P = v*t + (1/2)*a*t^2 + (1/6)*j*t^3 */
double sc_distance(double t, double v, double a, double j) {
return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t));
}
/* Velocity change: dV = a*t + (1/2)*j*t^2 */
double delta_velocity(double t, double a, double j) {
return t * (a + 0.5 * j * t);
}
/* Acceleration change: dA = j*t */
double delta_accel(double t, double j) {return j * t;}

60
wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.h vendored Normal file
View File

@@ -0,0 +1,60 @@
/********************************************************************
* Description: sp_scurve.h
* Discriminate-based trajectory planning
*
* Derived from a work by Yang Yang
*
* Author: Yang Yang
* Contact: mika-net@outlook.com
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef SP_SCURVE_H
#define SP_SCURVE_H
#include <rtapi_math.h>
#include "../motion/simple_tp.h"
/**
* Initialize the S-curve planner (call at program entry).
*
* @param cycle_time cycle time in seconds
* @return 0 on success, -1 on failure
*/
int sp_scurve_init(double cycle_time);
/**
* Clean up the S-curve planner (call at program exit).
*/
void sp_scurve_cleanup(void);
/* Legacy functions kept for simple_tp.c compatibility */
double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ);
double sc_distance(double t, double v, double a, double j);
double delta_velocity(double t, double a, double j);
double delta_accel(double t, double j);
double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j);
double stoppingDist(double v, double a, double maxA, double maxJ);
int findSCurveVSpeed(double distence,/* double maxV, */double maxA, double maxJ, double *req_v);
int findSCurveVSpeedWithEndSpeed(double distence, double Ve, double maxA, double maxJ, double* req_v);
int findSCurveMaxStartSpeed(double distance, double Ve, double maxA, double maxJ, double* req_v);
double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2);
double calcSCurveSpeedWithT(double amax, double jerk, double T);
/**
* tpCalculateSCurveAccel return value definitions
*
* TP_SCURVE_ACCEL_ERROR - calculation failed (maxjerk invalid or less than/equal to 1)
* TP_SCURVE_ACCEL_ACCEL - acceleration or normal state (no deceleration needed)
* TP_SCURVE_ACCEL_DECEL - deceleration needed
*/
#define TP_SCURVE_ACCEL_ERROR -5
#define TP_SCURVE_ACCEL_ACCEL 0
#define TP_SCURVE_ACCEL_DECEL 1
#endif

202
wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.c vendored Normal file
View File

@@ -0,0 +1,202 @@
/********************************************************************
* Description: spherical_arc.c
*
* A simple spherical linear interpolation library and related functions.
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2014 All rights reserved.
*
********************************************************************/
#include <rtapi_math.h>
#include "spherical_arc.h"
#include "tp_types.h"
#include "tp_debug.h"
int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
PmCartesian const * const end,
PmCartesian const * const center)
{
#ifdef ARC_PEDANTIC
if (!P0 || !P1 || !center)
return TP_ERR_MISSING_INPUT;
if (!arc)
return TP_ERR_MISSING_OUTPUT;
#endif
// Store the start, end, and center
arc->start = *start;
arc->end = *end;
arc->center = *center;
pmCartCartSub(start, center, &arc->rStart);
pmCartCartSub(end, center, &arc->rEnd);
// Find the radii at start and end. These are identical for a perfect spherical arc
double radius0, radius1;
pmCartMag(&arc->rStart, &radius0);
pmCartMag(&arc->rEnd, &radius1);
tp_debug_print("radii are %g and %g\n",
radius0,
radius1);
if (radius0 < ARC_MIN_RADIUS || radius1 < ARC_MIN_RADIUS) {
tp_debug_print("radius below min radius %f, aborting arc\n",
ARC_MIN_RADIUS);
return TP_ERR_RADIUS_TOO_SMALL;
}
// Choose initial radius as nominal radius
arc->radius = radius0;
// Get unit vectors from center to start and center to end
PmCartesian u0, u1;
pmCartScalMult(&arc->rStart, 1.0 / radius0, &u0);
pmCartScalMult(&arc->rEnd, 1.0 / radius1, &u1);
// Find arc angle
double dot;
pmCartCartDot(&u0, &u1, &dot);
arc->angle = acos(dot);
tp_debug_print("spherical arc angle = %f\n", arc->angle);
// Store spiral factor as radial difference. Archimedean spiral coef. a = spiral / angle
arc->spiral = (radius1 - radius0 );
if (arc->angle < ARC_MIN_ANGLE) {
tp_debug_print("angle %f below min angle %f, aborting arc\n",
arc->angle,
ARC_MIN_ANGLE);
return TP_ERR_GEOM;
}
// Store sin of arc angle since it is reused many times for SLERP
arc->Sangle = sin(arc->angle);
return TP_ERR_OK;
}
int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out)
{
//TODO pedantic
//Convert progress to actual progress around the arc
double net_progress = progress - arc->line_length;
if (net_progress <= 0.0 && arc->line_length > 0) {
tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length);
//Get position on line (not actually an angle in this case)
pmCartScalMult(&arc->uTan, net_progress, out);
pmCartCartAdd(out, &arc->start, out);
} else {
double angle_in = net_progress / arc->radius;
tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle);
double scale0 = sin(arc->angle - angle_in) / arc->Sangle;
double scale1 = sin(angle_in) / arc->Sangle;
PmCartesian interp0,interp1;
pmCartScalMult(&arc->rStart, scale0, &interp0);
pmCartScalMult(&arc->rEnd, scale1, &interp1);
pmCartCartAdd(&interp0, &interp1, out);
pmCartCartAdd(&arc->center, out, out);
}
return TP_ERR_OK;
}
int arcLength(SphericalArc const * const arc, double * const length)
{
*length = arc->radius * arc->angle + arc->line_length;
tp_debug_print("arc length = %g\n", *length);
return TP_ERR_OK;
}
int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1,
PmCartLine const * const line2, double radius,
double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume) {
(void)radius;
PmCartesian center, normal, binormal;
// Pointer to middle point of line segment pair
PmCartesian const * const middle = &line1->end;
//TODO assert line1 end = line2 start?
//Calculate the normal direction of the arc from the difference
//between the unit vectors
pmCartCartSub(&line2->uVec, &line1->uVec, &normal);
pmCartUnitEq(&normal);
pmCartScalMultEq(&normal, center_dist);
pmCartCartAdd(middle, &normal, &center);
//Calculate the binormal (vector perpendicular to the plane of the
//arc)
pmCartCartCross(&line1->uVec, &line2->uVec, &binormal);
pmCartUnitEq(&binormal);
// Start point is blend_dist away from middle point in the
// negative direction of line1
pmCartScalMult(&line1->uVec, -blend_dist, start);
pmCartCartAdd(start, middle, start);
// End point is blend_dist away from middle point in the positive
// direction of line2
pmCartScalMult(&line2->uVec, blend_dist, end);
pmCartCartAddEq(end, middle);
//Handle line portion of line-arc
arc->uTan = line1->uVec;
if (consume) {
arc->line_length = line1->tmag - blend_dist;
} else {
arc->line_length = 0;
}
return arcInitFromPoints(arc, start, end, &center);
}
int arcConvexTest(PmCartesian const * const center,
PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir)
{
//Check if an arc-line intersection is concave or convex
double dot;
PmCartesian diff;
pmCartCartSub(P, center, &diff);
pmCartCartDot(&diff, uVec, &dot);
tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir);
int convex = (reverse_dir != 0) ^ (dot < 0);
return convex;
}
int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end)
{
PmCartesian r_perp;
PmCartesian r_tan;
if (at_end) {
r_perp = arc->rEnd;
} else {
r_perp = arc->rStart;
}
pmCartCartCross(&arc->binormal, &r_perp, &r_tan);
//Get spiral component
double dr = arc->spiral / arc->angle;
//Get perpendicular component due to spiral
PmCartesian d_perp;
pmCartUnit(&r_perp, &d_perp);
pmCartScalMultEq(&d_perp, dr);
//TODO error checks
pmCartCartAdd(&d_perp, &r_tan, tan);
pmCartUnitEq(tan);
return TP_ERR_OK;
}

67
wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.h vendored Normal file
View File

@@ -0,0 +1,67 @@
/********************************************************************
* Description: spherical_arc.h
*
* A simple spherical linear interpolation library and related functions.
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2014 All rights reserved.
*
********************************************************************/
#ifndef SPHERICAL_ARC_H
#define SPHERICAL_ARC_H
#include <posemath.h>
#define ARC_POS_EPSILON 1e-12
#define ARC_MIN_RADIUS 1e-12
#define ARC_MIN_ANGLE 1e-6
//FIXME relate this to cornering acceleration?
#define ARC_ABS_ERR 5e-4
#define ARC_REL_ERR 5e-4
typedef struct {
// Three defining points for the arc
PmCartesian start;
PmCartesian end;
PmCartesian center;
// Relative vectors from center to start and center to end
// These are cached here since they'll be reused during SLERP
PmCartesian rStart;
PmCartesian rEnd;
PmCartesian uTan; /* Tangent vector at start of arc (copied from
prev. tangent line)*/
PmCartesian binormal;
double radius;
double spiral;
// Angle that the arc encloses
double angle;
double Sangle;
double line_length;
} SphericalArc;
int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
PmCartesian const * const end, PmCartesian const * const center);
int arcInitFromVectors(SphericalArc * const arc, PmCartesian const * const vec0,
PmCartesian const * const vec1,
PmCartesian const * const center);
int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * const out);
int arcNormalizedSlerp(SphericalArc const * const arc, double t, PmCartesian * const out);
int arcLength(SphericalArc const * const arc, double * const length);
int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1,
PmCartLine const * const line2, double radius,
double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume);
int arcConvexTest(PmCartesian const * const center,
PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir);
int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end);
#endif

1109
wasm-port/vendor/linuxcnc/src/emc/tp/tc.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

128
wasm-port/vendor/linuxcnc/src/emc/tp/tc.h vendored Normal file
View File

@@ -0,0 +1,128 @@
/********************************************************************
* Description: tc.h
* Discriminate-based trajectory planning
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef TC_H
#define TC_H
#include <posemath.h>
#include <emcpos.h>
#include <emcmotcfg.h>
#include "spherical_arc.h"
#include "tc_types.h"
#include "tp_types.h"
double tcGetMaxTargetVel(TC_STRUCT const * const tc,
double max_scale);
double tcGetOverallMaxAccel(TC_STRUCT const * tc);
double tcGetTangentialMaxAccel(TC_STRUCT const * const tc);
int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction);
int tcInitKinkProperties(TC_STRUCT *tc);
int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc);
int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out);
int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out);
int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out);
int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out);
int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out);
double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction);
double tcGetTarget(TC_STRUCT const * const tc, int direction);
int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc, PmCartesian * const point);
int tcCanConsume(TC_STRUCT const * const tc);
int tcSetTermCond(TC_STRUCT * prev_tc, TC_STRUCT * tc, int term_cond);
int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc,
PmCartesian const * const circ_start,
PmCartesian const * const circ_end);
int tcIsBlending(TC_STRUCT * const tc);
int tcFindBlendTolerance(TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance);
int pmCircleTangentVector(PmCircle const * const circle,
double angle_in, PmCartesian * const out);
int tcFlagEarlyStop(TC_STRUCT * const tc,
TC_STRUCT * const nexttc);
double pmLine9Target(PmLine9 * const line9);
int pmLine9Init(PmLine9 * const line9,
EmcPose const * const start,
EmcPose const * const end);
double pmCircle9Target(PmCircle9 const * const circ9);
int pmCircle9Init(PmCircle9 * const circ9,
EmcPose const * const start,
EmcPose const * const end,
PmCartesian const * const center,
PmCartesian const * const normal,
int turn);
int pmRigidTapInit(PmRigidTap * const tap,
EmcPose const * const start,
EmcPose const * const end,
double reversal_scale);
double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev);
int tcInit(TC_STRUCT * const tc,
int motion_type,
int canon_motion_type,
double cycle_time,
unsigned char enables,
char atspeed);
int tcSetupFromTP(TC_STRUCT * const tc, TP_STRUCT const * const tp);
int tcSetupMotion(TC_STRUCT * const tc,
double vel,
double ini_maxvel,
double acc,
double ini_maxjerk);
int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp);
int tcUpdateArcLimits(TC_STRUCT * tc);
int tcFinalizeLength(TC_STRUCT * const tc);
int tcClampVelocityByLength(TC_STRUCT * const tc);
int tcPureRotaryCheck(TC_STRUCT const * const tc);
int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ);
int tcClearFlags(TC_STRUCT * const tc);
/**
* Clean up Ruckig planner resources in a TC_STRUCT.
* Called when the trajectory segment is removed or reset.
*/
void tcCleanupRuckig(TC_STRUCT * const tc);
#endif /* TC_H */

215
wasm-port/vendor/linuxcnc/src/emc/tp/tc_types.h vendored Normal file
View File

@@ -0,0 +1,215 @@
/********************************************************************
* Description: tc.h
* Discriminate-based trajectory planning
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#ifndef TC_TYPES_H
#define TC_TYPES_H
#include <posemath.h>
#include <emcpos.h>
#include <emcmotcfg.h>
#include "spherical_arc.h"
#include "../motion/state_tag.h"
#define BLEND_DIST_FRACTION 0.5
/* values for endFlag */
typedef enum {
TC_TERM_COND_STOP = 0,
TC_TERM_COND_EXACT = 1,
TC_TERM_COND_PARABOLIC = 2,
TC_TERM_COND_TANGENT = 3
} tc_term_cond_t;
typedef enum {
TC_LINEAR = 1,
TC_CIRCULAR = 2,
TC_RIGIDTAP = 3,
TC_SPHERICAL = 4
} tc_motion_type_t;
typedef enum {
TC_SYNC_NONE = 0,
TC_SYNC_VELOCITY,
TC_SYNC_POSITION
} tc_spindle_sync_t;
typedef enum {
TC_DIR_FORWARD = 0,
TC_DIR_REVERSE
} tc_direction_t;
#define TC_GET_PROGRESS 0
#define TC_GET_STARTPOINT 1
#define TC_GET_ENDPOINT 2
#define TC_OPTIM_UNTOUCHED 0
#define TC_OPTIM_AT_MAX 1
#define TC_ACCEL_TRAPZ 0
#define TC_ACCEL_RAMP 1
/**
* Spiral arc length approximation by quadratic fit.
*/
typedef struct {
double b0; /* 2nd order coefficient */
double b1; /* 1st order coefficient */
double total_planar_length; /* total arc length in plane */
int spiral_in; /* flag indicating spiral is inward,
rather than outward */
} SpiralArcLengthFit;
/* structure for individual trajectory elements */
typedef struct {
PmCartLine xyz;
PmCartLine abc;
PmCartLine uvw;
} PmLine9;
typedef struct {
PmCircle xyz;
PmCartLine abc;
PmCartLine uvw;
SpiralArcLengthFit fit;
} PmCircle9;
typedef struct {
SphericalArc xyz;
PmCartesian abc;
PmCartesian uvw;
} Arc9;
typedef enum {
RIGIDTAP_START,
TAPPING, REVERSING, RETRACTION, FINAL_REVERSAL, FINAL_PLACEMENT
} RIGIDTAP_STATE;
typedef unsigned long long iomask_t; // 64 bits on both x86 and x86_64
typedef struct {
char anychanged;
iomask_t dio_mask;
iomask_t aio_mask;
signed char dios[EMCMOT_MAX_DIO];
double aios[EMCMOT_MAX_AIO];
} syncdio_t;
typedef struct {
PmCartLine xyz; // original, but elongated, move down
PmCartLine aux_xyz; // this will be generated on the fly, for the other
// two moves: retraction, final placement
PmCartesian abc;
PmCartesian uvw;
double reversal_target;
double reversal_scale;
double spindlerevs_at_reversal;
RIGIDTAP_STATE state;
} PmRigidTap;
typedef struct {
double cycle_time;
//Position stuff
double target; // actual segment length
double progress; // where are we in the segment? 0..target
double nominal_length;
//Velocity
double reqvel; // vel requested by F word, calc'd by task
double target_vel; // velocity to actually track, limited by other factors
double maxvel; // max possible vel (feed override stops here)
double currentvel; // keep track of current step (vel * cycle_time)
double last_move_length;// last move length
double finalvel; // velocity to aim for at end of segment
double term_vel; // actual velocity at termination of segment
double kink_vel; // Temporary way to store our calculation of maximum velocity we can handle if this segment is declared tangent with the next
double kink_accel_reduce_prev; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection.
double kink_accel_reduce; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection.
double factor;
double targetvel;
double vt;
//Jerk
double maxjerk; // max jerk for S-curve motion
double blend_maxjerk; // max jerk during blend (set by look-ahead)
double currentjerk; // current jerk for S-curve planning
double currentacc; // current acceleration for S-curve planning
double lastacc;
//Acceleration
double maxaccel; // accel calc'd by task
double acc_ratio_tan;// ratio between normal and tangential accel
int id; // segment's serial number
struct state_tag_t tag; // state tag corresponding to running motion
union { // describes the segment's start and end positions
PmLine9 line;
PmCircle9 circle;
PmRigidTap rigidtap;
Arc9 arc;
} coords;
int motion_type; // TC_LINEAR (coords.line) or
// TC_CIRCULAR (coords.circle) or
// TC_RIGIDTAP (coords.rigidtap)
int active; // this motion is being executed
int canon_motion_type; // this motion is due to which canon function?
int term_cond; // gcode requests continuous feed at the end of
// this segment (g64 mode)
int blending_next; // segment is being blended into following segment
double blend_vel; // velocity below which we should start blending
double tolerance; // during the blend at the end of this move,
// stay within this distance from the path.
int synchronized; // spindle sync state
double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi)
double vel_at_blend_start;
int sync_accel; // we're accelerating up to sync with the spindle
unsigned char enables; // Feed scale, etc, enable bits for this move
int atspeed; // wait for the spindle to be at-speed before starting this move
syncdio_t syncdio; // synched DIO's for this move. what to turn on/off
int indexer_jnum; // which joint to unlock (for a locking indexer) to make this move, -1 for none
int optimization_state; // At peak velocity during blends)
int on_final_decel;
int blend_prev;
int accel_mode;
int splitting; // the segment is less than 1 cycle time
// away from the end.
int remove; // Flag to remove the segment from the queue
int active_depth; /* Active depth (i.e. how many segments
* after this will it take to slow to zero
* speed) */
int finalized;
// Temporary status flags (reset each cycle)
int is_blending;
// Ruckig trajectory planner support
void *ruckig_planner; // Ruckig planner handle (opaque pointer)
double ruckig_trajectory_time; // current trajectory time (seconds from trajectory start)
int ruckig_planned; // whether Ruckig planning completed (1=planned, 0=not)
// Store last planning parameters for detecting parameter changes
double ruckig_last_maxaccel; // max acceleration used in last planning
double ruckig_last_maxjerk; // max jerk used in last planning
double ruckig_last_target_vel; // target velocity used in last planning
double ruckig_last_final_vel; // final velocity used in last planning
double ruckig_last_target_pos; // target position used in last planning
int ruckig_last_use_velocity_control; // control mode used in last planning (1=velocity, 0=position)
double ruckig_last_req_pos; // last req_pos value from Ruckig (for velocity control incremental calc)
double ruckig_last_feed_override; // feed override value at last planning (for debug and change detection)
} TC_STRUCT;
#endif /* TC_TYPES_H */

355
wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c vendored Normal file
View File

@@ -0,0 +1,355 @@
/*!
********************************************************************
* Description: tcq.c
*\brief queue handling functions for trajectory planner
* These following functions implement the motion queue that
* is fed by tpAddLine/tpAddCircle and consumed by tpRunCycle.
* They have been fully working for a long time and a wise programmer
* won't mess with them.
*
*\author Derived from a work by Fred Proctor & Will Shackleford
*\author rewritten by Chris Radek
*
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
********************************************************************/
#include <stddef.h>
#include "tcq.h"
/** Return 0 if queue is valid, -1 if not */
static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq)
{
if ((0 == tcq) || (0 == tcq->queue))
{
return -1;
}
return 0;
}
/*! tcqCreate() function
*
* \brief Creates a new queue for TC elements.
*
* This function creates a new queue for TC elements.
* It gets called by tpCreate()
*
* @param tcq pointer to the new TC_QUEUE_STRUCT
* @param _size size of the new queue
* @param tcSpace holds the space allocated for the new queue, allocated in motion.c
*
* @return int returns success or failure
*/
int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace)
{
if (!tcq || !tcSpace || _size < 1) {
return -1;
}
tcq->queue = tcSpace;
tcq->size = _size;
tcqInit(tcq);
return 0;
}
/*! tcqDelete() function
*
* \brief Deletes a queue holding TC elements.
*
* This function creates deletes a queue. It doesn't free the space
* only throws the pointer away.
* It gets called by tpDelete()
* \todo FIXME, it seems tpDelete() is gone, and this function isn't used.
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return int returns success
*/
int tcqDelete(TC_QUEUE_STRUCT * const tcq)
{
if (!tcqCheck(tcq)) {
/* free(tcq->queue); */
tcq->queue = 0;
}
return 0;
}
/*! tcqInit() function
*
* \brief Initializes a queue with TC elements.
*
* This function initializes a queue with TC elements.
* It gets called by tpClear() and
* by tpRunCycle() when we are aborting
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return int returns success or failure (if no tcq found)
*/
int tcqInit(TC_QUEUE_STRUCT * const tcq)
{
if (tcqCheck(tcq)) return -1;
tcq->_len = 0;
tcq->start = tcq->end = 0;
tcq->rend = 0;
tcq->_rlen = 0;
tcq->allFull = 0;
return 0;
}
/*! tcqPut() function
*
* \brief puts a TC element at the end of the queue
*
* This function adds a tc element at the end of the queue.
* It gets called by tpAddLine() and tpAddCircle()
*
* @param tcq pointer to the new TC_QUEUE_STRUCT
* @param tc the new TC element to be added
*
* @return int returns success or failure
*/
int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc)
{
/* check for initialized */
if (tcqCheck(tcq)) return -1;
/* check for allFull, so we don't overflow the queue */
if (tcq->allFull) {
return -1;
}
/* add it */
tcq->queue[tcq->end] = *tc;
tcq->_len++;
/* update end ptr, modulo size of queue */
tcq->end = (tcq->end + 1) % tcq->size;
/* set allFull flag if we're really full */
if (tcq->end == tcq->start) {
tcq->allFull = 1;
}
return 0;
}
/*! tcqPopBack() function
*
* \brief removes the newest TC element (converse of tcqRemove)
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return int returns success or failure
*/
int tcqPopBack(TC_QUEUE_STRUCT * const tcq)
{
/* check for initialized */
if (tcqCheck(tcq)) return -1;
/* Too short to pop! */
if (tcq->_len < 1) {
return -1;
}
int n = tcq->end - 1 + tcq->size;
tcq->end = n % tcq->size;
tcq->_len--;
return 0;
}
#define TCQ_REVERSE_MARGIN 200
int tcqPop(TC_QUEUE_STRUCT * const tcq)
{
if (tcqCheck(tcq)) {
return -1;
}
if (tcq->_len < 1 && !tcq->allFull) {
return -1;
}
/* update start ptr and reset allFull flag and len */
tcq->start = (tcq->start + 1) % tcq->size;
tcq->allFull = 0;
tcq->_len--;
if (tcq->_rlen < TCQ_REVERSE_MARGIN) {
//If we're not overwriting the history yet, then we have another segment added to the reverse history
tcq->_rlen++;
} else {
//If we're run out of spare reverse history, then advance rend
tcq->rend = (tcq->rend + 1) % tcq->size;
}
return 0;
}
/*! tcqRemove() function
*
* \brief removes n items from the queue
*
* This function removes the first n items from the queue,
* after checking that they can be removed
* (queue initialized, queue not empty, enough elements in it)
* Function gets called by tpRunCycle() with n=1
* \todo FIXME: Optimize the code to remove only 1 element, might speed it up
*
* @param tcq pointer to the new TC_QUEUE_STRUCT
* @param n the number of TC elements to be removed
*
* @return int returns success or failure
*/
int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n)
{
if (n <= 0) {
return 0; /* okay to remove 0 or fewer */
}
if (tcqCheck(tcq) || ((tcq->start == tcq->end) && !tcq->allFull) ||
(n > tcq->_len)) { /* too many requested */
return -1;
}
/* update start ptr and reset allFull flag and len */
tcq->start = (tcq->start + n) % tcq->size;
tcq->allFull = 0;
tcq->_len -= n;
return 0;
}
/**
* Step backward into the reverse history.
*/
int tcqBackStep(TC_QUEUE_STRUCT * const tcq)
{
if (tcqCheck(tcq)) {
return -1;
}
// start == end means that queue is empty
if ( tcq->start == tcq->rend) {
return -1;
}
/* update start ptr and reset allFull flag and len */
tcq->start = (tcq->start - 1 + tcq->size) % tcq->size;
tcq->_len++;
tcq->_rlen--;
return 0;
}
/*! tcqLen() function
*
* \brief returns the number of elements in the queue
*
* Function gets called by tpSetVScale(), tpAddLine(), tpAddCircle()
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return int returns number of elements
*/
int tcqLen(TC_QUEUE_STRUCT const * const tcq)
{
if (tcqCheck(tcq)) return -1;
return tcq->_len;
}
/*! tcqItem() function
*
* \brief gets the n-th TC element in the queue, without removing it
*
* Function gets called by tpSetVScale(), tpRunCycle(), tpIsPaused()
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return TC_STRUCT returns the TC elements
*/
TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n)
{
if (tcqCheck(tcq) || (n < 0) || (n >= tcq->_len)) return NULL;
return &(tcq->queue[(tcq->start + n) % tcq->size]);
}
/*!
* \def TC_QUEUE_MARGIN
* sets up a margin at the end of the queue, to reduce effects of race conditions
*/
#define TC_QUEUE_MARGIN (TCQ_REVERSE_MARGIN+20)
/*! tcqFull() function
*
* \brief get the full status of the queue
* Function returns full if the count is closer to the end of the queue than TC_QUEUE_MARGIN
*
* Function called by update_status() in control.c
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return int returns status (0==not full, 1==full)
*/
int tcqFull(TC_QUEUE_STRUCT const * const tcq)
{
if (tcqCheck(tcq)) {
return 1; /* null queue is full, for safety */
}
/* call the queue full if the length is into the margin, so reduce the
effect of a race condition where the appending process may not see the
full status immediately and send another motion */
if (tcq->size <= TC_QUEUE_MARGIN) {
/* no margin available, so full means really all full */
return tcq->allFull;
}
if (tcq->_len >= tcq->size - TC_QUEUE_MARGIN) {
/* we're into the margin, so call it full */
return 1;
}
/* we're not into the margin */
return 0;
}
/*! tcqLast() function
*
* \brief gets the last TC element in the queue, without removing it
*
*
* @param tcq pointer to the TC_QUEUE_STRUCT
*
* @return TC_STRUCT returns the TC element
*/
TC_STRUCT *tcqLast(TC_QUEUE_STRUCT const * const tcq)
{
if (tcqCheck(tcq)) {
return NULL;
}
if (tcq->_len == 0) {
return NULL;
}
//Fix for negative modulus error
int n = tcq->end-1 + tcq->size;
return &(tcq->queue[n % tcq->size]);
}

75
wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h vendored Normal file
View File

@@ -0,0 +1,75 @@
/********************************************************************
* Description: tcq.c
*\brief queue handling functions for trajectory planner
* These following functions implement the motion queue that
* is fed by tpAddLine/tpAddCircle and consumed by tpRunCycle.
* They have been fully working for a long time and a wise programmer
* won't mess with them.
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
/* queue of TC_STRUCT elements*/
#ifndef TCQ_H
#define TCQ_H
#include "tc_types.h"
typedef struct {
TC_STRUCT *queue; /* ptr to the tcs */
int size; /* size of queue */
int _len; /* number of tcs now in queue */
int _rlen; /* number of tcs now in reverse history */
int start, end; /* indices to next to get, next to put */
int rend;
int allFull; /* flag meaning it's actually full */
} TC_QUEUE_STRUCT;
/* TC_QUEUE_STRUCT functions */
/* create queue of _size */
extern int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size,
TC_STRUCT * const tcSpace);
/* free up queue */
extern int tcqDelete(TC_QUEUE_STRUCT * const tcq);
/* reset queue to empty */
extern int tcqInit(TC_QUEUE_STRUCT * const tcq);
/* put tc on end */
extern int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc);
/* remove a single tc from the back of the queue */
extern int tcqPopBack(TC_QUEUE_STRUCT * const tcq);
extern int tcqPop(TC_QUEUE_STRUCT * const tcq);
/* remove n tcs from front */
extern int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n);
extern int tcqBackStep(TC_QUEUE_STRUCT * const tcq);
/* how many tcs on queue */
extern int tcqLen(TC_QUEUE_STRUCT const * const tcq);
/* look at nth item, first is 0 */
extern TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n);
/**
* Get the "end" of the queue, the most recently added item.
*/
extern TC_STRUCT * tcqLast(TC_QUEUE_STRUCT const * const tcq);
/* get full status */
extern int tcqFull(TC_QUEUE_STRUCT const * const tcq);
#endif

4388
wasm-port/vendor/linuxcnc/src/emc/tp/tp.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

99
wasm-port/vendor/linuxcnc/src/emc/tp/tp.h vendored Normal file
View File

@@ -0,0 +1,99 @@
/********************************************************************
* Description: tp.h
* Trajectory planner based on TC elements
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
********************************************************************/
#ifndef TP_H
#define TP_H
#include <posemath.h>
#include "tc_types.h"
#include "tp_types.h"
#include "tcq.h"
// functions not used by motmod:
int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp);
int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos);
void tpToggleDIOs(TC_STRUCT * const tc); //gets called when a new tc is
//taken from the queue. it checks
//and toggles all needed DIO's
int tpIsMoving(TP_STRUCT const * const tp);
int tpInit(TP_STRUCT * const tp);
// functions used by motmod:
int tpCreate(TP_STRUCT * const tp, int _queueSize,int id);
int tpClear(TP_STRUCT * const tp);
int tpClearDIOs(TP_STRUCT * const tp);
int tpSetCycleTime(TP_STRUCT * tp, double secs);
int tpSetVmax(TP_STRUCT * tp, double vmax, double ini_maxvel);
int tpSetVlimit(TP_STRUCT * tp, double limit);
int tpSetAmax(TP_STRUCT * tp, double amax);
int tpSetId(TP_STRUCT * tp, int id);
int tpGetExecId(TP_STRUCT * tp);
struct state_tag_t tpGetExecTag(TP_STRUCT * const tp);
int tpSetTermCond(TP_STRUCT * tp, int cond, double tolerance);
int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos);
int tpRunCycle(TP_STRUCT * tp, long period);
int tpPause(TP_STRUCT * tp);
int tpResume(TP_STRUCT * tp);
int tpAbort(TP_STRUCT * tp);
int tpAddRigidTap(TP_STRUCT * const tp,
EmcPose end,
double vel,
double ini_maxvel,
double acc,
double ini_maxjerk,
unsigned char enables,
double scale,
struct state_tag_t tag);
int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type,
double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables,
char atspeed, int indexrotary, struct state_tag_t tag);
int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center,
PmCartesian normal, int turn, int canon_motion_type, double vel,
double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables,
char atspeed, struct state_tag_t tag);
int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos);
int tpIsDone(TP_STRUCT * const tp);
int tpQueueDepth(TP_STRUCT * const tp);
int tpActiveDepth(TP_STRUCT * const tp);
int tpGetMotionType(TP_STRUCT * const tp);
int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int wait);
int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end);
int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end); //gets called to place DIO toggles on the TC queue
int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir);
//---------------------------------------------------------------------
// Module interface
void tpMotFunctions(void(*pDioWrite)(int,char)
,void(*pAioWrite)(int,double)
,void(*pSetRotaryUnlock)(int,int)
,int( *pGetRotaryUnlock)(int)
,double(*paxis_get_vel_limit)(int)
,double(*paxis_get_acc_limit)(int)
);
// These are here so we don't need to include "motion/motion.h"
// because that feels very wrong. The real solution is to untangle
// motion controller and trajectory planner sources. Only the shared
// data should be exposed to each other.
typedef struct emcmot_status_t emcmot_status_t;
typedef struct emcmot_config_t emcmot_config_t;
void tpMotData(emcmot_status_t *
,emcmot_config_t *
);
//---------------------------------------------------------------------
#endif /* TP_H */

65
wasm-port/vendor/linuxcnc/src/emc/tp/tp_debug.h vendored Normal file
View File

@@ -0,0 +1,65 @@
/********************************************************************
* Description: tc_debug.h
*
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2013 All rights reserved.
*
* Last change:
********************************************************************/
#ifndef TP_DEBUG_H
#define TP_DEBUG_H
#include <rtapi.h> /* printing functions */
/** TP debug stuff */
#ifdef TP_DEBUG
//Kludge because I didn't know any better at the time
//FIXME replace these with better names?
#define tp_debug_print(...) rtapi_print(__VA_ARGS__)
#elif defined(UNIT_TEST)
#include <stdio.h>
#define tp_debug_print(...) printf(__VA_ARGS__)
#else
#define tp_debug_print(...)
#endif
// Verbose but effective wrappers for building faux-JSON debug output for a function
#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_)
#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_)
#define tp_debug_json_end() tp_debug_print("}\n")
/** Use for profiling to make static function names visible */
#ifdef TP_PROFILE
#define STATIC
#else
#define STATIC static
#endif
/** "TC" debug info for inspecting trajectory planner output at each timestep */
#ifdef TC_DEBUG
#define tc_debug_print(...) rtapi_print(__VA_ARGS__)
#else
#define tc_debug_print(...)
#endif
/** TP position data output to debug acceleration spikes */
#ifdef TP_POSEMATH_DEBUG
#define tp_posemath_debug(...) rtapi_print(__VA_ARGS__)
#else
#define tp_posemath_debug(...)
#endif
/** TP misc data logging */
#ifdef TP_INFO_LOGGING
#define tp_info_print(...) rtapi_print(__VA_ARGS__)
#else
#define tp_info_print(...)
#endif
int gdb_fake_catch(int condition);
int gdb_fake_assert(int condition);
#endif

154
wasm-port/vendor/linuxcnc/src/emc/tp/tp_types.h vendored Normal file
View File

@@ -0,0 +1,154 @@
/********************************************************************
* Description: tp_types.h
* Trajectory planner types and constants
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
********************************************************************/
#ifndef TP_TYPES_H
#define TP_TYPES_H
#include <rtapi_bool.h>
#include <posemath.h>
#include "tc_types.h"
#include "tcq.h"
#define TP_DEFAULT_QUEUE_SIZE 32
/* Minimum length of a segment in cycles (must be greater than 1 to ensure each
* segment is hit at least once.) */
#define TP_MIN_SEGMENT_CYCLES 1.02
/* Values chosen for accel ratio to match parabolic blend acceleration
* limits. */
#define TP_OPTIMIZATION_CUTOFF 4
/* If the queue is shorter than the threshold, assume that we're approaching
* the end of the program */
#define TP_QUEUE_THRESHOLD 3
/* closeness to zero, for determining if a move is pure rotation */
#define TP_PURE_ROTATION_EPSILON 1e-6
/* "neighborhood" size (if two values differ by less than the epsilon,
* then they are effectively equal.)*/
#define TP_ACCEL_EPSILON 1e-4
#define TP_VEL_EPSILON 1e-8
#define TP_POS_EPSILON 1e-12
#define TP_TIME_EPSILON 1e-12
#define TP_ANGLE_EPSILON 1e-6
#define TP_ANGLE_EPSILON_SQ (TP_ANGLE_EPSILON * TP_ANGLE_EPSILON)
#define TP_MIN_ARC_ANGLE 1e-3
#define TP_MIN_ARC_LENGTH 1e-6
#define TP_BIG_NUM 1e10
/**
* TP return codes.
* This enum is a catch-all for useful return statuses from TP
* internal functions. This may be replaced with a better system in
* the future.
*/
typedef enum {
TP_ERR_INVALID = -9,
TP_ERR_INPUT_TYPE = -8,
TP_ERR_TOLERANCE = -7,
TP_ERR_RADIUS_TOO_SMALL = -6,
TP_ERR_GEOM = -5,
TP_ERR_RANGE = -4,
TP_ERR_MISSING_OUTPUT = -3,
TP_ERR_MISSING_INPUT = -2,
TP_ERR_FAIL = -1,
TP_ERR_OK = 0,
TP_ERR_NO_ACTION,
TP_ERR_SLOWING,
TP_ERR_STOPPED,
TP_ERR_WAITING,
TP_ERR_ZERO_LENGTH,
TP_ERR_REVERSE_EMPTY,
TP_ERR_LAST
} tp_err_t;
/**
* Persistent data for spindle status within tpRunCycle.
* This structure encapsulates some static variables to simplify refactoring of
* synchronized motion code.
*/
typedef struct {
int spindle_num;
double offset;
double revs;
int waiting_for_index;
int waiting_for_atspeed;
} tp_spindle_t;
/**
* Trajectory planner state structure.
* Stores persistent data for the trajectory planner that should be accessible
* by outside functions.
*/
typedef struct {
TC_QUEUE_STRUCT queue;
tp_spindle_t spindle; //Spindle data
EmcPose currentPos;
EmcPose goalPos;
int queueSize;
double cycleTime;
double vMax; /* vel for subsequent moves */
double ini_maxvel; /* max velocity allowed by machine
constraints (INI file) for
subsequent moves */
double vLimit; /* absolute upper limit on all vels */
double aMax; /* max accel (unused) */
double ini_maxjerk;
//FIXME this shouldn't be a separate limit,
double aMaxCartesian; /* max cartesian acceleration by machine bounds */
double aLimit; /* max accel (unused) */
double wMax; /* rotational velocity max */
double wDotMax; /* rotational acceleration max */
int nextId;
int execId;
struct state_tag_t execTag; /* state tag corresponding to running motion */
int termCond;
int done;
int depth; /* number of total queued motions */
int activeDepth; /* number of motions blending */
int aborting;
int pausing;
int reverse_run; /* Indicates that TP is running in reverse */
int motionType;
double tolerance; /* for subsequent motions, stay within this
distance of the programmed path during
blends */
int synchronized; // spindle sync required for this move
int velocity_mode; /* TRUE if spindle sync is in velocity mode,
FALSE if in position mode */
double uu_per_rev; /* user units per spindle revolution */
syncdio_t syncdio; //record tpSetDout's here
} TP_STRUCT;
/**
* Describes blend modes used in the trajectory planner.
* @note these values are used as array indices, so make sure valid options
* start at 0 and increase by one.
*/
typedef enum {
NO_BLEND = -1,
PARABOLIC_BLEND,
TANGENT_SEGMENTS_BLEND,
ARC_BLEND
} tc_blend_type_t;
#endif /* TP_TYPES_H */

2005
wasm-port/vendor/linuxcnc/src/libnml/posemath/_posemath.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

3743
wasm-port/vendor/linuxcnc/src/libnml/posemath/gomath.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1411
wasm-port/vendor/linuxcnc/src/libnml/posemath/posemath.cc vendored Normal file
View File

File diff suppressed because it is too large Load Diff

27
wasm-port/vendor/linuxcnc/src/libnml/posemath/sincos.c vendored Normal file
View File

@@ -0,0 +1,27 @@
/********************************************************************
* Description: sincos.c
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: LGPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
/*
sincos.c
Modification history:
21-Jan-2004 P.C. Moved across from the original EMC source tree.
*/
#include <rtapi_math.h>
#include "sincos.h"
void pm_sincos(double x, double *sx, double *cx)
{
*sx = sin(x);
*cx = cos(x);
}

81
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_atomic.h vendored Normal file
View File

@@ -0,0 +1,81 @@
// Copyright 2015 Jeff Epler
// Copyright 2026 B.Stultiens
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#ifndef __LINUXCNC_RTAPI_ATOMIC_H
#define __LINUXCNC_RTAPI_ATOMIC_H
#if defined(__cplusplus)
// We use C++20 and that has all the atomics we need
#include <atomic>
#else // defined(__cplusplus)
// Standard C, we require C11 or better
#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
#define RTAPI_USE_STDATOMIC
#elif defined(__GNUC__) && ((__GNUC__ << 8) | __GNUC_MINOR__) >= 0x409
#define RTAPI_USE_STDATOMIC
#endif
#if defined(RTAPI_USE_STDATOMIC)
#include <stdatomic.h>
#if defined(__STDC_NO_ATOMICS__)
#error "Your compiler/libc has set __STDC_NO_ATOMICS__ and atomics are required."
#endif
#else // defined(RTAPI_USE_STDATOMIC)
#error "Old compiler has no C11 atomics. Please upgrade your compiler to support C11 or better."
#endif // defined(RTAPI_USE_STDATOMIC)
#endif // defined(__cplusplus)
/* Prefixed aliases for the C11 atomic typedefs. C++ pre-C++23 does not
expose the unqualified <stdatomic.h> typedefs at global scope, so use
these names when declaring atomic fields in headers shared between C
and C++ translation units. */
#if defined(__cplusplus)
typedef std::atomic_bool rtapi_atomic_bool;
typedef std::atomic_char rtapi_atomic_char;
typedef std::atomic_schar rtapi_atomic_schar;
typedef std::atomic_uchar rtapi_atomic_uchar;
typedef std::atomic_short rtapi_atomic_short;
typedef std::atomic_ushort rtapi_atomic_ushort;
typedef std::atomic_int rtapi_atomic_int;
typedef std::atomic_uint rtapi_atomic_uint;
typedef std::atomic_long rtapi_atomic_long;
typedef std::atomic_ulong rtapi_atomic_ulong;
typedef std::atomic_llong rtapi_atomic_llong;
typedef std::atomic_ullong rtapi_atomic_ullong;
#else
typedef atomic_bool rtapi_atomic_bool;
typedef atomic_char rtapi_atomic_char;
typedef atomic_schar rtapi_atomic_schar;
typedef atomic_uchar rtapi_atomic_uchar;
typedef atomic_short rtapi_atomic_short;
typedef atomic_ushort rtapi_atomic_ushort;
typedef atomic_int rtapi_atomic_int;
typedef atomic_uint rtapi_atomic_uint;
typedef atomic_long rtapi_atomic_long;
typedef atomic_ulong rtapi_atomic_ulong;
typedef atomic_llong rtapi_atomic_llong;
typedef atomic_ullong rtapi_atomic_ullong;
#endif
#endif

25
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_bool.h vendored Normal file
View File

@@ -0,0 +1,25 @@
// Copyright 2014 Jeff Epler
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#ifndef __LINUXCNC_RTAPI_BOOL_H
#if defined(__KERNEL__)
#include <linux/types.h>
#elif !defined(__cplusplus)
// a note in gcc's stdbool.h says "supporting <stdbool.h> in C++ is a GCC extension"
#include <stdbool.h>
#endif
#endif

27
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_limits.h vendored Normal file
View File

@@ -0,0 +1,27 @@
/*
* Copyright (C) 2013 Jeff Epler <jepler@unpythonic.net>
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#ifndef __LINUXCNC_RTAPI_LIMITS_H
#define __LINUXCNC_RTAPI_LIMITS_H
#if defined(__KERNEL__)
#include <linux/kernel.h>
#else
#include <limits.h>
#endif
#endif

38
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_slab.h vendored Normal file
View File

@@ -0,0 +1,38 @@
// Copyright 2014 Jeff Epler
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#ifndef __LINUXCNC_RTAPI_SLAB_H
#define __LINUXCNC_RTAPI_SLAB_H
#include "rtapi_gfp.h"
#ifdef __KERNEL__
#include <linux/slab.h>
#define rtapi_kfree kfree
#define rtapi_kmalloc kmalloc
#define rtapi_krealloc krealloc
#define rtapi_kzalloc kzalloc
#else
#include <stdlib.h>
#define rtapi_kfree free
#define rtapi_kmalloc(sz, flags) malloc((sz))
#define rtapi_kzalloc(sz, flags) calloc(1,(sz))
#define rtapi_krealloc(p, sz, flags) realloc((p), (sz))
#endif
#endif